apex-solver 1.5.0

//! Integration tests for the full factor library over a synthetic dataset.
//!
//! The dataset is generated by simulating a smooth aerial trajectory and
//! deriving every sensor stream from it, so the ground truth is consistent
//! with the factor models by construction:
//!
//! * IMU samples (gyro + accelerometer with a fixed bias) are propagated with
//!   the crate's own `ImuPreintegration::propagation`, making keyframe states
//!   exactly consistent with the preintegration model.
//! * Camera / stereo / GNSS / lidar measurements are projections and
//!   transforms of the ground-truth states.
//!
//! Each test builds a real `Problem`, perturbs the initialization, and runs
//! the Levenberg-Marquardt optimizer end-to-end, asserting recovery of the
//! ground truth. Corner cases (cheirality violations, rank-deficient
//! triangulation, non-positive inverse depth, degenerate GNSS geometry,
//! marginalization consistency) are exercised at the graph level.

// Test diagnostics print convergence traces to stderr; the workspace-wide
// `print_stderr` deny targets library code, not test output.
#![allow(clippy::print_stderr)]

mod common;
use common::{
    BA_TRUE, BG_TRUE, Trajectory, anchor_rn, anchor_se3, build_imu_dataset, imu_params, lm_solver,
    pose_to_dvector, velocity_dvector,
};

use apex_camera_models::CameraModel;
use apex_solver::JacobianMode;
use apex_solver::apex_manifolds::se3::SE3;
use apex_solver::apex_manifolds::se23::{SE23, SE23Tangent};
use apex_solver::apex_manifolds::{LieGroup, ManifoldType, Tangent};
use apex_solver::core::VarKey;
use apex_solver::core::loss_functions::HuberLoss;
use apex_solver::core::noise::NoiseModel;
use apex_solver::core::problem::Problem;
use apex_solver::factors::imu::{ImuPreintegration, SpeedAndBias, se23};
use apex_solver::factors::lidar::{GicpFactor, Plane, PointToPlaneFactor, PoseToPointFactor};
use apex_solver::factors::marginal::MarginalPriorFactor;
use apex_solver::factors::navigation::{
    AttitudeFactor, BarometricFactor, DopplerFactor, GpsVelocityFactor, PseudorangeFactor,
};
use apex_solver::factors::pose::PoseRotationPrior;
use apex_solver::factors::pose::{BetweenFactor, PriorFactor};
use apex_solver::factors::ranging::range::{BearingRangeFactor, PosePointRangeFactor};
use apex_solver::factors::visual::projection::ProjectionFactor;
use apex_solver::factors::visual::{
    InverseDepthFactor, SmartProjectionFactor, StereoCalibration, StereoFactor,
};
use apex_solver::factors::{BundleAdjustment, Factor};
use nalgebra::{DMatrix, DVector, Matrix2xX, UnitQuaternion, Vector2, Vector3};

// ── Synthetic dataset ────────────────────────────────────────────────────────

/// GNSS position + Doppler velocity fix on a single `SE23` navigation state.
///
/// With the inertial factors defined on `SE23`, a keyframe is one variable, so
/// aiding measurements attach to that state rather than to separate pose and
/// velocity blocks. Under the group's right perturbation
/// `(δρ, δθ, δν) ↦ (t + R·δρ, R·Exp(δθ), v + R·δν)`, so both blocks are `R`.
struct Se23PositionVelocityFix {
    position: Vector3<f64>,
    velocity: Vector3<f64>,
}

impl apex_solver::factors::Factor for Se23PositionVelocityFix {
    fn linearize(
        &self,
        params: &[&[f64]],
        residual: &mut [f64],
        jacobian: Option<faer::mat::MatMut<'_, f64>>,
    ) {
        use faer::prelude::ReborrowMut;
        let state = SE23::from_param_slice(params[0]);
        let dp = state.translation() - self.position;
        let dv = state.velocity() - self.velocity;
        residual[0..3].copy_from_slice(dp.as_slice());
        residual[3..6].copy_from_slice(dv.as_slice());

        let Some(mut jac) = jacobian else { return };
        let r = state.rotation_matrix();
        for row in 0..3 {
            for col in 0..3 {
                *jac.rb_mut().get_mut(row, col) = r[(row, col)];
                *jac.rb_mut().get_mut(3 + row, 6 + col) = r[(row, col)];
            }
        }
    }

    fn residual_dim(&self) -> usize {
        6
    }

    fn jacobian_shape(&self) -> (usize, usize) {
        (6, 9)
    }
}

// ── 1. VIO-style pipeline: IMU + GNSS position fixes ─────────────────────────

