use argmin::core::{CostFunction, Error, Executor, State, TerminationReason};
use argmin::solver::neldermead::NelderMead;
use log::*;
use image::GrayImage;
use lazy_static::lazy_static;
use crate::create_query::{PointFitter, QueryPointCandidate, PATCH_SIZE};
use crate::structs_f64::query::QueryPoint;
const INITIAL_GAUSSIAN_ALPHA: f64 = 3.5f64;
const MAX_ITERATIONS: u64 = 100;
const SD_TOLERANCE: f64 = 1.0;
lazy_static! {
static ref PERTURBATIONS: Vec<f64> = vec![-4.0, -4.0, 1.0, 1.0, 1.0];
}
pub struct PointFitterGaussianNelderMead {}
impl PointFitter for PointFitterGaussianNelderMead {
fn fit_point(&self, point: &QueryPointCandidate, img: &GrayImage) -> QueryPoint {
let problem = Gaussian2DFitProblem {
img,
centre_x: point.x,
centre_y: point.y,
};
let initial_params: Vec<f64> = vec![
0f64,
0f64,
INITIAL_GAUSSIAN_ALPHA,
INITIAL_GAUSSIAN_ALPHA,
img.get_pixel(problem.centre_x as u32, problem.centre_y as u32)[0] as f64,
];
let solver = NelderMead::new(create_simplex(&initial_params, &PERTURBATIONS))
.with_sd_tolerance(SD_TOLERANCE)
.unwrap();
let result = 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 = QueryPoint {
x: point.x + best[0],
y: point.y + best[1],
radius: (best[2] + best[3]) / 2.0,
amplitude: best[4] * ((best[2] + best[3]) / 2.0),
};
if result.state().get_termination_reason().unwrap() != &TerminationReason::MaxItersReached {
warn!(
"point fit unexpected result: {:#} (response: {:?})",
&result, &response
);
}
response
}
}
pub fn create_simplex(point: &[f64], perturbations: &[f64]) -> Vec<Vec<f64>> {
let simplex = perturbations
.iter()
.enumerate()
.map(|(perturbation_idx, &perturbation)| {
point
.iter()
.enumerate()
.map(|(coord_idx, &coord)| {
if coord_idx == perturbation_idx {
coord + perturbation
} else {
coord
}
})
.collect()
})
.collect();
simplex
}
struct Gaussian2DFitProblem<'a> {
img: &'a GrayImage,
centre_x: f64,
centre_y: f64,
}
impl CostFunction for Gaussian2DFitProblem<'_> {
type Param = Vec<f64>;
type Output = f64;
fn cost(&self, params: &Self::Param) -> Result<Self::Output, Error> {
let mut residual: f64 = 0.0;
for x in (self.centre_x as u32 - PATCH_SIZE).max(0)
..(self.centre_x as u32 + PATCH_SIZE).min(self.img.width())
{
for y in (self.centre_y as u32 - PATCH_SIZE).max(0)
..(self.centre_y as u32 + PATCH_SIZE).min(self.img.height())
{
let img_val = self.img.get_pixel(x, y)[0] as f64;
let model_val = gaussian_2d(
x as f64,
y as f64,
self.centre_x + params[0],
self.centre_y + params[1],
params[2],
params[3],
params[4],
);
let diff_sq = (model_val - img_val) * (model_val - img_val);
residual += diff_sq;
}
}
Ok(residual)
}
}
pub fn gaussian_2d(
x: f64,
y: f64,
x0: f64,
y0: f64,
x_alpha: f64,
y_alpha: f64,
amplitude: f64,
) -> f64 {
let x_part: f64 = (x - x0) / x_alpha;
let y_part: f64 = (y - y0) / y_alpha;
amplitude * (-(x_part * x_part) - (y_part * y_part)).exp()
}