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();
let solver = SteepestDescent::new(linesearch);
let res = Executor::new(problem, solver)
.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 Param = Vec<f64>;
type Output = f64;
fn cost(&self, position: &Self::Param) -> Result<Self::Output, Error> {
let wcs = wcs_from_position(&position);
let mut cummulative_dist = 0f64;
for row_index in 0..4 {
let star_world_vec: &Vector3<f64> = &self.world[row_index];
let star_pix_vec: &Vector2<f64> = &self.pix[row_index];
let transformed_star = wcs.world_2_pix(&star_world_vec);
if transformed_star.is_none() {
return Ok(INFINITY);
}
let xy_diff: Vector2<f64> = transformed_star.unwrap() - star_pix_vec;
let dist = xy_diff.norm();
cummulative_dist += dist;
}
Ok(cummulative_dist)
}
}
impl Gradient for WcsQuadOptProblem<'_> {
type Param = Vec<f64>;
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();
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]),
)
}