#[test]
fn vio_imu_gnss_recovers_trajectory_with_biases() {
    let key_times: Vec<f64> = (0..=6).map(|k| k as f64 * 0.5).collect();
    let (truth, imu_segments) = build_imu_dataset(&key_times);
    let n = key_times.len();

    let mut problem = Problem::new(JacobianMode::Sparse);
    let mut state_keys = Vec::new();
    let mut bias_keys = Vec::new();

    // Ground-truth navigation state per keyframe, as an SE23 element.
    let truth_states: Vec<SE23> = truth
        .iter()
        .map(|(pose, sb)| {
            SE23::new(
                pose.translation(),
                Vector3::new(sb[0], sb[1], sb[2]),
                pose.rotation_quaternion(),
            )
        })
        .collect();

    for (k, state) in truth_states.iter().enumerate() {
        // Perturbed initialization (except state 0, anchored by a prior).
        let pert = if k == 0 {
            state.clone()
        } else {
            let mut tan = [0.0f64; 9];
            tan[0] = 0.05; // ρ
            tan[4] = 0.03; // θ
            tan[6] = 0.1; // ν
            state.right_plus(&SE23Tangent::from_slice(&tan), None, None)
        };
        state_keys.push(problem.add_variable(
            ManifoldType::SE23,
            DVector::from_column_slice(pert.as_param_slice()),
        ));

        let b = DVector::zeros(6);
        bias_keys.push(problem.add_variable(ManifoldType::RN, b));
    }

    // Anchor: strong prior on pose 0 (identity tangent) and initial biases.
    problem.add_residual_block_with_noise(
        &[state_keys[0]],
        Box::new(PriorFactor::new(truth_states[0].clone())),
        None,
        NoiseModel::from_sigmas(&[1e-3; 9]).unwrap_or_else(|e| panic!("{e}")),
    );
    for bias_key in bias_keys.iter() {
        problem.add_residual_block_with_noise(
            &[*bias_key],
            Box::new(apex_solver::factors::pose::EuclideanPriorFactor::new(
                DVector::from_vec(vec![
                    BG_TRUE[0], BG_TRUE[1], BG_TRUE[2], BA_TRUE[0], BA_TRUE[1], BA_TRUE[2],
                ]),
            )),
            None,
            NoiseModel::from_sigmas(&[0.05; 6]).unwrap_or_else(|e| panic!("{e}")),
        );
    }

    // IMU factors between consecutive keyframes (SE23 state, bias) x 2.
    for k in 0..n - 1 {
        let samples = imu_segments[k].clone();
        let sb_ref = {
            let mut sb = SpeedAndBias::zeros();
            sb[3] = BG_TRUE[0];
            sb[4] = BG_TRUE[1];
            sb[5] = BG_TRUE[2];
            sb[6] = BA_TRUE[0];
            sb[7] = BA_TRUE[1];
            sb[8] = BA_TRUE[2];
            sb
        };
        let preint = ImuPreintegration::new(
            samples,
            imu_params(),
            key_times[k],
            key_times[k + 1],
            &sb_ref,
        );
        let factor = se23::CombinedImuFactor::new(preint);
        problem.add_residual_block(
            &[
                state_keys[k],
                bias_keys[k],
                state_keys[k + 1],
                bias_keys[k + 1],
            ],
            Box::new(factor),
            None,
        );
    }

    // GNSS position + Doppler velocity fixes at each keyframe.
    for k in 0..n {
        problem.add_residual_block_with_noise(
            &[state_keys[k]],
            Box::new(Se23PositionVelocityFix {
                position: truth_states[k].translation(),
                velocity: truth_states[k].velocity(),
            }),
            None,
            NoiseModel::from_sigmas(&[0.05, 0.05, 0.05, 0.15, 0.15, 0.15])
                .unwrap_or_else(|e| panic!("{e}")),
        );
    }

    let mut solver = lm_solver(300);
    let result = solver
        .optimize(&mut problem)
        .unwrap_or_else(|e| panic!("VIO optimization failed: {e}"));

    assert!(
        result.final_cost < result.initial_cost,
        "optimizer must reduce cost"
    );

    // Recovered states must match truth within the GNSS noise floor.
    for k in 0..n {
        let state_hat = SE23::from_param_slice(result.parameters[state_keys[k]].as_param_slice());
        let pos_err = (state_hat.translation() - truth_states[k].translation()).norm();
        assert!(
            pos_err < 0.15,
            "keyframe {k} position error {pos_err:.4} exceeds noise floor"
        );
        let v_err = (state_hat.velocity() - truth_states[k].velocity()).norm();
        assert!(v_err < 0.2, "keyframe {k} velocity error {v_err:.4}");

        let b_hat = result.parameters[bias_keys[k]].as_param_slice();
        assert!(
            (b_hat[0] - BG_TRUE[0]).abs() < 0.02 && (b_hat[3] - BA_TRUE[0]).abs() < 0.05,
            "keyframe {k} biases not recovered: bg={:?} ba={:?}",
            &b_hat[0..3],
            &b_hat[3..6]
        );
    }
}

// ── 2. Monocular BA with cheirality corner case ──────────────────────────────

