solvastro 0.1.1

A Rust blind plate solver
Documentation
use crate::coords::{tan_project, CoordCartesian, CoordCelestial, CoordUnitSphere};
use crate::settings::{QuadCentreMode, SolverConfig};
use crate::wcs::WcsTan;
use nalgebra::{
    Matrix2, Matrix3, Matrix3x4, Matrix4x2, Matrix4x3, RowVector2, SMatrix, Unit, Vector2, Vector3,
    SVD, U2,
};
use std::ops::SubAssign;

pub fn calculate_wcs_from_quad_match(
    orig: &Matrix4x3<f64>,
    img: &Matrix4x2<f64>,
    config: &SolverConfig,
) -> Option<WcsTan> {
    let crval = calc_crval_xyz(&orig, &config.quad_centre_mode);

    project_world_to_proj_plane(&orig, &crval).map(|orig_proj| {
        let r = kabsch(&orig_proj, &img);
        let crpix = calc_crpix(&img, &config.quad_centre_mode);

        WcsTan::new(crpix, CoordCelestial::from(&crval), r.transpose())
    })
}

pub fn calc_crpix(img: &Matrix4x2<f64>, quad_centre_mode: &QuadCentreMode) -> Vector2<f64> {
    match quad_centre_mode {
        // this version is the mean of just A and B
        QuadCentreMode::CentreAb => img.fixed_rows::<2>(0).row_mean().transpose(),

        // this gives the mean of all 4 points
        QuadCentreMode::CentreAbcd => img.row_mean().transpose(),

        // why does using star A's pos and correcting for skew
        // give a much better result than using centroids?
        QuadCentreMode::CentreA => Vector2::new(img.m11, img.m12),
    }
}

pub fn calc_crval_xyz(orig: &Matrix4x3<f64>, quad_centre_mode: &QuadCentreMode) -> Vector3<f64> {
    match quad_centre_mode {
        // this gives the mean of just the A and B points
        QuadCentreMode::CentreAb => orig.fixed_rows::<2>(0).row_mean().transpose(),

        // get the mean of all 4 points
        QuadCentreMode::CentreAbcd => orig.row_mean().transpose(),

        QuadCentreMode::CentreA => Vector3::new(orig.m11, orig.m12, orig.m13).normalize(),
    }
}

pub fn project_world_to_proj_plane(
    orig: &Matrix4x3<f64>,
    crval: &Vector3<f64>,
) -> Option<Matrix4x2<f64>> {
    // use same technique as in WcsTan
    //    1. create a w2i matrix
    let w2i: Matrix3<f64> = WcsTan::create_world_to_iwc_matrix(CoordCelestial::from(crval));

    //    2. multiply w2i * orig to get orig in IWCs
    let iwc: Matrix3x4<f64> = w2i * orig.transpose();

    //    3. apply WcsTan::iwc_2_proj_plane to each row to get the proj plane coords
    let proj_a = WcsTan::iwc_2_proj_plane(&Vector3::new(
        iwc.column(0).x,
        iwc.column(0).y,
        iwc.column(0).z,
    ));
    let proj_b = WcsTan::iwc_2_proj_plane(&Vector3::new(
        iwc.column(1).x,
        iwc.column(1).y,
        iwc.column(1).z,
    ));
    let proj_c = WcsTan::iwc_2_proj_plane(&Vector3::new(
        iwc.column(2).x,
        iwc.column(2).y,
        iwc.column(2).z,
    ));
    let proj_d = WcsTan::iwc_2_proj_plane(&Vector3::new(
        iwc.column(3).x,
        iwc.column(3).y,
        iwc.column(3).z,
    ));

    if proj_a.is_none() || proj_b.is_none() || proj_c.is_none() || proj_d.is_none() {
        return None;
    }

    let proj_a = proj_a.unwrap();
    let proj_b = proj_b.unwrap();
    let proj_c = proj_c.unwrap();
    let proj_d = proj_d.unwrap();

    let new_projected = Matrix4x2::new(
        proj_a.x, proj_a.y, proj_b.x, proj_b.y, proj_c.x, proj_c.y, proj_d.x, proj_d.y,
    );
    //println!("new_proj: {:}", &new_projected);
    Some(new_projected)
}

