solvastro 0.1.1

A Rust blind plate solver
Documentation
use log::*;
use std::f64::consts::PI;
use std::num::NonZero;
use kiddo_v6::{MutableKdTree, SquaredEuclidean};
use super::solution::Solution;
use super::star_index::StarIndex;
use super::verified_solution::VerifiedSolution;
use crate::dynamic_index::star_index::{BestNeighbour, StarIndexTrait};
use crate::dynamic_index::star_index_container::{ReadableStarIndexContainer, SIDX};
use crate::structs_f64::quad::Quad;
use crate::structs_f64::query::Query;
use nalgebra::{distance_squared, Point2, Vector2, Vector3};
use sorted_vec::partial::SortedVec;

type ProjectedStar<'a> = (u32, Vector2<f64>, &'a str);

const PI_TIMES_2: f64 = PI + PI;

const THRESHOLD_BAIL: f64 = 10f64; // f64::NEG_INFINITY;
const THRESHOLD_STOP: f64 = f64::INFINITY;
const MAX_SIGMA2: f64 = 500f64; // max sigma2 for matches (default 5 sigma)
const MAX_CAT_STARS_TO_QUERY_POINTS_RATIO: f64 = 2f64;
const PIX2: f64 = 100f64; // pixel jitter squared
const RATIO_DISTRACTORS: f64 = 0.25f64;
const IMAGE_POINT_MIN_SQ_SEP: f64 = 144f64; // min 12 pixels between image objects for verification

pub fn verifier<'a>(
    cand_solns: &'a mut (dyn Iterator<Item = Solution> + 'a),
    query: &'a Query,
    index: &'a ReadableStarIndexContainer,
    cat_star_query_log: &'a mut Vec<(Vector3<f64>, f32, usize)>,
) -> impl Iterator<Item = VerifiedSolution> + 'a {
    cand_solns.map(move |cand_soln| verify_item(cand_soln, &query, &index, cat_star_query_log))
}

pub fn verify_item(
    solution: Solution,
    query: &Query,
    index: &ReadableStarIndexContainer,
    cat_star_query_log: &mut Vec<(Vector3<f64>, f32, usize)>,
) -> VerifiedSolution {
    let cat_stars = get_cat_stars_within_radius(&query, index, &solution, cat_star_query_log);

    let cat_stars_proj: Vec<_> = project_cat_stars(query, &index, &solution, cat_stars).collect();
    let cat_stars_proj_tree = build_cat_stars_proj_kdtree(&cat_stars_proj);

    let (quadr2, log_bg) = calc_bayes_factors(query, &solution);

    // FIXME: I had to refactor VerifiedSolution so that I could pass solution into
    //        it after calling match_projected_to_image. Surely there's a better way
    match_projected_to_image(&query, cat_stars_proj, cat_stars_proj_tree, quadr2, log_bg)
        .take_sol(solution)
}

fn calc_bayes_factors(query: &Query, solution: &Solution) -> (f64, f64) {
    // pixel dist squared of query quad point A to centre point of query quad A and B
    let quadr2: f64 = get_query_quad_radius2(&query, &solution.query_quad);
    let effective_area = query.img_h * query.img_w;
    let log_bg = (1f64 / effective_area as f64).ln();
    (quadr2, log_bg)
}

fn match_projected_to_image(
    query: &Query,
    cat_stars_proj: Vec<ProjectedStar>,
    cat_stars_proj_tree: MutableKdTree<f64, 2>,
    quadr2: f64,
    log_bg: f64,
) -> VerifiedSolution {
    let mut result = VerifiedSolution::new(THRESHOLD_BAIL, THRESHOLD_STOP, query.points.len());
    let mut match_count: usize = 0;

    for (item_idx, item) in query.points.iter().enumerate() {
        if result.should_stop() {
            //println!("STOPPING EARLY");
            break;
        }
        let nearest = cat_stars_proj_tree
            .query(&[item.x, item.y])
            .nearest_one::<SquaredEuclidean<f64>>()
            .execute();
        if let Some(&cat_match) = cat_stars_proj.get(nearest.item as usize) {
            let sigma2 = PIX2 * (1f64 + nearest.distance / quadr2);

            if nearest.distance <= sigma2 * MAX_SIGMA2 {
                if let Some(log_odds) = calc_log_odds(
                    match_count,
                    cat_stars_proj.len(),
                    log_bg,
                    sigma2,
                    nearest.distance,
                ) {
                    if let Some(existing_log_odds) =
                        result.get_existing_logodds(cat_match.0 as usize)
                    {
                        // conflict
                        if log_odds > existing_log_odds {
                            // we have a better match. replace the old one with this one
                            result.replace_match(
                                item_idx,
                                cat_match.0 as usize,
                                log_odds,
                                item.x,
                                item.y,
                                cat_match.1.x,
                                cat_match.1.y,
                                cat_match.2,
                            );
                        } else {
                            result.add_conflict();
                        }
                    } else {
                        result.add_match(
                            item_idx,
                            cat_match.0 as usize,
                            log_odds,
                            item.x,
                            item.y,
                            cat_match.1.x,
                            cat_match.1.y,
                            cat_match.2,
                        );
                        match_count += 1;
                    }
                    continue;
                }
            }
        }
        result.add_distractor();
    }

    result
}