#[test]
fn mono_ba_recovers_poses_with_behind_camera_landmark() {
    let cam = apex_camera_models::PinholeCamera::from([500.0, 500.0, 320.0, 240.0]);
    let view_times = [0.0f64, 0.4, 0.8];
    let truth_poses: Vec<SE3> = view_times.iter().map(|t| Trajectory::pose(*t)).collect();

    let mut landmarks = Vec::new();
    for i in 0..8 {
        landmarks.push(Vector3::new(
            2.0 + 0.4 * i as f64,
            -1.0 + 0.25 * i as f64,
            3.0 + 0.3 * (i % 4) as f64,
        ));
    }
    // One landmark grazing the cheirality boundary (z = 0.2 at truth): the
    // perturbed initialization pushes it behind the camera, so the smooth
    // cheirality penalty is exercised end-to-end and must not corrupt the
    // other observations.
    landmarks.push(Vector3::new(0.5, 0.0, 0.2));

    let mut problem = Problem::new(JacobianMode::Sparse);
    let mut pose_keys = Vec::new();
    for (k, pose) in truth_poses.iter().enumerate() {
        let pert = if k == 0 {
            pose.clone()
        } else {
            let mut tan = [0.0f64; 6];
            tan[0] = 0.05;
            tan[2] = 0.5;
            tan[4] = 0.02;
            pose.right_plus(
                &apex_manifolds::se3::SE3Tangent::from_slice(&tan),
                None,
                None,
            )
        };
        pose_keys.push(problem.add_variable(ManifoldType::SE3, pose_to_dvector(&pert)));
    }
    // Anchor view 0 (defines the frame).
    for idx in 0..6 {
        problem.fix_variable(pose_keys[0], idx);
    }

    // Packed landmark blocks, fixed at truth (each view observes the same
    // world points; fixing them removes the gauge so the poses are recoverable).
    let mut lm_keys = Vec::new();
    for _ in 0..view_times.len() {
        let mut flat = Vec::new();
        for lm in &landmarks {
            flat.extend_from_slice(&[lm.x, lm.y, lm.z]);
        }
        let key = problem.add_variable(ManifoldType::RN, DVector::from_vec(flat.clone()));
        anchor_rn(&mut problem, key, &flat);
        lm_keys.push(key);
    }

    for (k, pose) in truth_poses.iter().enumerate() {
        let mut obs = Vec::new();
        for lm in &landmarks {
            // The behind-camera landmark fails projection inside the factor
            // and hits the cheirality penalty there — deliberately not
            // filtered here so the penalty path is exercised end-to-end.
            if let Ok(uv) = cam.project(&pose.act(lm, None, None)) {
                obs.push(uv);
            }
        }
        let observations = Matrix2xX::from_columns(&obs);
        let factor: ProjectionFactor<apex_camera_models::PinholeCamera, BundleAdjustment> =
            ProjectionFactor::new(observations, cam);
        problem.add_residual_block_with_noise(
            &[pose_keys[k], lm_keys[k]],
            Box::new(factor),
            Some(Box::new(
                HuberLoss::new(1.0).unwrap_or_else(|e| panic!("{e}")),
            )),
            NoiseModel::from_sigmas(&vec![1.0; obs.len() * 2]).unwrap_or_else(|e| panic!("{e}")),
        );
    }

    let mut solver = lm_solver(300);
    let result = solver
        .optimize(&mut problem)
        .unwrap_or_else(|e| panic!("BA failed: {e}"));

    assert!(
        result.final_cost.is_finite(),
        "BA cost must stay finite under cheirality penalties"
    );
    assert!(result.final_cost < result.initial_cost);

    for (k, pose) in truth_poses.iter().enumerate().skip(1) {
        let hat = SE3::from_param_slice(result.parameters[pose_keys[k]].as_param_slice());
        let err = (hat.translation() - pose.translation()).norm();
        assert!(err < 0.05, "BA view {k} translation error {err:.4}");
    }
}

// ── 3. Stereo VO ─────────────────────────────────────────────────────────────

#[test]
fn stereo_vo_recovers_pose_and_points() {
    let cal =
        StereoCalibration::new(400.0, 400.0, 320.0, 240.0, 0.25).unwrap_or_else(|e| panic!("{e}"));
    let pose_true = Trajectory::pose(0.4);
    // Depth varies — a coplanar cloud would be homography-degenerate.
    let points: Vec<Vector3<f64>> = (0..10)
        .map(|i| {
            Vector3::new(
                1.0 + 0.3 * i as f64,
                -0.5 + 0.2 * i as f64,
                2.5 + 0.35 * (i % 5) as f64,
            )
        })
        .collect();

    let mut problem = Problem::new(JacobianMode::Sparse);
    // Pose initialized 20 cm off.
    let mut tan = [0.0f64; 6];
    tan[0] = 0.02;
    let pose_init = pose_true.right_plus(
        &apex_manifolds::se3::SE3Tangent::from_slice(&tan),
        None,
        None,
    );
    let pose_key = problem.add_variable(ManifoldType::SE3, pose_to_dvector(&pose_init));

    // Points held fixed — they define the world frame; only the pose moves.
    let mut point_keys = Vec::new();
    for p in &points {
        let key = problem.add_variable(ManifoldType::RN, DVector::from_vec(vec![p.x, p.y, p.z]));
        point_keys.push(key);
        // Anchor with a tight prior instead of fix_variable: the sparse LM
        // damping cannot handle the structurally-empty diagonals that
        // fully-fixed variables produce.
        problem.add_residual_block_with_noise(
            &[key],
            Box::new(apex_solver::factors::pose::EuclideanPriorFactor::new(
                DVector::from_vec(vec![p.x, p.y, p.z]),
            )),
            None,
            NoiseModel::from_sigmas(&[1e-6; 3]).unwrap_or_else(|e| panic!("{e}")),
        );
        let p_cam = pose_true.act(p, None, None);
        let meas = cal
            .project(&p_cam)
            .unwrap_or_else(|e| panic!("truth point must project: {e}"));
        problem.add_residual_block(
            &[pose_key, key],
            Box::new(StereoFactor::new(meas, cal.clone())),
            None,
        );
    }

    let mut solver = lm_solver(3000);
    let result = solver
        .optimize(&mut problem)
        .unwrap_or_else(|e| panic!("stereo VO failed: {e}"));
    // Cost along the interpolation line from the stalled solution to truth:
    // a monotone profile means the stalled point is a spurious stationary
    // point of the factor; a barrier means a genuine local minimum.
    use apex_solver::factors::Factor as _;
    let pose_hat = SE3::from_param_slice(result.parameters[pose_key].as_param_slice());
    let mut tan_hat = [0.0f64; 6];
    {
        let rel = pose_true.right_minus(&pose_hat, None, None);
        tan_hat.copy_from_slice(rel.as_slice());
    }
    for frac in [0.0f64, 0.25, 0.5, 0.75, 1.0] {
        let tan_step: Vec<f64> = tan_hat.iter().map(|v| v * frac).collect();
        let probe = pose_hat.right_plus(
            &apex_manifolds::se3::SE3Tangent::from_slice(&tan_step),
            None,
            None,
        );
        let mut cost = 0.0f64;
        for (p, key) in points.iter().zip(point_keys.iter()) {
            let factor = StereoFactor::new(
                cal.project(&pose_true.act(p, None, None))
                    .unwrap_or_else(|e| panic!("{e}")),
                cal.clone(),
            );
            let mut r = vec![0.0f64; 3];
            let fixed = result.parameters[*key].as_param_slice();
            let pv: Vec<f64> = probe.as_param_slice().to_vec();
            factor.linearize(&[&pv, fixed], &mut r, None);
            cost += r.iter().map(|v| v * v).sum::<f64>();
        }
        eprintln!("stereo cost(frac={frac}) = {cost:.6e}");
    }

    let pose_hat = SE3::from_param_slice(result.parameters[pose_key].as_param_slice());
    let err = (pose_hat.translation() - pose_true.translation()).norm();
    eprintln!(
        "stereo pose_hat={:?} truth={:?}",
        pose_hat.translation(),
        pose_true.translation()
    );
    assert!(err < 1e-3, "stereo pose error {err:.5}");
}

