solvastro 0.1.1

A Rust blind plate solver
Documentation
use crate::coords::CoordCelestial;
use crate::wcs::WcsTan;
use argmin::solver::gradientdescent::SteepestDescent;
use argmin::solver::linesearch::HagerZhangLineSearch;
use nalgebra::{Matrix2, Vector2, Vector3};
use std::f64::INFINITY;
use std::fmt;
use argmin::core::{CostFunction, Error, Executor, Gradient, State};

const GRADIENT_DELTA: f64 = 0.0000001;
const INITIAL_ALPHA: f64 = 0.000001;
const MAX_ITERATIONS: u64 = 1000000;

#[derive(Debug)]
struct OptimiserError {
    details: String,
}

impl OptimiserError {
    fn new(msg: &str) -> OptimiserError {
        OptimiserError {
            details: msg.to_string(),
        }
    }
}

impl fmt::Display for OptimiserError {
    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
        write!(f, "{}", self.details)
    }
}

impl std::error::Error for OptimiserError {
    fn description(&self) -> &str {
        &self.details
    }
}

#[allow(dead_code)]
#[cfg(not(tarpaulin_include))]
pub fn optimize_wcs(
    orig_wcs: &WcsTan,
    quad_world_pos: &[Vector3<f64>],
    quad_pix_pos: &[Vector2<f64>],
) -> WcsTan {
    let ip = vec![
        orig_wcs.crpix.x,
        orig_wcs.crpix.y,
        orig_wcs.crval.ra,
        orig_wcs.crval.dec,
        orig_wcs.cd.m11,
        orig_wcs.cd.m12,
        orig_wcs.cd.m21,
        orig_wcs.cd.m22,
    ];

    let problem = WcsQuadOptProblem {
        world: quad_world_pos,
        pix: quad_pix_pos,
    };

    let mut linesearch = HagerZhangLineSearch::new();
    // linesearch.set_init_alpha(INITIAL_ALPHA).unwrap();
    let solver = SteepestDescent::new(linesearch);

    // Run solver
    let res = Executor::new(problem, solver)
        //.add_observer(ArgminSlogLogger::term(), ObserverMode::NewBest)
        .configure(|state| state.max_iters(MAX_ITERATIONS))
        .timer(false)
        .run()
        .unwrap();

    let best = res.state().get_best_param().unwrap();
    wcs_from_position(&best)
}

struct WcsQuadOptProblem<'a> {
    world: &'a [Vector3<f64>],
    pix: &'a [Vector2<f64>],
}

impl CostFunction for WcsQuadOptProblem<'_> {
    /// Type of the parameter vector
    type Param = Vec<f64>;

    /// Type of the return value computed by the cost function
    type Output = f64;

    fn cost(&self, position: &Self::Param) -> Result<Self::Output, Error> {
        let wcs = wcs_from_position(&position);
        //println!("WCS: {:?}", &wcs);

        let mut cummulative_dist = 0f64;

        for row_index in 0..4 {
            let star_world_vec: &Vector3<f64> = &self.world[row_index];
            //println!("star_world_vec: {:?}", &star_world_vec);
            let star_pix_vec: &Vector2<f64> = &self.pix[row_index];
            //println!("star_pix_vec: {:?}", &star_pix_vec);

            let transformed_star = wcs.world_2_pix(&star_world_vec);
            //println!("transformed_star: {:?}", &transformed_star);

            if transformed_star.is_none() {
                return Ok(INFINITY);
            }

            let xy_diff: Vector2<f64> = transformed_star.unwrap() - star_pix_vec;
            //println!("xy_diff: {:?}", &xy_diff);

            let dist = xy_diff.norm();
            //println!("dist: {:?}", &dist);

            cummulative_dist += dist;
        }

        //println!("cummulative_dist: {:?}", &cummulative_dist);
        Ok(cummulative_dist)
    }
}

impl Gradient for WcsQuadOptProblem<'_> {
    /// Type of the parameter vector
    type Param = Vec<f64>;

    /// Type of the return value computed by the cost function
    type Gradient = Vec<f64>;
    fn gradient(&self, params: &Self::Param) -> Result<Self::Param, Error> {
        let orig = self.gradient(params).unwrap();

        let jacobian: Vec<Result<f64, Error>> = params
            .iter()
            .enumerate()
            .map(|(idx, val)| {
                let mut tweaked_params = params.clone();
                tweaked_params[idx] = *val + GRADIENT_DELTA;

                let result = self.gradient(&tweaked_params).unwrap();

                result.iter().map(|new_val| (new_val - orig) / GRADIENT_DELTA)
            })
            .collect();
        //println!("jacobian: {:?}", &jacobian);

        let err = jacobian.iter().find(|&grad| grad.is_err());
        if err.is_some() {
            return Err(Error::from(OptimiserError::new("Problem projecting stars")));
        }

        Ok(jacobian.into_iter().map(|res| res.unwrap()).collect())
    }
}

fn wcs_from_position(position: &[f64]) -> WcsTan {
    WcsTan::new(
        Vector2::new(position[0], position[1]),
        CoordCelestial {
            ra: position[2],
            dec: position[3],
        },
        Matrix2::new(position[4], position[5], position[6], position[7]),
    )
}