pub fn kabsch(proj: &Matrix4x2<f64>, img: &Matrix4x2<f64>) -> Matrix2<f64> {
    let img_0 = mtx_subtract_centroid(&img);
    let proj_0 = mtx_subtract_centroid(&proj);

    let xcov = mtx_cross_covariance_4x2(img_0, proj_0);

    // perform singular value decomposition:
    // SVD formula: M = U Σ V* (decompose M into  UΣV*)

    // M is the covariance matrix (m x n)
    // U is an orthogonal unitary matrix, (m x m)
    // Σ is a diagonal matrix, (m x n)
    // V is a unitary matrix, (n x n). V* is orthogonal
    let svd: SVD<f64, U2, U2> = xcov.svd(true, true);

    // R =  V U*
    let r: Matrix2<f64> = svd.v_t.unwrap().transpose() * svd.u.unwrap().transpose();

    // scale: determine by making variances equal
    let scale: f64 = (proj_0.variance() / img_0.variance()).sqrt();

    scale * r
}

/* reshape image_quad into a column matrix, shaped thusly:
(x1, y1)
(x2, y2)
(x3, y3)
(x4, y4)
*/
pub fn mtx_from_cartesian_quad(quad: [CoordCartesian; 4]) -> Matrix4x2<f64> {
    let res: Matrix4x2<f64> = Matrix4x2::new(
        quad[0].x, quad[0].y, quad[1].x, quad[1].y, quad[2].x, quad[2].y, quad[3].x, quad[3].y,
    );

    //println!("{:}", res);
    res
}

pub fn mtx_from_unit_sphere_quad(quad: [CoordUnitSphere; 4]) -> Matrix4x3<f64> {
    Matrix4x3::new(
        quad[0].x, quad[0].y, quad[0].z, quad[1].x, quad[1].y, quad[1].z, quad[2].x, quad[2].y,
        quad[2].z, quad[3].x, quad[3].y, quad[3].z,
    )
}

pub fn mtx_subtract_centroid<const M: usize, const N: usize>(
    mtx: &SMatrix<f64, M, N>,
) -> SMatrix<f64, M, N> {
    let centroid = mtx.row_mean();
    let mut result = mtx.clone_owned();

    for mut row in result.row_iter_mut() {
        row.sub_assign(&centroid);
    }

    result
}

pub fn mtx_cross_covariance_4x2(x: Matrix4x2<f64>, y: Matrix4x2<f64>) -> Matrix2<f64> {
    // y transpose and multiply by x
    y.tr_mul(&x)
}

pub fn mtx_tan_project_2(s: &Matrix4x3<f64>, r: &Vector3<f64>) -> Option<Matrix4x2<f64>> {
    #[allow(invalid_value)]
    let mut projected: [RowVector2<f64>; 4] =
        unsafe { std::mem::MaybeUninit::uninit().assume_init() };

    for (idx, row) in s.row_iter().enumerate() {
        let projected_row = tan_project(&row.transpose(), &r);
        projected_row?;

        projected[idx] = projected_row.unwrap().transpose();
    }

    Some(Matrix4x2::from_rows(&projected))
}

#[deprecated]
pub fn mtx_tan_project_3(s: &Matrix4x3<f64>, r: &Unit<Vector3<f64>>) -> Option<Matrix4x2<f64>> {
    let r_tup = CoordUnitSphere::new(r.x, r.y, r.z);

    let r1 = tan_project(&s.row(0).transpose(), &r_tup);
    let r2 = tan_project(&s.row(1).transpose(), &r_tup);
    let r3 = tan_project(&s.row(2).transpose(), &r_tup);
    let r4 = tan_project(&s.row(3).transpose(), &r_tup);

    if let Some(r1) = r1 {
        if let Some(r2) = r2 {
            if let Some(r3) = r3 {
                if let Some(r4) = r4 {
                    return Some(Matrix4x2::from_rows(&[
                        r1.transpose(),
                        r2.transpose(),
                        r3.transpose(),
                        r4.transpose(),
                    ]));
                }
            }
        }
    }

    None
}

#[deprecated]
pub fn mtx_tan_project(s: &Matrix4x3<f64>, r: &Unit<Vector3<f64>>) -> Option<Matrix4x2<f64>> {
    let r_tup = CoordUnitSphere::new(r.x, r.y, r.z);

    let r1 = tan_project(&CoordUnitSphere::new(s.m11, s.m12, s.m13), &r_tup);
    let r2 = tan_project(&CoordUnitSphere::new(s.m21, s.m22, s.m23), &r_tup);
    let r3 = tan_project(&CoordUnitSphere::new(s.m31, s.m32, s.m33), &r_tup);
    let r4 = tan_project(&CoordUnitSphere::new(s.m41, s.m42, s.m43), &r_tup);

    if let Some(r1) = r1 {
        if let Some(r2) = r2 {
            if let Some(r3) = r3 {
                if let Some(r4) = r4 {
                    return Some(Matrix4x2::new(
                        r1.x, r1.y, r2.x, r2.y, r3.x, r3.y, r4.x, r4.y,
                    ));
                }
            }
        }
    }

    None
}