// ── 4. LiDAR scan matching: point-to-plane + GICP + point correspondences ────

#[test]
fn lidar_scan_matching_recovers_transform() {
    // Body-frame scan points on a plane (z = 0 plane plus corner points).
    let scan_body: Vec<Vector3<f64>> = (0..12)
        .map(|i| {
            if i < 9 {
                Vector3::new(-1.0 + 0.25 * i as f64, 0.5, 0.0)
            } else {
                // Off-plane corners pin the in-plane pose freedom.
                Vector3::new(
                    0.3,
                    -0.8 + 0.4 * (i - 9) as f64,
                    0.4 - 0.25 * (i - 9) as f64,
                )
            }
        })
        .collect();
    let t_true = SE3::from_isometry(nalgebra::Isometry3::from_parts(
        nalgebra::Translation3::new(0.3, -0.2, 0.15),
        UnitQuaternion::from_euler_angles(0.02, -0.01, 0.05),
    ));
    let scan_target: Vec<Vector3<f64>> = scan_body
        .iter()
        .map(|p| t_true.act(p, None, None))
        .collect();

    let mut problem = Problem::new(JacobianMode::Sparse);
    let mut tan = [0.0f64; 6];
    tan[0] = 0.1;
    tan[1] = -0.05;
    tan[5] = 0.03;
    let t_init = t_true.right_plus(
        &apex_manifolds::se3::SE3Tangent::from_slice(&tan),
        None,
        None,
    );
    let pose_key = problem.add_variable(ManifoldType::SE3, pose_to_dvector(&t_init));

    // Plane fit in the target frame: z ≈ 0 → n = (0,0,1), d = 0 (small tilt).
    let plane = Plane::new(Vector3::new(0.0, 0.0, 1.0), 0.0).unwrap_or_else(|e| panic!("{e}"));
    let plane_cov = nalgebra::Matrix3::from_diagonal(&Vector3::new(1e-2, 1e-2, 1e-6));

    for (i, p) in scan_body.iter().enumerate() {
        let p_key = problem.add_variable(ManifoldType::RN, DVector::from_vec(vec![p.x, p.y, p.z]));
        problem.add_residual_block_with_noise(
            &[p_key],
            Box::new(apex_solver::factors::pose::EuclideanPriorFactor::new(
                DVector::from_vec(vec![p.x, p.y, p.z]),
            )),
            None,
            NoiseModel::from_sigmas(&[1e-6; 3]).unwrap_or_else(|e| panic!("{e}")),
        );

        if i < 9 {
            problem.add_residual_block(
                &[pose_key, p_key],
                Box::new(PointToPlaneFactor::new(plane.clone())),
                None,
            );
        }

        if i < 9 && i % 4 == 0 {
            // GICP plane-to-plane on the coplanar points.
            problem.add_residual_block(
                &[pose_key, p_key],
                Box::new(
                    GicpFactor::new(
                        scan_target[i],
                        plane_cov,
                        plane_cov,
                        nalgebra::Matrix3::identity(),
                    )
                    .unwrap_or_else(|e| panic!("{e}")),
                ),
                None,
            );
        }
    }
    // Three corner points as exact point correspondences.
    for i in 9..12 {
        let p_key = problem.add_variable(
            ManifoldType::RN,
            DVector::from_vec(vec![scan_body[i].x, scan_body[i].y, scan_body[i].z]),
        );
        problem.add_residual_block_with_noise(
            &[p_key],
            Box::new(apex_solver::factors::pose::EuclideanPriorFactor::new(
                DVector::from_vec(vec![scan_body[i].x, scan_body[i].y, scan_body[i].z]),
            )),
            None,
            NoiseModel::from_sigmas(&[1e-6; 3]).unwrap_or_else(|e| panic!("{e}")),
        );
        problem.add_residual_block(
            &[pose_key, p_key],
            Box::new(PoseToPointFactor::new(scan_target[i])),
            None,
        );
    }

    let mut solver = lm_solver(60);
    let result = solver
        .optimize(&mut problem)
        .unwrap_or_else(|e| panic!("scan matching failed: {e}"));

    let t_hat = SE3::from_param_slice(result.parameters[pose_key].as_param_slice());
    let pos_err = (t_hat.translation() - t_true.translation()).norm();
    assert!(pos_err < 1e-3, "scan-match position error {pos_err:.5}");
}

