use crate::wcs::WcsTan;
use argmin::core::{CostFunction, Error, Executor, State};
use argmin::solver::goldensectionsearch::GoldenSectionSearch;
use nalgebra::{Matrix2, Matrix4x2, Matrix4x3, Vector2, Vector3};
const SKEW_FACTOR_MIN: f64 = -2f64;
const SKEW_FACTOR_MAX: f64 = 2f64;
const SKEW_FACTOR_TOL: f64 = 0.0001f64;
const MAX_ITERATIONS: u64 = 20;
pub fn optimize_wcs_skew(
orig_wcs: &WcsTan,
quad_world_pos: &Matrix4x3<f64>,
quad_pix_pos: &Matrix4x2<f64>,
) -> WcsTan {
let initial_skew: f64 = 1f64;
let problem = WcsSkewOptProblem {
orig_wcs,
world: quad_world_pos,
pix: quad_pix_pos,
};
let solver = GoldenSectionSearch::new(SKEW_FACTOR_MIN, SKEW_FACTOR_MAX)
.unwrap()
.with_tolerance(SKEW_FACTOR_TOL)
.unwrap();
let res = Executor::new(problem, solver)
.configure(|state| state.param(initial_skew).max_iters(MAX_ITERATIONS))
.timer(false)
.run()
.unwrap();
let best = res.state().get_param().unwrap();
clone_wcs_with_skew(orig_wcs, *best)
}
struct WcsSkewOptProblem<'a> {
orig_wcs: &'a WcsTan,
world: &'a Matrix4x3<f64>,
pix: &'a Matrix4x2<f64>,
}
impl CostFunction for WcsSkewOptProblem<'_> {
type Param = f64;
type Output = f64;
fn cost(&self, skew: &Self::Param) -> Result<Self::Output, Error> {
let wcs = clone_wcs_with_skew(&self.orig_wcs, *skew);
let mut cummulative_dist = 0f64;
for row_index in 0..4 {
let star_world_vec: Vector3<f64> = Vector3::new(
self.world[(row_index, 0)],
self.world[(row_index, 1)],
self.world[(row_index, 2)],
);
let star_pix_vec: Vector2<f64> =
Vector2::new(self.pix[(row_index, 0)], self.pix[(row_index, 1)]);
let transformed_star = wcs.world_2_pix(&star_world_vec);
if transformed_star.is_none() {
return Ok(f64::INFINITY);
}
let xy_diff: Vector2<f64> = transformed_star.unwrap() - star_pix_vec;
let dist = xy_diff.norm();
cummulative_dist += dist;
}
Ok(cummulative_dist)
}
}
fn clone_wcs_with_skew(orig_wcs: &WcsTan, skew: f64) -> WcsTan {
let skew_mtx: Matrix2<f64> = Matrix2::new(1f64, skew, 1f64, 1f64);
WcsTan::new(
orig_wcs.crpix,
orig_wcs.crval,
orig_wcs.cd.component_mul(&skew_mtx),
)
}