pub fn filter_and_sort_query_points(query: &Query, solution: &Solution) -> Vec<usize> {
    let _query_quad = &solution.query_quad;
    let cleaned_image_objects_range = 0..query.points.len();

    // deduplicate stars
    let mut deduplicated_image_objects: Vec<usize> = vec![];
    let mut kd_img_obj: MutableKdTree<f64, 2> = MutableKdTree::new_from_slice(&[]).expect("KdTree::new_from_slice failed");
    for cleaned_img_obj in cleaned_image_objects_range {
        let point: [f64; 2] = [
            query.points[cleaned_img_obj].x,
            query.points[cleaned_img_obj].y,
        ];
        if kd_img_obj
            .query(&point)
            .within::<SquaredEuclidean<f64>>(IMAGE_POINT_MIN_SQ_SEP)
            .execute()
            //.expect("query within kd_img_obj failed")
            .is_empty()
        {
            kd_img_obj.add(&point, cleaned_img_obj as u32).unwrap();
            deduplicated_image_objects.push(cleaned_img_obj);
        } else {
            debug!(
                "Discarding image point #{:}, deduplication",
                cleaned_img_obj
            );
        }
    }

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

    // perhaps in the future:
    // * uniformize? Not needed at present
    // * RoR (radius of relevance)
    deduplicated_image_objects
}

// create a kd tree of the projected stars so that we can find the nearest
pub fn build_cat_stars_proj_kdtree<'a>(cat_stars_proj: &Vec<ProjectedStar<'a>>) -> MutableKdTree<f64, 2> {
    let mut cat_stars_proj_tree: MutableKdTree<f64, 2> = MutableKdTree::new_from_slice(&[]).expect("KdTree::new_from_slice failed");

    for (idx, proj_star) in cat_stars_proj.iter().enumerate() {
        cat_stars_proj_tree.add(&[proj_star.1.x, proj_star.1.y], idx as u32).unwrap();
    }

    cat_stars_proj_tree
}

pub fn get_cat_stars_within_radius_old(
    query: &Query,
    index: &StarIndex,
    solution: &Solution,
) -> Option<Vec<usize>> {
    // get centre of query image in xyz space
    let img_centre_xyz: Vector3<f64> = solution.wcs.pix_2_world(&query.get_img_centre());

    // get radius2 of query image in xyz space radians
    let img_corner_xyz: Vector3<f64> = solution.wcs.pix_2_world(&query.get_img_corner());

    let img_xyz_radius2: f64 = (img_centre_xyz - img_corner_xyz).norm_squared();

    // build a list of the brightest cat star indexes, sorted by ascending v_mag, with a max length.
    let desired_quantity =
        (MAX_CAT_STARS_TO_QUERY_POINTS_RATIO * query.points.len() as f64) as usize;

    // find all stars in the catalog that are within the bounding circle of the field. Vec<cat_stars_idx>
    let cat_stars = index.stars_within_radius(
        &[
            (&img_centre_xyz).x,
            (&img_centre_xyz).y,
            (&img_centre_xyz).z,
        ],
        img_xyz_radius2,
    );

    // if cat_stars.is_err() {
    //     return None;
    // }
    //let cat_stars = cat_stars.unwrap();

    let mut num_processed: usize = 0;
    let mut v_mag_threshold: f64 = f64::NEG_INFINITY;
    let mut best_cat_stars_in_radius: Vec<usize> = Vec::with_capacity(desired_quantity);
    let mut best_v_mag_in_radius: SortedVec<f64> = SortedVec::with_capacity(desired_quantity);

    for cat_star in cat_stars {
        let v_mag = index.get_star(cat_star.item as SIDX).mag();

        // if we've not yet filled the capacity, insert in sorted order
        // and increase the v_mag threshold if the inserted star is dimmer than any yet seen
        if num_processed < desired_quantity {
            let insert_pos = best_v_mag_in_radius.insert(v_mag);
            best_cat_stars_in_radius.insert(insert_pos, cat_star.item as usize);
            v_mag_threshold = v_mag_threshold.max(v_mag);
            num_processed += 1;
        } else if v_mag < v_mag_threshold {
            // once we've filled the capacity, only insert if we have a star
            // brightest than the dimmest in the vec.

            let insert_pos = best_v_mag_in_radius.insert(v_mag);
            best_cat_stars_in_radius.insert(insert_pos, cat_star.item as usize);

            v_mag_threshold = best_v_mag_in_radius.pop().unwrap();
            best_cat_stars_in_radius.pop();
        }
    }

    Some(best_cat_stars_in_radius)
}