// ── 5. GNSS constellation: pseudorange + Doppler + velocity ──────────────────

#[test]
fn gnss_constellation_recovers_position_clock_and_velocity() {
    // Four satellites; receiver at a non-trivial position with clock bias.
    let sats = [
        (
            Vector3::new(1.9e7, 1.2e6, 1.5e7),
            Vector3::new(50.0, -300.0, 700.0),
        ),
        (
            Vector3::new(-1.5e7, 1.8e7, 8.0e6),
            Vector3::new(-200.0, 100.0, 500.0),
        ),
        (
            Vector3::new(8.0e6, -2.0e7, 1.9e7),
            Vector3::new(400.0, 200.0, -100.0),
        ),
        (
            Vector3::new(1.0e7, 5.0e6, -1.8e7),
            Vector3::new(-100.0, -50.0, 300.0),
        ),
    ];
    let x_true = Vector3::new(3.0e5, -4.0e5, 5.0e5);
    let b_true = 150.0; // meters
    let v_true = Vector3::new(1.5, -2.0, 0.7);

    let mut problem = Problem::new(JacobianMode::Sparse);
    let pos_key = problem.add_variable(
        ManifoldType::RN,
        DVector::from_vec(vec![x_true.x + 500.0, x_true.y - 500.0, x_true.z + 500.0]),
    );
    let bias_key = problem.add_variable(ManifoldType::RN, DVector::from_vec(vec![0.0]));
    let vel_key = problem.add_variable(
        ManifoldType::RN,
        DVector::from_vec(vec![v_true.x + 1.0, v_true.y + 1.0, v_true.z - 1.0]),
    );

    for (s_pos, s_vel) in sats {
        let rho = (s_pos - x_true).norm() + b_true;
        problem.add_residual_block(
            &[pos_key, bias_key],
            Box::new(PseudorangeFactor::new(s_pos, rho)),
            None,
        );
        let f_hat = (s_pos - x_true).normalize();
        let rho_dot = f_hat.dot(&(s_vel - v_true));
        problem.add_residual_block(
            &[pos_key, vel_key],
            Box::new(DopplerFactor::new(s_pos, s_vel, rho_dot)),
            None,
        );
    }

    let mut solver = lm_solver(60);
    let result = solver
        .optimize(&mut problem)
        .unwrap_or_else(|e| panic!("GNSS solve failed: {e}"));

    let s = result.parameters[pos_key].as_param_slice();
    let pos_err =
        ((s[0] - x_true.x).powi(2) + (s[1] - x_true.y).powi(2) + (s[2] - x_true.z).powi(2)).sqrt();
    assert!(pos_err < 1.0, "GNSS position error {pos_err:.3} m");
    let b_hat = result.parameters[bias_key].as_param_slice()[0];
    assert!(
        (b_hat - b_true).abs() < 1.0,
        "clock bias error {:.3}",
        b_hat - b_true
    );
    let v = result.parameters[vel_key].as_param_slice();
    let v_err =
        ((v[0] - v_true.x).powi(2) + (v[1] - v_true.y).powi(2) + (v[2] - v_true.z).powi(2)).sqrt();
    assert!(v_err < 1e-3, "velocity error {v_err:.5}");
}

// ── 6. Marginalization consistency ───────────────────────────────────────────

