use crate::point_fit_gaussian_neldermead::create_simplex;
use crate::wcs::WcsTan;
use argmin::core::{CostFunction, Error, State};
use argmin::solver::neldermead::NelderMead;
use lazy_static::lazy_static;
use log::*;
use nalgebra::{Matrix2, Vector2, Vector3};
const MAX_ITERATIONS: u64 = 100;
const SD_TOLERANCE: f64 = 0.001;
lazy_static! {
static ref PERTURBATIONS: Vec<f64> = vec![0.1, 0.1, 0.00001, -0.00001, 0.00001, -0.00001];
}
#[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 initial_params = vec![
orig_wcs.crpix.x,
orig_wcs.crpix.y,
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,
orig_wcs,
};
let solver = NelderMead::new(create_simplex(&initial_params, &PERTURBATIONS))
.with_sd_tolerance(SD_TOLERANCE)
.unwrap();
let result = argmin::core::Executor::new(problem, solver)
.configure(|state| state.max_iters(MAX_ITERATIONS))
.timer(false)
.run()
.unwrap();
let best = result.state().get_best_param().unwrap();
let response = wcs_from_position(&orig_wcs, best);
debug!("WCS Tune result: {:#} (WCS: {:?})", &result, &response);
response
}
struct WcsQuadOptProblem<'a> {
world: &'a [Vector3<f64>],
pix: &'a [Vector2<f64>],
orig_wcs: &'a WcsTan,
}
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(&self.orig_wcs, &position);
let mut cummulative_dist = 0f64;
for row_index in 0..self.world.len() {
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(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 wcs_from_position(orig_wcs: &WcsTan, params: &[f64]) -> WcsTan {
WcsTan::new(
Vector2::new(params[0], params[1]),
orig_wcs.crval,
Matrix2::new(params[2], params[3], params[4], params[5]),
)
}