pub fn get_cat_stars_within_radius(
    query: &Query,
    index: &ReadableStarIndexContainer,
    solution: &Solution,
    cat_star_query_log: &mut Vec<(Vector3<f64>, f32, usize)>,
) -> impl Iterator<Item = BestNeighbour<f64, u32>> {
    // get centre of query image in xyz space
    let img_centre_xyz: Vector3<f64> = solution.wcs.pix_2_world(&query.get_img_centre());

    // get radius2 of query image in xyz space radians
    let img_corner_xyz: Vector3<f64> = solution.wcs.pix_2_world(&query.get_img_corner());

    let img_xyz_radius2: f64 = (img_centre_xyz - img_corner_xyz).norm_squared();

    // build a list of the brightest cat star indexes, sorted by ascending v_mag, with a max length.
    let desired_quantity =
        (MAX_CAT_STARS_TO_QUERY_POINTS_RATIO * query.points.len() as f64) as usize;

    let query_point = [img_centre_xyz.x, img_centre_xyz.y, img_centre_xyz.z];

    cat_star_query_log.push((img_centre_xyz, img_xyz_radius2 as f32, desired_quantity));

    // find all stars in the catalog that are within the bounding circle of the field. Vec<cat_stars_idx>
    // info!("index.kd_stars.best_n_within: qp={:?}, img_xyz_radius2{}, desired_quantity={}", &query_point, &img_xyz_radius2, &desired_quantity);
    index.best_n_stars_within(
        &query_point,
        img_xyz_radius2,
        NonZero::new(desired_quantity).unwrap(),
    )
}

pub fn project_cat_stars<'a>(
    query: &'a Query,
    index: &'a ReadableStarIndexContainer,
    solution: &'a Solution,
    cat_stars: impl Iterator<Item = BestNeighbour<f64, u32>> + 'a,
) -> impl Iterator<Item = (u32, Vector2<f64>, &'a str)> + 'a {
    let width: f64 = query.img_w as f64;
    let height: f64 = query.img_h as f64;

    cat_stars
        // project them all into pixel space. Vec<(cat_stars_idx, (x, y, opt<name>))>
        .map(move |best_neighbour| {
            // let star = &index.stars[best_neighbour.item as usize];
            let star = &index.get_star(best_neighbour.item);
            solution
                .wcs
                .world_2_pix(&star.spherical())
                .map(|v| (best_neighbour.item, v, star.name_or_identifier()))
        })
        // remove the ones that fail to be projected and unwrap
        .flatten()
        // filter out those that are outside the image
        .filter(move |(_, p, _)| {
            p.x.is_sign_positive() && p.y.is_sign_positive() && p.x <= width && p.y <= height
        })
}

fn get_query_quad_radius2(query: &Query, query_quad: &Quad) -> f64 {
    let a = query.points[query_quad.a];
    let b = query.points[query_quad.b];

    let centre_ab = Point2::new((a.x + b.x) / 2f64, (a.y + b.y) / 2f64);

    let a: Point2<f64> = Point2::new(a.x, b.x);

    distance_squared(&centre_ab, &a)
}

fn calc_log_odds(
    match_count: usize,
    num_cat_stars: usize,
    log_bg: f64,
    sigma2: f64,
    dist2: f64,
) -> Option<f64> {
    // dynamic distractor threshold
    let log_d = (RATIO_DISTRACTORS
        + (1f64 - RATIO_DISTRACTORS) * match_count as f64 / num_cat_stars as f64)
        .ln()
        + log_bg;

    // peak value of the Gaussian
    let log_g_max = ((1.0 - RATIO_DISTRACTORS) / (PI_TIMES_2 * sigma2 * num_cat_stars as f64)).ln();

    // value of the foreground Gaussian
    let log_fg = log_g_max - dist2 / (2f64 * sigma2);

    if log_fg < log_d {
        None
    } else {
        Some(log_fg - log_bg)
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    use crate::dynamic_index::verified_solution::ItemMatchResult;
    use crate::structs_f64::query::QueryPoint;

    #[test]
    fn empty_projected_catalogue_counts_query_point_as_distractor() {
        let query = Query {
            filename: "test.jpg".to_owned(),
            img_w: 100,
            img_h: 100,
            points: vec![QueryPoint {
                x: 50.0,
                y: 50.0,
                amplitude: 1.0,
                radius: 1.0,
            }],
        };
        let projected_stars = vec![];
        let tree = build_cat_stars_proj_kdtree(&projected_stars);

        let result = match_projected_to_image(&query, projected_stars, tree, 1.0, -10.0);
        assert!(matches!(result.matches.as_slice(), [ItemMatchResult::Distractor]));
    }
}