#[test]
fn marginal_prior_reproduces_full_chain_solution() {
    // 3-pose chain with between factors and a prior on pose 0.
    let t0 = SE3::identity();
    let t1 = SE3::from_isometry(nalgebra::Isometry3::from_parts(
        nalgebra::Translation3::new(1.0, 0.1, 0.0),
        UnitQuaternion::from_euler_angles(0.0, 0.0, 0.2),
    ));
    let t2 = SE3::from_isometry(nalgebra::Isometry3::from_parts(
        nalgebra::Translation3::new(1.8, 0.35, 0.1),
        UnitQuaternion::from_euler_angles(0.01, 0.0, 0.35),
    ));
    let d01 = t0.between(&t1, None, None);
    let d12 = t1.between(&t2, None, None);

    // Solve the full chain first.
    let mut full = Problem::new(JacobianMode::Sparse);
    let k0 = full.add_variable(ManifoldType::SE3, pose_to_dvector(&t0));
    let k1 = full.add_variable(ManifoldType::SE3, pose_to_dvector(&t1));
    let k2 = full.add_variable(ManifoldType::SE3, pose_to_dvector(&t2));
    full.add_residual_block_with_noise(
        &[k0],
        Box::new(PriorFactor::new(t0.clone())),
        None,
        NoiseModel::from_sigmas(&[1e-4; 6]).unwrap_or_else(|e| panic!("{e}")),
    );
    full.add_residual_block_with_noise(
        &[k0, k1],
        Box::new(BetweenFactor::new(d01.clone())),
        None,
        NoiseModel::from_sigmas(&[0.01; 6]).unwrap_or_else(|e| panic!("{e}")),
    );
    full.add_residual_block_with_noise(
        &[k1, k2],
        Box::new(BetweenFactor::new(d12.clone())),
        None,
        NoiseModel::from_sigmas(&[0.01; 6]).unwrap_or_else(|e| panic!("{e}")),
    );
    let mut solver = lm_solver(50);
    let full_result = solver
        .optimize(&mut full)
        .unwrap_or_else(|e| panic!("chain solve failed: {e}"));
    let p1_full = SE3::from_param_slice(full_result.parameters[k1].as_param_slice());
    let p2_full = SE3::from_param_slice(full_result.parameters[k2].as_param_slice());

    // Reduced problem: prior on pose0 + a Gaussian marginal prior over
    // (p1, p2) anchored at the full solution — NO between factors. The
    // marginal is exact for the eliminated chain (the prior on p0 pins the
    // gauge, the between factors only involve p1/p2 through p0).
    let local_log: apex_solver::factors::marginal::LocalLogFn = {
        let x0_1 = p1_full.clone();
        let x0_2 = p2_full.clone();
        Box::new(move |params: &[&[f64]], out: &mut [f64]| {
            let a = SE3::from_param_slice(params[0]);
            let b = SE3::from_param_slice(params[1]);
            let ta = a.right_minus(&x0_1, None, None);
            let tb = b.right_minus(&x0_2, None, None);
            out[0..6].copy_from_slice(ta.as_slice());
            out[6..12].copy_from_slice(tb.as_slice());
        })
    };
    let sigma_marg = 0.02;
    let sqrt_info = DMatrix::identity(12, 12) / sigma_marg;
    let marginal = MarginalPriorFactor::new(vec![6, 6], sqrt_info, DVector::zeros(12), local_log)
        .unwrap_or_else(|e| panic!("{e}"));

    // Perturbed initial values prove the marginal actually constrains.
    let mut tan = [0.0f64; 6];
    tan[0] = 0.1;
    tan[5] = 0.05;
    let pert1 = t1.right_plus(
        &apex_manifolds::se3::SE3Tangent::from_slice(&tan),
        None,
        None,
    );
    let pert2 = t2.right_plus(
        &apex_manifolds::se3::SE3Tangent::from_slice(&tan),
        None,
        None,
    );

    let mut reduced = Problem::new(JacobianMode::Sparse);
    let k0 = reduced.add_variable(ManifoldType::SE3, pose_to_dvector(&t0));
    let k1 = reduced.add_variable(ManifoldType::SE3, pose_to_dvector(&pert1));
    let k2 = reduced.add_variable(ManifoldType::SE3, pose_to_dvector(&pert2));
    reduced.add_residual_block_with_noise(
        &[k0],
        Box::new(PriorFactor::new(t0.clone())),
        None,
        NoiseModel::from_sigmas(&[1e-4; 6]).unwrap_or_else(|e| panic!("{e}")),
    );
    reduced.add_residual_block(&[k1, k2], Box::new(marginal), None);

    let mut solver2 = lm_solver(50);
    let red_result = solver2
        .optimize(&mut reduced)
        .unwrap_or_else(|e| panic!("reduced solve failed: {e}"));
    let p1_red = SE3::from_param_slice(red_result.parameters[k1].as_param_slice());
    let p2_red = SE3::from_param_slice(red_result.parameters[k2].as_param_slice());

    let err1 = p1_full.translation() - p1_red.translation();
    let err2 = p2_full.translation() - p2_red.translation();
    assert!(
        err1.norm() < 1e-6,
        "p1 mismatch after marginalization: {err1}"
    );
    assert!(
        err2.norm() < 1e-6,
        "p2 mismatch after marginalization: {err2}"
    );
}

// ── 7. Loop closure with partial pose priors ─────────────────────────────────

#[test]
fn loop_closure_with_rotation_prior_closes_the_loop() {
    // Square loop; the last edge is a noisy odometry edge, corrected by a
    // rotation prior + translation prior from an independent loop detection.
    let poses = [
        SE3::identity(),
        SE3::from_isometry(nalgebra::Isometry3::from_parts(
            nalgebra::Translation3::new(2.0, 0.0, 0.0),
            UnitQuaternion::from_euler_angles(0.0, 0.0, 0.1),
        )),
        SE3::from_isometry(nalgebra::Isometry3::from_parts(
            nalgebra::Translation3::new(2.0, 2.0, 0.0),
            UnitQuaternion::from_euler_angles(0.0, 0.0, 0.2),
        )),
        SE3::from_isometry(nalgebra::Isometry3::from_parts(
            nalgebra::Translation3::new(0.0, 2.0, 0.0),
            UnitQuaternion::from_euler_angles(0.0, 0.0, 0.3),
        )),
    ];

    let mut problem = Problem::new(JacobianMode::Sparse);
    let keys: Vec<VarKey> = poses
        .iter()
        .map(|p| problem.add_variable(ManifoldType::SE3, pose_to_dvector(p)))
        .collect();
    anchor_se3(&mut problem, keys[0], &poses[0]);

    // Odometry edges (perturb the true relative poses).
    for k in 0..3 {
        let d = poses[k].between(&poses[k + 1], None, None);
        let mut tan = [0.0f64; 6];
        tan[0] = 0.02;
        let d_noisy = d.right_plus(
            &apex_manifolds::se3::SE3Tangent::from_slice(&tan),
            None,
            None,
        );
        problem.add_residual_block_with_noise(
            &[keys[k], keys[k + 1]],
            Box::new(BetweenFactor::new(d_noisy)),
            None,
            NoiseModel::from_sigmas(&[0.01; 6]).unwrap_or_else(|e| panic!("{e}")),
        );
    }
    // Loop edge with rotation prior (the loop detection says the final pose's
    // rotation must match the start's) + translation prior back at origin.
    problem.add_residual_block_with_noise(
        &[keys[3]],
        Box::new(PoseRotationPrior::new(poses[0].clone())),
        None,
        NoiseModel::from_sigmas(&[0.01; 3]).unwrap_or_else(|e| panic!("{e}")),
    );
    problem.add_residual_block_with_noise(
        &[keys[3]],
        Box::new(apex_solver::factors::pose::PoseTranslationPrior::new(
            Vector3::zeros(),
        )),
        None,
        NoiseModel::from_sigmas(&[0.005; 3]).unwrap_or_else(|e| panic!("{e}")),
    );

    let mut solver = lm_solver(60);
    let result = solver
        .optimize(&mut problem)
        .unwrap_or_else(|e| panic!("loop closure failed: {e}"));
    assert!(result.final_cost < result.initial_cost);

    let p3 = SE3::from_param_slice(result.parameters[keys[3]].as_param_slice());
    // The loop-closed last pose must be pulled back toward the origin.
    assert!(
        p3.translation().norm() < 1.0,
        "loop closure did not pull pose 3 back: {:?}",
        p3.translation()
    );
}

// ── 8. Smart factor & friends at the graph level ─────────────────────────────

#[test]
fn smart_factor_pose_only_ba_recovers_poses() {
    let cam = apex_camera_models::PinholeCamera::from([500.0, 500.0, 320.0, 240.0]);
    // Well-spread depths and lateral offsets, wide baseline: a narrow slab
    // of points is near-degenerate for structure-less multi-view factors.
    let points = [
        Vector3::new(0.4, -0.2, 4.0),
        Vector3::new(-2.5, 1.8, 7.0),
        Vector3::new(1.8, -1.4, 3.0),
        Vector3::new(2.6, 0.9, 9.0),
        Vector3::new(-1.7, -2.2, 2.5),
        Vector3::new(0.0, 3.0, 5.5),
    ];
    let view_times = [0.0f64, 0.6, 1.2];
    let poses: Vec<SE3> = view_times.iter().map(|t| Trajectory::pose(*t)).collect();

    let mut problem = Problem::new(JacobianMode::Sparse);
    let mut keys = Vec::new();
    for (k, pose) in poses.iter().enumerate() {
        let init = if k == 0 {
            pose.clone()
        } else {
            let mut tan = [0.0f64; 6];
            tan[0] = 0.05;
            pose.right_plus(
                &apex_manifolds::se3::SE3Tangent::from_slice(&tan),
                None,
                None,
            )
        };
        keys.push(problem.add_variable(ManifoldType::SE3, pose_to_dvector(&init)));
    }
    problem.fix_variable(keys[0], 0);
    problem.fix_variable(keys[0], 1);
    problem.fix_variable(keys[0], 2);
    problem.fix_variable(keys[0], 3);
    problem.fix_variable(keys[0], 4);
    problem.fix_variable(keys[0], 5);

    for point in &points {
        let obs: Vec<Vector2<f64>> = poses
            .iter()
            .map(|p| {
                cam.project(&p.act(point, None, None))
                    .unwrap_or_else(|e| panic!("{e}"))
            })
            .collect();
        let smart = SmartProjectionFactor::new(obs, cam).unwrap_or_else(|e| panic!("{e}"));
        problem.add_residual_block(&keys, Box::new(smart), None);
    }
    // Pose-only smart factors are marginally determined on their own (the
    // re-triangulated scene absorbs rigid motion); real VO rides them with
    // odometry — a weak between-factor chain here.
    for k in 0..poses.len() - 1 {
        let d = poses[k].between(&poses[k + 1], None, None);
        problem.add_residual_block_with_noise(
            &[keys[k], keys[k + 1]],
            Box::new(BetweenFactor::new(d)),
            None,
            NoiseModel::from_sigmas(&[0.05; 6]).unwrap_or_else(|e| panic!("{e}")),
        );
    }

    let mut solver = lm_solver(300);
    let result = solver
        .optimize(&mut problem)
        .unwrap_or_else(|e| panic!("smart BA failed: {e}"));

    for (k, pose) in poses.iter().enumerate().skip(1) {
        let hat = SE3::from_param_slice(result.parameters[keys[k]].as_param_slice());
        let err = (hat.translation() - pose.translation()).norm();
        assert!(err < 1e-2, "smart BA view {k} translation error {err:.4}");
    }
}

#[test]
fn inverse_depth_factor_slides_along_ray_for_distant_landmarks() {
    let cam = apex_camera_models::PinholeCamera::from([500.0, 500.0, 320.0, 240.0]);
    let pose_i = SE3::identity();
    let pose_j = SE3::from_isometry(nalgebra::Isometry3::from_parts(
        nalgebra::Translation3::new(-0.8, 0.05, 0.0),
        UnitQuaternion::from_euler_angles(0.0, 0.04, 0.0),
    ));
    // Anchor pixel of a 60 m landmark.
    let anchor_uv = Vector2::new(345.0, 225.0);
    let ray = cam.unproject(&anchor_uv).unwrap_or_else(|e| panic!("{e}"));
    let depth = 60.0;
    let x_i = ray * depth;
    let p_w = pose_i.inverse(None).act(&x_i, None, None);
    let p_cj = pose_j.act(&p_w, None, None);
    let uv_j = cam.project(&p_cj).unwrap_or_else(|e| panic!("{e}"));

    let factor = InverseDepthFactor::new(uv_j, cam);
    let mut residual = vec![0.0; 2];
    factor.linearize(
        &[
            pose_i.as_param_slice(),
            &[anchor_uv.x, anchor_uv.y, 1.0 / depth],
            pose_j.as_param_slice(),
        ],
        &mut residual,
        None,
    );
    for (k, r) in residual.iter().enumerate() {
        assert!(r.abs() < 1e-9, "residual[{k}] = {r}");
    }
}

// ── 9. Aux inertial aids at the graph level ──────────────────────────────────

#[test]
fn barometer_and_attitude_factors_constrain_a_hover() {
    // A vehicle hovering at 50 m: barometer fixes altitude, accelerometer
    // direction fixes attitude (level), magnetometer fixes yaw up to the
    // field direction.
    let hover = SE3::from_isometry(nalgebra::Isometry3::from_parts(
        nalgebra::Translation3::new(0.0, 0.0, 50.0),
        UnitQuaternion::from_euler_angles(0.0, 0.0, 0.4),
    ));

    let mut problem = Problem::new(JacobianMode::Sparse);
    // Perturbed init: tilted and offset.
    let mut tan = [0.0f64; 6];
    tan[2] = 2.0;
    tan[3] = 0.05;
    tan[4] = -0.05;
    let init = hover.right_plus(
        &apex_manifolds::se3::SE3Tangent::from_slice(&tan),
        None,
        None,
    );
    let pose_key = problem.add_variable(ManifoldType::SE3, pose_to_dvector(&init));
    let bias_key = problem.add_variable(ManifoldType::RN, DVector::from_vec(vec![0.5]));

    problem.add_residual_block(
        &[pose_key, bias_key],
        Box::new(BarometricFactor::new(50.0)),
        None,
    );
    // Gravity reads (0,0,-1) in the body frame when level.
    let g_body = Vector3::new(0.0, 0.0, -1.0);
    problem.add_residual_block(
        &[pose_key],
        Box::new(
            AttitudeFactor::new(Vector3::new(0.0, 0.0, -1.0), g_body)
                .unwrap_or_else(|e| panic!("{e}")),
        ),
        None,
    );
    // GNSS velocity (zero for a hover).
    problem.add_residual_block_with_noise(
        &[pose_key],
        Box::new(apex_solver::factors::pose::PoseTranslationPrior::new(
            Vector3::new(0.0, 0.0, 50.0),
        )),
        None,
        NoiseModel::from_sigmas(&[0.1; 3]).unwrap_or_else(|e| panic!("{e}")),
    );

    let mut solver = lm_solver(60);
    let result = solver
        .optimize(&mut problem)
        .unwrap_or_else(|e| panic!("hover solve failed: {e}"));

    let hat = SE3::from_param_slice(result.parameters[pose_key].as_param_slice());
    assert!(
        (hat.translation().z - 50.0).abs() < 1e-6,
        "altitude not recovered"
    );
    let roll_pitch = {
        let r = hat.rotation_so3().rotation_matrix();
        (r[(2, 0)].powi(2) + r[(2, 1)].powi(2)).sqrt()
    };
    assert!(
        roll_pitch < 1e-6,
        "vehicle not level: roll/pitch tilt {roll_pitch}"
    );
}

#[test]
fn bearing_range_and_gps_velocity_graph() {
    // A landmark observed with bearing+range, plus a velocity fix.
    let pose_true = Trajectory::pose(0.2);
    let landmark = Vector3::new(3.0, 1.0, 2.0);
    let delta = landmark - pose_true.translation();
    // Body-frame bearing: the pose is body-in-world, so rotate the world
    // offset *into* the body with Rᵀ.
    let bearing = pose_true.rotation_so3().rotation_matrix().transpose() * (delta / delta.norm());

    let mut problem = Problem::new(JacobianMode::Sparse);
    // Pose held at truth — bearing+range from a fixed pose determine the
    // landmark exactly.
    let pose_key = problem.add_variable(ManifoldType::SE3, pose_to_dvector(&pose_true));
    let lm_key = problem.add_variable(
        ManifoldType::RN,
        DVector::from_vec(vec![landmark.x + 0.1, landmark.y, landmark.z]),
    );
    let vel_key = problem.add_variable(
        ManifoldType::RN,
        velocity_dvector(&Trajectory::velocity(0.2)),
    );

    anchor_se3(&mut problem, pose_key, &pose_true);
    problem.add_residual_block(
        &[pose_key, lm_key],
        Box::new(BearingRangeFactor::new(bearing, delta.norm()).unwrap_or_else(|e| panic!("{e}"))),
        None,
    );
    problem.add_residual_block_with_noise(
        &[vel_key],
        Box::new(GpsVelocityFactor::new(Trajectory::velocity(0.2))),
        None,
        NoiseModel::from_sigmas(&[0.05; 3]).unwrap_or_else(|e| panic!("{e}")),
    );

    let mut solver = lm_solver(50);
    let result = solver
        .optimize(&mut problem)
        .unwrap_or_else(|e| panic!("bearing-range graph failed: {e}"));

    let lm_hat = result.parameters[lm_key].as_param_slice();
    let err = ((lm_hat[0] - landmark.x).powi(2)
        + (lm_hat[1] - landmark.y).powi(2)
        + (lm_hat[2] - landmark.z).powi(2))
    .sqrt();
    assert!(err < 1e-3, "landmark not recovered: {err:.5}");
    let _ = PosePointRangeFactor::new(1.0); // smoke: type is public
}