apex_manifolds/

sim3.rs

//! Sim(3) - Similarity Transformations in 3D
//!
//! This module implements the Similarity group Sim(3), which represents
//! transformations including rotation, translation, and uniform scaling in 3D space.
//!
//! Sim(3) is the semi-direct product: (SO(3) × ℝ₊) ⋉ ℝ³
//!
//! Sim(3) elements are represented as (R, t, s) where:
//! - R ∈ SO(3): rotation
//! - t ∈ ℝ³: translation
//! - s ∈ ℝ₊: scale (positive real number)
//!
//! Sim(3) tangent elements are represented as [ρ(3), θ(3), σ(1)] = 7 components:
//! - ρ: translational component
//! - θ: rotational component (axis-angle)
//! - σ: logarithmic scale (log(s))
//!
//! The implementation follows conventions from:
//! - Ethan Eade's "Lie Groups for Computer Vision" (Section 6)
//! - Sophus library Sim3 implementation
//! - manif C++ library patterns
//!
//! # Use Cases
//! - Visual SLAM with scale ambiguity (monocular cameras)
//! - Structure from Motion
//! - 3D reconstruction with unknown scale
//!
//! # References
//! - Ethan Eade: "Lie Groups for Computer Vision" - <https://www.ethaneade.com/lie.pdf>
//! - Sophus library: sophus/sim3.hpp

use crate::{
    LieGroup, Tangent,
    so3::{SO3, SO3Tangent},
};
use nalgebra::{Matrix3, Matrix4, SMatrix, SVector, UnitQuaternion, Vector3};

// Type aliases for Sim(3) - 7 DOF
type Vector7<T> = SVector<T, 7>;
type Matrix7<T> = SMatrix<T, 7, 7>;
use std::{
    fmt,
    fmt::{Display, Formatter},
};

/// Sim(3) group element representing similarity transformations in 3D.
///
/// Stored as a flat `SVector<f64, 8>` = [tx, ty, tz, qw, qx, qy, qz, scale].
#[derive(Clone, PartialEq)]
pub struct Sim3 {
    /// Flat parameter storage: [tx, ty, tz, qw, qx, qy, qz, scale]
    params: SVector<f64, 8>,
}

impl Display for Sim3 {
    fn fmt(&self, f: &mut Formatter<'_>) -> fmt::Result {
        let t = self.translation();
        let s = self.scale();
        let q = self.rotation_quaternion();
        write!(
            f,
            "Sim3(translation: [{:.4}, {:.4}, {:.4}], scale: {:.4}, rotation: [w: {:.4}, x: {:.4}, y: {:.4}, z: {:.4}])",
            t.x, t.y, t.z, s, q.w, q.i, q.j, q.k
        )
    }
}

/// Tikhonov-regularized inverse for numerically singular Sim(3) blocks.
///
/// A σ ≈ 0 tangent element makes the V/Jacobian blocks singular mid-solve.
/// Returning identity (the old fallback) silently corrupts the step to a
/// generic identity map; the regularized inverse keeps the block's structure
/// so the step degrades gracefully, and the event is surfaced.
fn regularized_inverse_3(m: &Matrix3<f64>, what: &str) -> Matrix3<f64> {
    if let Some(inv) = m.try_inverse() {
        return inv;
    }
    let eps = 1e-9 * (m.trace().abs() + 1.0) / 3.0;
    tracing::warn!(
        "Sim(3) {what} is numerically singular; using Tikhonov-regularized inverse (eps = {eps:.3e})"
    );
    (*m + Matrix3::identity() * eps)
        .try_inverse()
        .unwrap_or_else(|| {
            tracing::error!("Sim(3) {what}: regularized inverse also failed; using identity");
            Matrix3::identity()
        })
}

fn regularized_inverse_7(m: &Matrix7<f64>, what: &str) -> Matrix7<f64> {
    if let Some(inv) = m.try_inverse() {
        return inv;
    }
    let eps = 1e-9 * (m.trace().abs() + 1.0) / 7.0;
    tracing::warn!(
        "Sim(3) {what} is numerically singular; using Tikhonov-regularized inverse (eps = {eps:.3e})"
    );
    (*m + Matrix7::identity() * eps)
        .try_inverse()
        .unwrap_or_else(|| {
            tracing::error!("Sim(3) {what}: regularized inverse also failed; using identity");
            Matrix7::identity()
        })
}

impl Sim3 {
    /// Space dimension - dimension of the ambient space
    pub const DIM: usize = 3;

    /// Degrees of freedom - dimension of the tangent space
    pub const DOF: usize = 7;

    /// Representation size - size of the underlying data representation
    pub const REP_SIZE: usize = 8;

    #[inline]
    fn translation_impl(&self) -> Vector3<f64> {
        Vector3::new(self.params[0], self.params[1], self.params[2])
    }

    #[inline]
    fn rotation_impl(&self) -> SO3 {
        SO3::from_quaternion_wxyz(
            self.params[3],
            self.params[4],
            self.params[5],
            self.params[6],
        )
    }

    #[inline]
    fn scale_impl(&self) -> f64 {
        self.params[7]
    }

    #[inline]
    fn from_parts(t: Vector3<f64>, r: &SO3, scale: f64) -> Self {
        let q = r.params();
        Sim3 {
            params: SVector::<f64, 8>::from([t.x, t.y, t.z, q[0], q[1], q[2], q[3], scale]),
        }
    }

    /// Get the identity element of the group.
    pub fn identity() -> Self {
        Sim3 {
            params: SVector::<f64, 8>::from([0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]),
        }
    }

    /// Get the identity matrix for Jacobians.
    pub fn jacobian_identity() -> Matrix7<f64> {
        Matrix7::<f64>::identity()
    }

    /// Create a new Sim(3) element from translation, rotation, and scale.
    ///
    /// # Arguments
    /// * `translation` - Translation vector [x, y, z]
    /// * `rotation` - Unit quaternion representing rotation
    /// * `scale` - Scale factor (must be positive)
    pub fn new(translation: Vector3<f64>, rotation: UnitQuaternion<f64>, scale: f64) -> Self {
        assert!(scale > 0.0, "Scale must be positive");
        Sim3::from_parts(translation, &SO3::new(rotation), scale)
    }

    /// Create Sim(3) from components.
    pub fn from_components(translation: Vector3<f64>, rotation: SO3, scale: f64) -> Self {
        assert!(scale > 0.0, "Scale must be positive");
        Sim3::from_parts(translation, &rotation, scale)
    }

    /// Get the translation part as a Vector3.
    pub fn translation(&self) -> Vector3<f64> {
        self.translation_impl()
    }

    /// Get the scale factor.
    pub fn scale(&self) -> f64 {
        self.scale_impl()
    }

    /// Get the rotation part as SO3.
    pub fn rotation_so3(&self) -> SO3 {
        self.rotation_impl()
    }

    /// Get the rotation part as a UnitQuaternion.
    pub fn rotation_quaternion(&self) -> UnitQuaternion<f64> {
        self.rotation_impl().quaternion()
    }

    /// Get the rotation matrix (3x3).
    pub fn rotation_matrix(&self) -> Matrix3<f64> {
        self.rotation_impl().rotation_matrix()
    }

    /// Get the 4x4 homogeneous transformation matrix.
    pub fn matrix(&self) -> Matrix4<f64> {
        let mut mat = Matrix4::identity();
        let rot_mat = self.rotation_matrix();
        let scale = self.scale_impl();

        for i in 0..3 {
            for j in 0..3 {
                mat[(i, j)] = scale * rot_mat[(i, j)];
            }
        }

        mat[(0, 3)] = self.params[0];
        mat[(1, 3)] = self.params[1];
        mat[(2, 3)] = self.params[2];

        mat
    }

    /// Get the x component of translation.
    pub fn x(&self) -> f64 {
        self.params[0]
    }

    /// Get the y component of translation.
    pub fn y(&self) -> f64 {
        self.params[1]
    }

    /// Get the z component of translation.
    pub fn z(&self) -> f64 {
        self.params[2]
    }

    /// Get the parameter vector [tx, ty, tz, qw, qx, qy, qz, s].
    pub fn coeffs(&self) -> [f64; 8] {
        [
            self.params[0],
            self.params[1],
            self.params[2],
            self.params[3],
            self.params[4],
            self.params[5],
            self.params[6],
            self.params[7],
        ]
    }
}

impl LieGroup for Sim3 {
    const NAME: &'static str = "Sim3";

    type TangentVector = Sim3Tangent;
    type JacobianMatrix = Matrix7<f64>;
    type LieAlgebra = Matrix4<f64>;

    /// Get the inverse.
    ///
    /// For Sim(3): g^{-1} = (R^T, -R^T * t / s, 1/s)
    fn inverse(&self, jacobian: Option<&mut Self::JacobianMatrix>) -> Self {
        let rot = self.rotation_impl();
        let rot_inv = rot.inverse(None);
        let scale_inv = 1.0 / self.scale_impl();
        let trans_inv = -rot_inv.act(&self.translation_impl(), None, None) * scale_inv;

        if let Some(jac) = jacobian {
            *jac = -self.adjoint();
        }

        Sim3::from_parts(trans_inv, &rot_inv, scale_inv)
    }

    /// Composition of this and another Sim(3) element.
    ///
    /// g1 ∘ g2 = (R1*R2, s1*R1*t2 + t1, s1*s2)
    fn compose(
        &self,
        other: &Self,
        jacobian_self: Option<&mut Self::JacobianMatrix>,
        jacobian_other: Option<&mut Self::JacobianMatrix>,
    ) -> Self {
        let rot = self.rotation_impl();
        let scale = self.scale_impl();
        let composed_rotation = rot.compose(&other.rotation_impl(), None, None);
        let composed_translation =
            scale * rot.act(&other.translation_impl(), None, None) + self.translation_impl();
        let composed_scale = scale * other.scale_impl();

        let result = Sim3::from_parts(composed_translation, &composed_rotation, composed_scale);

        if let Some(jac_self) = jacobian_self {
            *jac_self = other.inverse(None).adjoint();
        }

        if let Some(jac_other) = jacobian_other {
            *jac_other = Matrix7::identity();
        }

        result
    }

    /// Logarithmic map from Sim(3) to its tangent space.
    fn log(&self, jacobian: Option<&mut Self::JacobianMatrix>) -> Self::TangentVector {
        let theta = self.rotation_impl().log(None);
        let sigma = self.scale_impl().ln();
        let mut data = Vector7::zeros();

        // Compute the V^{-1} matrix for Sim(3)
        // V^{-1} = J_l^{-1}(θ) * (I - σ/2 * [θ]× + ...)
        let theta_tangent = SO3Tangent::new(theta.coeffs());
        let v_inv = Self::compute_v_inv(&theta_tangent, sigma);
        let translation_vector = v_inv * self.translation_impl();

        data.fixed_rows_mut::<3>(0).copy_from(&translation_vector);
        data.fixed_rows_mut::<3>(3).copy_from(&theta.coeffs());
        data[6] = sigma;

        let result = Sim3Tangent { data };

        if let Some(jac) = jacobian {
            *jac = result.right_jacobian_inv();
        }

        result
    }

    fn act(
        &self,
        vector: &Vector3<f64>,
        jacobian_self: Option<&mut Self::JacobianMatrix>,
        jacobian_vector: Option<&mut Matrix3<f64>>,
    ) -> Vector3<f64> {
        let rot = self.rotation_impl();
        let scale = self.scale_impl();
        let rotation_matrix = rot.rotation_matrix();
        let result = scale * rot.act(vector, None, None) + self.translation_impl();

        if let Some(jac_self) = jacobian_self {
            let rotated_vector = rot.act(vector, None, None);

            // y = s·R·p + t; under a right perturbation X·Exp(δ) the
            // translation column is d(sRp+t)/dρ = s·R (not the identity —
            // the point is transformed by the full scaled rotation first).
            jac_self
                .fixed_view_mut::<3, 3>(0, 0)
                .copy_from(&(scale * rotation_matrix));

            jac_self
                .fixed_view_mut::<3, 3>(0, 3)
                .copy_from(&(-scale * rotation_matrix * SO3Tangent::new(*vector).hat()));

            // d(sRp)/dσ = s·R·p (the exponential's scale term contributes a
            // factor of the already-scaled-and-rotated point, not R·p alone).
            jac_self
                .fixed_view_mut::<3, 1>(0, 6)
                .copy_from(&(scale * rotated_vector));
        }

        if let Some(jac_vector) = jacobian_vector {
            *jac_vector = scale * rotation_matrix;
        }

        result
    }

    fn adjoint(&self) -> Self::JacobianMatrix {
        let rotation_matrix = self.rotation_impl().rotation_matrix();
        let translation = self.translation_impl();
        let scale = self.scale_impl();
        let mut adjoint_matrix = Matrix7::zeros();

        // From X·ξ^·X⁻¹ = (Ad_X ξ)^, block structure for Sim(3):
        // [sR   [t]×R   -t]
        // [0      R      0]
        // [0      0       1]

        // Top-left: s*R
        adjoint_matrix
            .fixed_view_mut::<3, 3>(0, 0)
            .copy_from(&(scale * rotation_matrix));

        // Top-middle: [t]× * R (the scale factors cancel against X⁻¹'s 1/s)
        let top_middle = SO3Tangent::new(translation).hat() * rotation_matrix;
        adjoint_matrix
            .fixed_view_mut::<3, 3>(0, 3)
            .copy_from(&top_middle);

        // Top-right: -t (scale perturbation of X⁻¹'s translation contributes -t)
        adjoint_matrix
            .fixed_view_mut::<3, 1>(0, 6)
            .copy_from(&(-translation));

        // Middle-middle: R
        adjoint_matrix
            .fixed_view_mut::<3, 3>(3, 3)
            .copy_from(&rotation_matrix);

        // Bottom-right: 1
        adjoint_matrix[(6, 6)] = 1.0;

        adjoint_matrix
    }

    fn random() -> Self {
        use rand::Rng;
        let mut rng = rand::rng();

        let translation = Vector3::new(
            rng.random_range(-1.0..1.0),
            rng.random_range(-1.0..1.0),
            rng.random_range(-1.0..1.0),
        );

        let rotation = SO3::random();
        let scale = rng.random_range(0.5..2.0);

        Sim3::from_parts(translation, &rotation, scale)
    }

    fn normalize(&mut self) {
        let mut rot = self.rotation_impl();
        rot.normalize();
        let q = rot.params();
        self.params[3] = q[0];
        self.params[4] = q[1];
        self.params[5] = q[2];
        self.params[6] = q[3];
        if self.params[7] <= 0.0 {
            self.params[7] = 1.0;
        }
    }

    fn is_valid(&self, tolerance: f64) -> bool {
        // ISSUE-0008: translation was never checked, and scale only checked
        // `> 0.0` — `+infinity` passes a bare positivity check even though
        // NaN correctly fails it (`NaN > 0.0` is `false`). Both translation
        // and scale must be finite in addition to scale being positive.
        self.translation_impl().iter().all(|t| t.is_finite())
            && self.rotation_impl().is_valid(tolerance)
            && self.scale_impl().is_finite()
            && self.scale_impl() > 0.0
    }

    fn as_param_slice(&self) -> &[f64] {
        self.params.as_slice()
    }

    fn as_param_slice_mut(&mut self) -> &mut [f64] {
        self.params.as_mut_slice()
    }

    fn from_param_slice(s: &[f64]) -> Self {
        debug_assert_eq!(s.len(), 8);
        Sim3 {
            params: SVector::from_column_slice(s),
        }
    }

    fn vee(&self) -> Self::TangentVector {
        self.log(None)
    }

    fn is_approx(&self, other: &Self, tolerance: f64) -> bool {
        let difference = self.right_minus(other, None, None);
        difference.is_zero(tolerance)
    }

    fn jacobian_identity() -> Self::JacobianMatrix {
        Matrix7::<f64>::identity()
    }

    fn zero_jacobian() -> Self::JacobianMatrix {
        Matrix7::<f64>::zeros()
    }
}

impl Sim3 {
    /// Compute V^{-1} matrix for Sim(3) logarithm.
    ///
    /// Compute the inverse of the V matrix for Sim(3) logarithm.
    fn compute_v_inv(theta: &SO3Tangent, sigma: f64) -> Matrix3<f64> {
        let v = Sim3Tangent::v_matrix(theta, sigma);
        regularized_inverse_3(&v, "V⁻¹")
    }
}

/// Sim(3) tangent space element.
///
/// Represented as [ρ(3), θ(3), σ(1)] where:
/// - ρ: translational component
/// - θ: rotational component (axis-angle)
/// - σ: logarithmic scale
#[derive(Clone, PartialEq)]
pub struct Sim3Tangent {
    /// Internal data: [ρ_x, ρ_y, ρ_z, θ_x, θ_y, θ_z, σ]
    data: Vector7<f64>,
}

impl fmt::Display for Sim3Tangent {
    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
        let rho = self.rho();
        let theta = self.theta();
        let sigma = self.sigma();
        write!(
            f,
            "sim3(rho: [{:.4}, {:.4}, {:.4}], theta: [{:.4}, {:.4}, {:.4}], sigma: {:.4})",
            rho.x, rho.y, rho.z, theta.x, theta.y, theta.z, sigma
        )
    }
}

impl Sim3Tangent {
    /// Create a new Sim(3)Tangent from components.
    pub fn new(rho: Vector3<f64>, theta: Vector3<f64>, sigma: f64) -> Self {
        let mut data = Vector7::zeros();
        data.fixed_rows_mut::<3>(0).copy_from(&rho);
        data.fixed_rows_mut::<3>(3).copy_from(&theta);
        data[6] = sigma;
        Sim3Tangent { data }
    }

    /// Get the ρ (translational) part.
    pub fn rho(&self) -> Vector3<f64> {
        self.data.fixed_rows::<3>(0).into_owned()
    }

    /// Get the θ (rotational) part.
    pub fn theta(&self) -> Vector3<f64> {
        self.data.fixed_rows::<3>(3).into_owned()
    }

    /// Get the σ (logarithmic scale) part.
    pub fn sigma(&self) -> f64 {
        self.data[6]
    }

    /// Create Sim3Tangent from individual scalar components.
    pub fn from_components(
        rho_x: f64,
        rho_y: f64,
        rho_z: f64,
        theta_x: f64,
        theta_y: f64,
        theta_z: f64,
        sigma: f64,
    ) -> Self {
        Sim3Tangent {
            data: Vector7::from_column_slice(&[
                rho_x, rho_y, rho_z, theta_x, theta_y, theta_z, sigma,
            ]),
        }
    }

    /// Compute the V matrix for Sim(3) exponential map.
    ///
    /// V = ∫₀¹ exp(σt) · exp(θt×) dt = A·I + B·[θ]× + C·[θ]ײ
    ///
    /// Based on Ethan Eade's "Lie Groups for Computer Vision" formulation.
    fn v_matrix(theta: &SO3Tangent, sigma: f64) -> Matrix3<f64> {
        let theta_norm_sq = theta.coeffs().norm_squared();

        if theta_norm_sq < crate::SMALL_ANGLE_THRESHOLD
            && sigma.abs() < crate::SMALL_ANGLE_THRESHOLD
        {
            return Matrix3::identity();
        }

        let theta_hat = theta.hat();

        if theta_norm_sq < crate::SMALL_ANGLE_THRESHOLD {
            // Pure scale, no rotation: V = (e^σ - 1)/σ · I
            let a = (sigma.exp() - 1.0) / sigma;
            return a * Matrix3::identity();
        }

        let theta_norm = theta_norm_sq.sqrt();
        let sin_theta = theta_norm.sin();
        let cos_theta = theta_norm.cos();

        if sigma.abs() < crate::SMALL_ANGLE_THRESHOLD {
            // Pure rotation, no scale: V = SO(3) left Jacobian
            let a = 1.0;
            let b = (1.0 - cos_theta) / theta_norm_sq;
            let c = (theta_norm - sin_theta) / (theta_norm * theta_norm_sq);
            return a * Matrix3::identity() + b * theta_hat + c * theta_hat * theta_hat;
        }

        // General case: both sigma and theta nonzero
        let e_sigma = sigma.exp();
        let alpha_sq = sigma * sigma + theta_norm_sq;

        let a = (e_sigma - 1.0) / sigma;
        let b = (e_sigma * (sigma * sin_theta - theta_norm * cos_theta) + theta_norm)
            / (theta_norm * alpha_sq);
        let cos_integral =
            (e_sigma * (sigma * cos_theta + theta_norm * sin_theta) - sigma) / alpha_sq;
        let c = (a - cos_integral) / theta_norm_sq;

        a * Matrix3::identity() + b * theta_hat + c * theta_hat * theta_hat
    }
}

/// `∫₀¹ Ad(Exp(-t·ξ)) dt` via composite Simpson's rule — see
/// [`Sim3Tangent::right_jacobian`] for why this replaces a closed-form `Q` block.
fn integrate_adjoint_of_negated_exp(xi: &Sim3Tangent) -> Matrix7<f64> {
    // Every adjoint()/exp() entry integrated here is closed-form and
    // singularity-free for all θ, σ, so the integrand is smooth (built from
    // sin/cos/exp) with no small-angle branch to mistune; 256 intervals gives
    // several orders of magnitude of margin below central-difference test
    // tolerances (error bound ~1e-11 even at θ near π).
    const INTERVALS: usize = 256;
    let h = 1.0 / INTERVALS as f64;

    let adjoint_at = |t: f64| -> Matrix7<f64> {
        let scaled = Sim3Tangent {
            data: xi.data * (-t),
        };
        scaled.exp(None).adjoint()
    };

    let mut sum = adjoint_at(0.0) + adjoint_at(1.0);
    for i in 1..INTERVALS {
        let t = i as f64 * h;
        let weight = if i % 2 == 1 { 4.0 } else { 2.0 };
        sum += adjoint_at(t) * weight;
    }

    sum * (h / 3.0)
}

impl Tangent<Sim3> for Sim3Tangent {
    const DIM: usize = 7;

    /// Exponential map to Sim(3).
    fn exp(&self, jacobian: Option<&mut <Sim3 as LieGroup>::JacobianMatrix>) -> Sim3 {
        let rho = self.rho();
        let theta = self.theta();
        let sigma = self.sigma();

        let theta_tangent = SO3Tangent::new(theta);
        let rotation = theta_tangent.exp(None);
        let v_matrix = Self::v_matrix(&theta_tangent, sigma);
        let translation = v_matrix * rho;
        let scale = sigma.exp();

        if let Some(jac) = jacobian {
            *jac = self.right_jacobian();
        }

        Sim3::from_components(translation, rotation, scale)
    }

    /// Right Jacobian for Sim(3).
    ///
    /// Computed from the general Lie-group identity `Jr(ξ) = ∫₀¹ Ad(Exp(-tξ)) dt`
    /// (e.g. Solà, "A micro Lie theory for state estimation in robotics", eq. 146),
    /// evaluated by [`integrate_adjoint_of_negated_exp`]. Unlike SE(3)'s `Q`
    /// block, Sim(3)'s exact translation/rotation/scale coupling does not
    /// reduce to a compact closed form; this identity sidesteps re-deriving
    /// one by hand and is verified against central differences of `Exp` in
    /// the test suite.
    fn right_jacobian(&self) -> <Sim3 as LieGroup>::JacobianMatrix {
        integrate_adjoint_of_negated_exp(self)
    }

    /// Left Jacobian for Sim(3): `Jl(ξ) = Jr(-ξ)` (general Lie-group identity).
    fn left_jacobian(&self) -> <Sim3 as LieGroup>::JacobianMatrix {
        let negated = Sim3Tangent { data: -self.data };
        negated.right_jacobian()
    }

    /// Inverse of right Jacobian.
    fn right_jacobian_inv(&self) -> <Sim3 as LieGroup>::JacobianMatrix {
        regularized_inverse_7(&self.right_jacobian(), "right-Jacobian⁻¹")
    }

    /// Inverse of left Jacobian.
    fn left_jacobian_inv(&self) -> <Sim3 as LieGroup>::JacobianMatrix {
        regularized_inverse_7(&self.left_jacobian(), "left-Jacobian⁻¹")
    }

    /// Hat operator: maps tangent vector to Lie algebra matrix (4x4).
    fn hat(&self) -> <Sim3 as LieGroup>::LieAlgebra {
        let mut lie_alg = Matrix4::zeros();

        let theta_hat = SO3Tangent::new(self.theta()).hat();

        // Top-left 3x3: [θ]× + σ*I
        for i in 0..3 {
            for j in 0..3 {
                lie_alg[(i, j)] = theta_hat[(i, j)];
            }
            lie_alg[(i, i)] += self.sigma();
        }

        // Top-right 3x1: ρ
        let rho = self.rho();
        lie_alg[(0, 3)] = rho[0];
        lie_alg[(1, 3)] = rho[1];
        lie_alg[(2, 3)] = rho[2];

        lie_alg
    }

    fn zero() -> <Sim3 as LieGroup>::TangentVector {
        Sim3Tangent::new(Vector3::zeros(), Vector3::zeros(), 0.0)
    }

    fn random() -> <Sim3 as LieGroup>::TangentVector {
        use rand::Rng;
        let mut rng = rand::rng();
        Sim3Tangent::new(
            Vector3::new(
                rng.random_range(-1.0..1.0),
                rng.random_range(-1.0..1.0),
                rng.random_range(-1.0..1.0),
            ),
            Vector3::new(
                rng.random_range(-0.1..0.1),
                rng.random_range(-0.1..0.1),
                rng.random_range(-0.1..0.1),
            ),
            rng.random_range(-0.5..0.5),
        )
    }

    fn is_zero(&self, tolerance: f64) -> bool {
        self.data.norm() < tolerance
    }

    fn normalize(&mut self) {
        let theta_norm = self.theta().norm();
        if theta_norm > f64::EPSILON {
            self.data[3] /= theta_norm;
            self.data[4] /= theta_norm;
            self.data[5] /= theta_norm;
        }
    }

    fn normalized(&self) -> <Sim3 as LieGroup>::TangentVector {
        let norm = self.theta().norm();
        if norm > f64::EPSILON {
            Sim3Tangent::new(self.rho(), self.theta() / norm, self.sigma())
        } else {
            Sim3Tangent::new(self.rho(), Vector3::zeros(), self.sigma())
        }
    }

    fn as_slice(&self) -> &[f64] {
        self.data.as_slice()
    }

    fn from_slice(s: &[f64]) -> Self {
        debug_assert_eq!(s.len(), 7);
        Sim3Tangent {
            data: Vector7::from_column_slice(s),
        }
    }

    fn small_adj(&self) -> <Sim3 as LieGroup>::JacobianMatrix {
        let mut small_adj = Matrix7::zeros();
        let rho_skew = SO3Tangent::new(self.rho()).hat();
        let theta_skew = SO3Tangent::new(self.theta()).hat();
        let sigma = self.sigma();

        // Block structure for Sim(3):
        // [θ× + σ*I   ρ×   ρ]
        // [   0       θ×   0]
        // [   0       0    0]

        for i in 0..3 {
            for j in 0..3 {
                small_adj[(i, j)] = theta_skew[(i, j)];
            }
            small_adj[(i, i)] += sigma;
        }

        small_adj.fixed_view_mut::<3, 3>(0, 3).copy_from(&rho_skew);
        small_adj
            .fixed_view_mut::<3, 1>(0, 6)
            .copy_from(&(-self.rho()));
        small_adj
            .fixed_view_mut::<3, 3>(3, 3)
            .copy_from(&theta_skew);

        small_adj
    }

    fn lie_bracket(&self, other: &Self) -> <Sim3 as LieGroup>::TangentVector {
        let bracket_result = self.small_adj() * other.data;
        Sim3Tangent {
            data: bracket_result,
        }
    }

    fn is_approx(&self, other: &Self, tolerance: f64) -> bool {
        (self.data - other.data).norm() < tolerance
    }

    fn generator(&self, i: usize) -> <Sim3 as LieGroup>::LieAlgebra {
        assert!(i < 7, "Sim(3) only has generators for indices 0-6");

        let mut generator = Matrix4::zeros();

        match i {
            0..=2 => {
                // Translation generators (rho)
                generator[(i, 3)] = 1.0;
            }
            3..=5 => {
                // Rotation generators (theta)
                let idx = i - 3;
                match idx {
                    0 => {
                        generator[(1, 2)] = -1.0;
                        generator[(2, 1)] = 1.0;
                    }
                    1 => {
                        generator[(0, 2)] = 1.0;
                        generator[(2, 0)] = -1.0;
                    }
                    2 => {
                        generator[(0, 1)] = -1.0;
                        generator[(1, 0)] = 1.0;
                    }
                    _ => unreachable!(),
                }
            }
            6 => {
                // Scale generator (sigma)
                generator[(0, 0)] = 1.0;
                generator[(1, 1)] = 1.0;
                generator[(2, 2)] = 1.0;
            }
            _ => unreachable!(),
        }

        generator
    }
}

#[cfg(test)]
mod tests {
    use super::*;

    const TOLERANCE: f64 = 1e-9;

    #[test]
    fn test_sim3_identity() {
        let identity = Sim3::identity();
        assert!(identity.is_valid(TOLERANCE));
        assert!(identity.translation().norm() < TOLERANCE);
        assert!((identity.scale() - 1.0).abs() < TOLERANCE);
        assert!(identity.rotation_quaternion().angle() < TOLERANCE);
    }

    /// ISSUE-0008 regression: every stored parameter must be checked, not
    /// just the quaternion — a NaN/Inf translation previously passed
    /// `is_valid` silently, and scale only checked `> 0.0` (so `+infinity`
    /// passed even though it isn't a valid finite scale). Also covers a
    /// non-finite tolerance.
    #[test]
    fn test_sim3_is_valid_rejects_nonfinite_at_every_index() {
        let base = Sim3::identity();
        for idx in 0..Sim3::REP_SIZE {
            for bad in [f64::NAN, f64::INFINITY, f64::NEG_INFINITY] {
                let mut params: Vec<f64> = base.as_param_slice().to_vec();
                params[idx] = bad;
                let perturbed = Sim3::from_param_slice(&params);
                assert!(
                    !perturbed.is_valid(TOLERANCE),
                    "index {idx} = {bad} must be rejected"
                );
            }
        }
        assert!(
            !base.is_valid(f64::INFINITY),
            "infinite tolerance must be rejected"
        );
        assert!(!base.is_valid(f64::NAN), "NaN tolerance must be rejected");
    }

    #[test]
    fn test_sim3_new() {
        let translation = Vector3::new(1.0, 2.0, 3.0);
        let rotation = UnitQuaternion::from_euler_angles(0.1, 0.2, 0.3);
        let scale = 1.5;

        let sim3 = Sim3::new(translation, rotation, scale);
        assert!(sim3.is_valid(TOLERANCE));
        assert!((sim3.translation() - translation).norm() < TOLERANCE);
        assert!((sim3.scale() - scale).abs() < TOLERANCE);
    }

    #[test]
    #[should_panic(expected = "Scale must be positive")]
    fn test_sim3_new_negative_scale() {
        let translation = Vector3::new(1.0, 2.0, 3.0);
        let rotation = UnitQuaternion::identity();
        let _sim3 = Sim3::new(translation, rotation, -1.0);
    }

    #[test]
    fn test_sim3_random() {
        let sim3 = Sim3::random();
        assert!(sim3.is_valid(TOLERANCE));
        assert!(sim3.scale() > 0.0);
    }

    #[test]
    fn test_sim3_inverse() {
        let sim3 = Sim3::random();
        let sim3_inv = sim3.inverse(None);

        let composed = sim3.compose(&sim3_inv, None, None);
        let identity = Sim3::identity();

        assert!(composed.is_approx(&identity, TOLERANCE));
    }

    #[test]
    fn test_sim3_compose() {
        let sim3_1 = Sim3::random();
        let sim3_2 = Sim3::random();

        let composed = sim3_1.compose(&sim3_2, None, None);
        assert!(composed.is_valid(TOLERANCE));

        let identity = Sim3::identity();
        let composed_with_identity = sim3_1.compose(&identity, None, None);
        assert!(composed_with_identity.is_approx(&sim3_1, TOLERANCE));
    }

    #[test]
    fn test_sim3_exp_log() {
        let tangent = Sim3Tangent::new(
            Vector3::new(0.1, 0.2, 0.3),
            Vector3::new(0.01, 0.02, 0.03),
            0.1,
        );

        let sim3 = tangent.exp(None);
        let recovered_tangent = sim3.log(None);

        assert!((tangent.data - recovered_tangent.data).norm() < TOLERANCE);
    }

    #[test]
    fn test_sim3_exp_zero() {
        let zero_tangent = Sim3Tangent::zero();
        let sim3 = zero_tangent.exp(None);
        let identity = Sim3::identity();

        assert!(sim3.is_approx(&identity, TOLERANCE));
    }

    #[test]
    fn test_sim3_log_identity() {
        let identity = Sim3::identity();
        let tangent = identity.log(None);

        assert!(tangent.data.norm() < TOLERANCE);
    }

    #[test]
    fn test_sim3_adjoint() {
        let sim3 = Sim3::random();
        let adj = sim3.adjoint();

        assert_eq!(adj.nrows(), 7);
        assert_eq!(adj.ncols(), 7);
    }

    #[test]
    fn test_sim3_act() {
        let sim3 = Sim3::random();
        let point = Vector3::new(1.0, 2.0, 3.0);

        let _transformed_point = sim3.act(&point, None, None);

        let identity = Sim3::identity();
        let identity_transformed = identity.act(&point, None, None);

        assert!((identity_transformed - point).norm() < TOLERANCE);
    }

    #[test]
    fn test_sim3_between() {
        let sim3_a = Sim3::random();
        let sim3_b = sim3_a.clone();
        let sim3_between_identity = sim3_a.between(&sim3_b, None, None);
        assert!(sim3_between_identity.is_approx(&Sim3::identity(), TOLERANCE));

        let sim3_c = Sim3::random();
        let sim3_between = sim3_a.between(&sim3_c, None, None);
        let expected = sim3_a.inverse(None).compose(&sim3_c, None, None);
        assert!(sim3_between.is_approx(&expected, TOLERANCE));
    }

    #[test]
    fn test_sim3_tangent_zero() {
        let zero = Sim3Tangent::zero();
        assert!(zero.data.norm() < TOLERANCE);

        let tangent = Sim3Tangent::new(Vector3::zeros(), Vector3::zeros(), 0.0);
        assert!(tangent.is_zero(TOLERANCE));
    }

    #[test]
    fn test_sim3_manifold_properties() {
        assert_eq!(Sim3::DIM, 3);
        assert_eq!(Sim3::DOF, 7);
        assert_eq!(Sim3::REP_SIZE, 8);
    }

    #[test]
    fn test_sim3_consistency() {
        let sim3_1 = Sim3::random();
        let sim3_2 = Sim3::random();
        let sim3_3 = Sim3::random();

        // Test associativity
        let left_assoc = sim3_1
            .compose(&sim3_2, None, None)
            .compose(&sim3_3, None, None);
        let right_assoc = sim3_1.compose(&sim3_2.compose(&sim3_3, None, None), None, None);

        assert!(left_assoc.is_approx(&right_assoc, 1e-10));
    }

    #[test]
    fn test_sim3_scale_composition() {
        let sim3_1 = Sim3::new(Vector3::zeros(), UnitQuaternion::identity(), 2.0);
        let sim3_2 = Sim3::new(Vector3::zeros(), UnitQuaternion::identity(), 3.0);

        let composed = sim3_1.compose(&sim3_2, None, None);
        assert!((composed.scale() - 6.0).abs() < TOLERANCE);
    }

    #[test]
    fn test_sim3_tangent_small_adj() {
        let tangent = Sim3Tangent::new(
            Vector3::new(0.1, 0.2, 0.3),
            Vector3::new(0.4, 0.5, 0.6),
            0.1,
        );
        let small_adj = tangent.small_adj();

        assert_eq!(small_adj.nrows(), 7);
        assert_eq!(small_adj.ncols(), 7);
    }

    #[test]
    fn test_sim3_tangent_lie_bracket() {
        let tangent_a = Sim3Tangent::new(
            Vector3::new(0.1, 0.0, 0.0),
            Vector3::new(0.0, 0.2, 0.0),
            0.1,
        );
        let tangent_b = Sim3Tangent::new(
            Vector3::new(0.0, 0.3, 0.0),
            Vector3::new(0.0, 0.0, 0.4),
            0.2,
        );

        let bracket_ab = tangent_a.lie_bracket(&tangent_b);
        let bracket_ba = tangent_b.lie_bracket(&tangent_a);

        // Anti-symmetry
        assert!((bracket_ab.data + bracket_ba.data).norm() < 1e-10);

        // [a,a] = 0
        let bracket_aa = tangent_a.lie_bracket(&tangent_a);
        assert!(bracket_aa.is_zero(1e-10));
    }

    #[test]
    fn test_sim3_tangent_is_approx() {
        let tangent_1 = Sim3Tangent::new(
            Vector3::new(0.1, 0.2, 0.3),
            Vector3::new(0.4, 0.5, 0.6),
            0.1,
        );
        let tangent_2 = Sim3Tangent::new(
            Vector3::new(0.1 + 1e-12, 0.2, 0.3),
            Vector3::new(0.4, 0.5, 0.6),
            0.1,
        );
        let tangent_3 = Sim3Tangent::new(
            Vector3::new(1.0, 2.0, 3.0),
            Vector3::new(4.0, 5.0, 6.0),
            1.0,
        );

        assert!(tangent_1.is_approx(&tangent_1, 1e-10));
        assert!(tangent_1.is_approx(&tangent_2, 1e-10));
        assert!(!tangent_1.is_approx(&tangent_3, 1e-10));
    }

    #[test]
    fn test_sim3_generators() {
        let tangent = Sim3Tangent::new(
            Vector3::new(1.0, 1.0, 1.0),
            Vector3::new(1.0, 1.0, 1.0),
            1.0,
        );

        for i in 0..7 {
            let generator = tangent.generator(i);
            assert_eq!(generator.nrows(), 4);
            assert_eq!(generator.ncols(), 4);

            // Bottom row should be zeros
            assert_eq!(generator[(3, 0)], 0.0);
            assert_eq!(generator[(3, 1)], 0.0);
            assert_eq!(generator[(3, 2)], 0.0);
            assert_eq!(generator[(3, 3)], 0.0);
        }
    }

    #[test]
    #[should_panic]
    fn test_sim3_generator_invalid_index() {
        let tangent = Sim3Tangent::new(
            Vector3::new(1.0, 1.0, 1.0),
            Vector3::new(1.0, 1.0, 1.0),
            1.0,
        );
        let _generator = tangent.generator(7);
    }

    #[test]
    fn test_sim3_vee() {
        let sim3 = Sim3::random();
        let tangent_log = sim3.log(None);
        let tangent_vee = sim3.vee();

        assert!((tangent_log.data - tangent_vee.data).norm() < 1e-10);
    }

    #[test]
    fn test_sim3_is_approx() {
        let sim3_1 = Sim3::random();
        let sim3_2 = sim3_1.clone();

        assert!(sim3_1.is_approx(&sim3_1, 1e-10));
        assert!(sim3_1.is_approx(&sim3_2, 1e-10));

        let small_tangent = Sim3Tangent::new(
            Vector3::new(1e-12, 1e-12, 1e-12),
            Vector3::new(1e-12, 1e-12, 1e-12),
            1e-12,
        );
        let sim3_perturbed = sim3_1.right_plus(&small_tangent, None, None);
        assert!(sim3_1.is_approx(&sim3_perturbed, 1e-10));
    }

    #[test]
    fn test_sim3_small_angle_approximations() {
        let small_tangent = Sim3Tangent::new(
            Vector3::new(1e-8, 2e-8, 3e-8),
            Vector3::new(1e-9, 2e-9, 3e-9),
            1e-8,
        );

        let sim3 = small_tangent.exp(None);
        let recovered = sim3.log(None);

        assert!((small_tangent.data - recovered.data).norm() < TOLERANCE);
    }

    #[test]
    fn test_sim3_accessors() {
        let translation = Vector3::new(1.0, 2.0, 3.0);
        let rotation = UnitQuaternion::identity();
        let scale = 1.5;

        let sim3 = Sim3::new(translation, rotation, scale);

        assert_eq!(sim3.x(), 1.0);
        assert_eq!(sim3.y(), 2.0);
        assert_eq!(sim3.z(), 3.0);
        assert_eq!(sim3.scale(), 1.5);
    }

    #[test]
    fn test_sim3_matrix() {
        let translation = Vector3::new(1.0, 2.0, 3.0);
        let rotation = UnitQuaternion::identity();
        let scale = 2.0;

        let sim3 = Sim3::new(translation, rotation, scale);
        let mat = sim3.matrix();

        // Check that top-left 3x3 is scaled rotation (2*I in this case)
        for i in 0..3 {
            for j in 0..3 {
                if i == j {
                    assert!((mat[(i, j)] - 2.0).abs() < TOLERANCE);
                } else {
                    assert!(mat[(i, j)].abs() < TOLERANCE);
                }
            }
        }

        // Check translation
        assert!((mat[(0, 3)] - 1.0).abs() < TOLERANCE);
        assert!((mat[(1, 3)] - 2.0).abs() < TOLERANCE);
        assert!((mat[(2, 3)] - 3.0).abs() < TOLERANCE);

        // Check bottom row
        assert!(mat[(3, 0)].abs() < TOLERANCE);
        assert!(mat[(3, 1)].abs() < TOLERANCE);
        assert!(mat[(3, 2)].abs() < TOLERANCE);
        assert!((mat[(3, 3)] - 1.0).abs() < TOLERANCE);
    }

    #[test]
    fn test_sim3_scale_action() {
        let scale = 2.0;
        let sim3 = Sim3::new(Vector3::zeros(), UnitQuaternion::identity(), scale);
        let point = Vector3::new(1.0, 2.0, 3.0);

        let transformed = sim3.act(&point, None, None);

        assert!((transformed.x - 2.0).abs() < TOLERANCE);
        assert!((transformed.y - 4.0).abs() < TOLERANCE);
        assert!((transformed.z - 6.0).abs() < TOLERANCE);
    }

    #[test]
    fn test_sim3_tangent_basic() {
        let rho = Vector3::new(0.1, 0.2, 0.3);
        let theta = Vector3::new(0.4, 0.5, 0.6);
        let sigma = 0.1;
        let tangent = Sim3Tangent::new(rho, theta, sigma);
        assert!((tangent.rho() - rho).norm() < TOLERANCE);
        assert!((tangent.theta() - theta).norm() < TOLERANCE);
        assert!((tangent.sigma() - sigma).abs() < TOLERANCE);
    }

    #[test]
    fn test_sim3_tangent_from_components() {
        let t1 = Sim3Tangent::new(
            Vector3::new(1.0, 2.0, 3.0),
            Vector3::new(0.4, 0.5, 0.6),
            0.1,
        );
        let t2 = Sim3Tangent::from_components(1.0, 2.0, 3.0, 0.4, 0.5, 0.6, 0.1);
        assert!(t1.is_approx(&t2, TOLERANCE));
    }

    #[test]
    fn test_sim3_normalize() {
        let mut sim3 = Sim3::random();
        sim3.normalize();
        assert!(sim3.is_valid(TOLERANCE));
    }

    #[test]
    fn test_sim3_coeffs() {
        let translation = Vector3::new(1.0, 2.0, 3.0);
        let rotation = UnitQuaternion::identity();
        let scale = 1.5;
        let sim3 = Sim3::new(translation, rotation, scale);
        let c = sim3.coeffs();
        assert!((c[0] - 1.0).abs() < TOLERANCE);
        assert!((c[1] - 2.0).abs() < TOLERANCE);
        assert!((c[2] - 3.0).abs() < TOLERANCE);
        assert!((c[3] - 1.0).abs() < TOLERANCE); // qw
        assert!((c[7] - 1.5).abs() < TOLERANCE); // scale
    }

    #[test]
    fn test_sim3_manif_like_operations() {
        let a = Sim3::random();
        let b = Sim3::random();

        let a_to_b = a.between(&b, None, None);
        let recovered_b = a.compose(&a_to_b, None, None);
        assert!(recovered_b.is_approx(&b, TOLERANCE));

        let tangent = Sim3Tangent::new(
            Vector3::new(0.01, 0.02, 0.03),
            Vector3::new(0.001, 0.002, 0.003),
            0.01,
        );
        let perturbed = a.plus(&tangent, None, None);
        let recovered_tangent = perturbed.minus(&a, None, None);
        assert!((tangent.data - recovered_tangent.data).norm() < 1e-6);
    }

    #[test]
    fn test_sim3_right_jacobian_inverse_identity() {
        let tangent = Sim3Tangent::new(
            Vector3::new(0.1, 0.2, 0.3),
            Vector3::new(0.01, 0.02, 0.03),
            0.1,
        );
        let jr = tangent.right_jacobian();
        let jr_inv = tangent.right_jacobian_inv();
        let product = jr * jr_inv;
        let identity = Matrix7::<f64>::identity();
        assert!((product - identity).norm() < 0.01);
    }

    #[test]
    fn test_sim3_left_jacobian_inverse_identity() {
        let tangent = Sim3Tangent::new(
            Vector3::new(0.1, 0.2, 0.3),
            Vector3::new(0.01, 0.02, 0.03),
            0.1,
        );
        let jl = tangent.left_jacobian();
        let jl_inv = tangent.left_jacobian_inv();
        let product = jl * jl_inv;
        let identity = Matrix7::<f64>::identity();
        assert!((product - identity).norm() < 0.01);
    }

    #[test]
    fn test_sim3_jacobi_identity() {
        let a = Sim3Tangent::random();
        let b = Sim3Tangent::random();
        let c = Sim3Tangent::random();

        let term1 = a.lie_bracket(&b.lie_bracket(&c));
        let term2 = b.lie_bracket(&c.lie_bracket(&a));
        let term3 = c.lie_bracket(&a.lie_bracket(&b));

        assert!((term1.data + term2.data + term3.data).norm() < 1e-8);
    }

    #[test]
    fn test_sim3_hat_matrix_structure() {
        let tangent = Sim3Tangent::new(
            Vector3::new(1.0, 2.0, 3.0),
            Vector3::new(0.1, 0.2, 0.3),
            0.5,
        );
        let hat = tangent.hat();

        for j in 0..4 {
            assert_eq!(hat[(3, j)], 0.0);
        }

        assert!((hat[(0, 3)] - 1.0).abs() < TOLERANCE);
        assert!((hat[(1, 3)] - 2.0).abs() < TOLERANCE);
        assert!((hat[(2, 3)] - 3.0).abs() < TOLERANCE);

        // Diagonal includes sigma
        assert!((hat[(0, 0)] - 0.5).abs() < 0.5); // theta_skew + sigma
    }

    #[test]
    fn test_sim3_param_slice_round_trip() {
        let sim3 = Sim3::random();
        let recovered = Sim3::from_param_slice(sim3.as_param_slice());
        assert!(sim3.is_approx(&recovered, TOLERANCE));
    }

    // --- Additional coverage tests ---

    #[test]
    fn test_sim3_display() {
        let sim3 = Sim3::identity();
        let s = format!("{}", sim3);
        assert!(s.contains("Sim3"));

        let tangent = Sim3Tangent::new(
            Vector3::new(1.0, 2.0, 3.0),
            Vector3::new(0.1, 0.2, 0.3),
            0.5,
        );
        let ts = format!("{}", tangent);
        assert!(ts.contains("sim3"));
    }

    #[test]
    fn test_sim3_jacobian_identity_static() {
        let jac = Sim3::jacobian_identity();
        assert!((jac - Matrix7::identity()).norm() < TOLERANCE);
    }

    #[test]
    fn test_sim3_rotation_so3() {
        let sim3 = Sim3::random();
        let so3 = sim3.rotation_so3();
        assert!(so3.is_valid(TOLERANCE));
    }

    #[test]
    fn test_sim3_inverse_with_jacobian() {
        let sim3 = Sim3::random();
        let mut jac = Matrix7::zeros();
        let inv = sim3.inverse(Some(&mut jac));
        let expected_jac = -sim3.adjoint();
        assert!((jac - expected_jac).norm() < TOLERANCE);
        let composed = sim3.compose(&inv, None, None);
        assert!(composed.is_approx(&Sim3::identity(), TOLERANCE));
    }

    #[test]
    fn test_sim3_compose_with_jacobians() {
        let a = Sim3::random();
        let b = Sim3::random();
        let mut jac_a = Matrix7::zeros();
        let mut jac_b = Matrix7::zeros();
        let _composed = a.compose(&b, Some(&mut jac_a), Some(&mut jac_b));
        assert!(jac_a.norm() > 0.0);
        assert!(jac_b.norm() > 0.0);
    }

    #[test]
    fn test_sim3_act_with_jacobians() {
        let sim3 = Sim3::random();
        let point = Vector3::new(1.0, 2.0, 3.0);
        let mut jac_self = Matrix7::zeros();
        let mut jac_point = Matrix3::<f64>::zeros();
        let result = sim3.act(&point, Some(&mut jac_self), Some(&mut jac_point));
        assert!(result.norm() > 0.0);
        assert!(jac_self.norm() > 0.0);
        assert!(jac_point.norm() > 0.0);
    }

    #[test]
    fn test_sim3_liegroup_jacobian_identity() {
        let jac = <Sim3 as LieGroup>::jacobian_identity();
        assert!((jac - Matrix7::identity()).norm() < TOLERANCE);

        let zero = <Sim3 as LieGroup>::zero_jacobian();
        assert!(zero.norm() < TOLERANCE);
    }

    #[test]
    fn test_sim3_tangent_slice_round_trip() {
        let tangent = Sim3Tangent::new(
            Vector3::new(1.0, 2.0, 3.0),
            Vector3::new(0.1, 0.2, 0.3),
            0.5,
        );
        let recovered = Sim3Tangent::from_slice(tangent.as_slice());
        assert!(tangent.is_approx(&recovered, TOLERANCE));
    }

    #[test]
    fn test_sim3_exp_with_jacobian() {
        let tangent = Sim3Tangent::new(
            Vector3::new(0.1, 0.2, 0.3),
            Vector3::new(0.01, 0.02, 0.03),
            0.1,
        );
        let mut jac = Matrix7::zeros();
        let _result = tangent.exp(Some(&mut jac));
        assert!(jac.norm() > 0.0);
    }

    #[test]
    fn test_sim3_exp_log_stress() {
        for _ in 0..100 {
            let tangent = Sim3Tangent::random();
            let sim3 = tangent.exp(None);
            let recovered = sim3.log(None);
            assert!(
                tangent.is_approx(&recovered, 1e-6),
                "exp/log round-trip failed: error = {}",
                (tangent.data - recovered.data).norm()
            );
        }
    }

    #[test]
    fn test_sim3_right_plus_minus_round_trip() {
        let a = Sim3::random();
        let b = Sim3::random();
        let diff = a.right_minus(&b, None, None);
        let recovered = b.right_plus(&diff, None, None);
        assert!(a.is_approx(&recovered, 1e-6));
    }

    #[test]
    fn test_sim3_left_plus_minus_round_trip() {
        let a = Sim3::random();
        let b = Sim3::random();
        let diff = a.left_minus(&b, None, None);
        let recovered = b.left_plus(&diff, None, None);
        assert!(a.is_approx(&recovered, 1e-6));
    }

    #[test]
    fn test_sim3_compose_associativity() {
        let a = Sim3::random();
        let b = Sim3::random();
        let c = Sim3::random();
        let ab_c = a.compose(&b, None, None).compose(&c, None, None);
        let a_bc = a.compose(&b.compose(&c, None, None), None, None);
        assert!(ab_c.is_approx(&a_bc, 1e-6));
    }

    #[test]
    fn test_sim3_inverse_twice() {
        let g = Sim3::random();
        let g_inv_inv = g.inverse(None).inverse(None);
        assert!(g.is_approx(&g_inv_inv, 1e-6));
    }

    #[test]
    fn test_sim3_pure_scale() {
        // exp/log with only sigma nonzero (theta=0)
        let tangent = Sim3Tangent::new(Vector3::zeros(), Vector3::zeros(), 0.5);
        let sim3 = tangent.exp(None);
        let recovered = sim3.log(None);
        assert!(tangent.is_approx(&recovered, TOLERANCE));
        assert!((sim3.scale() - 0.5_f64.exp()).abs() < TOLERANCE);
    }

    #[test]
    fn test_sim3_pure_rotation() {
        // exp/log with only theta nonzero (sigma=0)
        let tangent = Sim3Tangent::new(
            Vector3::new(0.1, 0.2, 0.3),
            Vector3::new(0.05, 0.1, 0.15),
            0.0,
        );
        let sim3 = tangent.exp(None);
        let recovered = sim3.log(None);
        assert!(tangent.is_approx(&recovered, 1e-6));
        assert!((sim3.scale() - 1.0).abs() < TOLERANCE);
    }

    #[test]
    fn test_sim3_scale_compose() {
        let a = Sim3::from_components(Vector3::zeros(), SO3::identity(), 2.0);
        let b = Sim3::from_components(Vector3::zeros(), SO3::identity(), 3.0);
        let ab = a.compose(&b, None, None);
        assert!((ab.scale() - 6.0).abs() < TOLERANCE);
    }

    #[test]
    fn test_sim3_tangent_normalize_zero_theta() {
        let tangent = Sim3Tangent::new(Vector3::new(1.0, 2.0, 3.0), Vector3::zeros(), 0.5);
        let normalized = tangent.normalized();
        assert!(normalized.theta().norm() < TOLERANCE);
    }

    #[test]
    fn test_sim3_tangent_generator_all() {
        let tangent = Sim3Tangent::zero();
        for i in 0..7 {
            let g = tangent.generator(i);
            assert!(g.norm() > 0.0, "Generator {} should be non-zero", i);
        }
    }

    #[test]
    fn test_sim3_v_matrix_sigma_zero_matches_so3_jl() {
        // When sigma=0, V-matrix should equal SO3 left Jacobian
        let theta = SO3Tangent::new(Vector3::new(0.3, 0.4, 0.5));
        let v = Sim3Tangent::v_matrix(&theta, 0.0);
        let jl = theta.left_jacobian();
        assert!(
            (v - jl).norm() < 1e-10,
            "V(θ,0) should equal Jl(θ), error = {}",
            (v - jl).norm()
        );
    }

    #[test]
    fn test_sim3_v_matrix_theta_zero() {
        // When theta=0, V-matrix should be (e^σ - 1)/σ * I
        let theta = SO3Tangent::new(Vector3::zeros());
        let sigma = 0.5;
        let v = Sim3Tangent::v_matrix(&theta, sigma);
        let expected = (sigma.exp() - 1.0) / sigma * Matrix3::<f64>::identity();
        assert!(
            (v - expected).norm() < 1e-10,
            "V(0,σ) should equal (e^σ-1)/σ * I, error = {}",
            (v - expected).norm()
        );
    }

    #[test]
    fn sim3_param_slice_round_trip() {
        let g = Sim3::random();
        let recovered = Sim3::from_param_slice(g.as_param_slice());
        assert!(g.is_approx(&recovered, 1e-14));
    }

    #[test]
    fn sim3_tangent_slice_round_trip() {
        let t = Sim3Tangent::random();
        let recovered = Sim3Tangent::from_slice(t.as_slice());
        assert!(t.is_approx(&recovered, 1e-14));
    }
}

#[cfg(test)]
mod regularized_inverse_tests {
    use super::*;
    use nalgebra::Matrix3;

    #[test]
    fn regular_matrix_inverts_unchanged() {
        let m = Matrix3::new(2.0, 1.0, 0.0, 1.0, 3.0, 1.0, 0.0, 1.0, 2.0);
        let inv = regularized_inverse_3(&m, "test");
        let identity = m * inv;
        assert!((identity - Matrix3::identity()).norm() < 1e-12);
    }

    #[test]
    fn singular_matrix_warns_and_regularizes() {
        // Rank-1 matrix: try_inverse returns None, the helper must NOT return
        // identity (the old silent corruption) — the regularized inverse keeps
        // the block's row structure.
        let singular = Matrix3::from_columns(&[
            Vector3::new(1.0, 2.0, 3.0),
            Vector3::new(2.0, 4.0, 6.0),
            Vector3::zeros(),
        ]);
        let inv = regularized_inverse_3(&singular, "test");
        assert!(
            (inv - Matrix3::identity()).norm() > 1e-3,
            "regularized inverse must not silently fall back to identity"
        );
    }
}

/// Independent central-difference regression tests for ISSUE-0002 (Sim(3)
/// differential geometry). These build their own finite-difference oracle
/// from `exp()`/`matrix()`/`inverse()` alone — never from `log()`,
/// `right_jacobian_inv()`, `left_jacobian_inv()`, or `v_matrix()` — so a bug
/// shared between an analytic Jacobian and its "inverse" cannot hide here
/// (see `codex/issues/ISSUE-0002-sim3-differentials.md`, "Do not validate an
/// analytic Jacobian solely with an inverse built from that same Jacobian.").
#[cfg(test)]
mod sim3_jacobian_fd_tests {
    use super::*;

    // `SMALL_ANGLE_THRESHOLD` (1e-10, compared against θ²) means a step below
    // ~1e-5 can leave a perturbed θ inside `v_matrix`'s small-angle branch,
    // silently discarding the perturbation and making the central difference
    // artificially zero — not a bug in the analytic Jacobian. 1e-4 clears
    // that threshold with comfortable margin while keeping truncation error
    // far below `FD_TOL`.
    const FD_STEP: f64 = 1e-4;
    const FD_TOL: f64 = 1e-5;

    /// Extract [ρ,θ,σ] from a 4x4 Sim(3) Lie-algebra matrix
    /// `[θ× + σI, ρ; 0, 0]` (the "vee" map). Purely algebraic — no calls into
    /// this module's own `log`/`v_matrix`/Jacobian code.
    fn vee(m: &Matrix4<f64>) -> Vector7<f64> {
        let mut v = Vector7::zeros();
        v[0] = m[(0, 3)];
        v[1] = m[(1, 3)];
        v[2] = m[(2, 3)];
        v[3] = m[(2, 1)];
        v[4] = m[(0, 2)];
        v[5] = m[(1, 0)];
        v[6] = (m[(0, 0)] + m[(1, 1)] + m[(2, 2)]) / 3.0;
        v
    }

    fn perturbed(xi: &Sim3Tangent, k: usize, eps: f64) -> Sim3Tangent {
        let mut data = xi.data;
        data[k] += eps;
        Sim3Tangent { data }
    }

    /// Central-difference right Jacobian from first principles: column k is
    /// `vee(X(ξ)⁻¹ · dX/dξ_k)`, using only `exp()` and `matrix()`.
    fn fd_right_jacobian(xi: &Sim3Tangent) -> Matrix7<f64> {
        let x_inv_mat = xi.exp(None).inverse(None).matrix();
        let mut jac = Matrix7::zeros();
        for k in 0..7 {
            let plus = perturbed(xi, k, FD_STEP).exp(None).matrix();
            let minus = perturbed(xi, k, -FD_STEP).exp(None).matrix();
            let d_dxi_k = (plus - minus) / (2.0 * FD_STEP);
            jac.set_column(k, &vee(&(x_inv_mat * d_dxi_k)));
        }
        jac
    }

    #[test]
    fn sim3_right_jacobian_matches_matrix_exp_central_difference() {
        let cases = [
            (
                Vector3::new(0.7, -0.2, 0.4),
                Vector3::new(0.4, -0.3, 0.25),
                0.3,
            ),
            (
                Vector3::new(0.1, 0.2, -0.3),
                Vector3::new(2.8, 0.1, -0.2),
                0.5,
            ), // near pi
            (Vector3::new(-0.5, 0.3, 0.2), Vector3::zeros(), 0.4), // theta = 0
            (
                Vector3::new(0.3, -0.1, 0.2),
                Vector3::new(0.2, 0.1, -0.1),
                0.0,
            ), // sigma = 0
            (
                Vector3::new(1e-8, -2e-8, 3e-8),
                Vector3::new(1e-8, 2e-8, -1e-8),
                1e-8,
            ), // joint small-angle/small-scale limit
        ];
        for (rho, theta, sigma) in cases {
            let xi = Sim3Tangent::new(rho, theta, sigma);
            let analytic = xi.right_jacobian();
            let numeric = fd_right_jacobian(&xi);
            let err = (analytic - numeric).norm();
            assert!(
                err < FD_TOL,
                "right_jacobian mismatch at rho={rho:?} theta={theta:?} sigma={sigma}: error={err}"
            );
        }
    }

    #[test]
    fn sim3_left_jacobian_matches_matrix_exp_central_difference() {
        let xi = Sim3Tangent::new(
            Vector3::new(0.6, -0.3, 0.2),
            Vector3::new(-0.4, 0.5, 0.1),
            0.35,
        );
        let x_inv_mat = xi.exp(None).inverse(None).matrix();

        let mut numeric = Matrix7::zeros();
        for k in 0..7 {
            let plus = perturbed(&xi, k, FD_STEP).exp(None).matrix();
            let minus = perturbed(&xi, k, -FD_STEP).exp(None).matrix();
            let d_dxi_k = (plus - minus) / (2.0 * FD_STEP);
            // Left Jacobian definition: dX/dξ_k = (Jl(ξ)e_k)^ · X(ξ).
            numeric.set_column(k, &vee(&(d_dxi_k * x_inv_mat)));
        }

        let analytic = xi.left_jacobian();
        let err = (analytic - numeric).norm();
        assert!(err < FD_TOL, "left_jacobian mismatch: error={err}");
    }

    #[test]
    fn sim3_act_jacobian_matches_finite_difference_at_nonunit_scale() {
        let sim3 = Sim3::new(
            Vector3::new(0.3, -0.2, 0.5),
            UnitQuaternion::from_euler_angles(0.2, -0.4, 0.3),
            1.7,
        );
        let p = Vector3::new(0.9, -1.1, 0.4);

        let mut jac_self = Matrix7::zeros();
        let mut jac_point = Matrix3::zeros();
        sim3.act(&p, Some(&mut jac_self), Some(&mut jac_point));

        for k in 0..3 {
            let mut p_plus = p;
            p_plus[k] += FD_STEP;
            let mut p_minus = p;
            p_minus[k] -= FD_STEP;
            let numeric =
                (sim3.act(&p_plus, None, None) - sim3.act(&p_minus, None, None)) / (2.0 * FD_STEP);
            let analytic = jac_point.column(k).into_owned();
            let err = (analytic - numeric).norm();
            assert!(err < FD_TOL, "act jacobian_vector col {k} mismatch: {err}");
        }

        for k in 0..7 {
            let mut e = Vector7::zeros();
            e[k] = FD_STEP;
            let plus = sim3.right_plus(&Sim3Tangent { data: e }, None, None);
            e[k] = -FD_STEP;
            let minus = sim3.right_plus(&Sim3Tangent { data: e }, None, None);
            let numeric = (plus.act(&p, None, None) - minus.act(&p, None, None)) / (2.0 * FD_STEP);
            let analytic = Vector3::new(jac_self[(0, k)], jac_self[(1, k)], jac_self[(2, k)]);
            let err = (analytic - numeric).norm();
            assert!(err < FD_TOL, "act jacobian_self col {k} mismatch: {err}");
        }
    }

    #[test]
    fn sim3_adjoint_matches_conjugation_finite_difference() {
        let cases = [
            (
                Vector3::new(0.8, -0.4, 0.6),
                Vector3::new(0.3, -0.2, 0.5),
                1.6,
            ), // general
            (Vector3::new(1.2, 0.5, -0.7), Vector3::zeros(), 1.0), // pure translation
            (Vector3::zeros(), Vector3::zeros(), 2.3),             // pure scale
        ];
        for (t, r, s) in cases {
            let x = Sim3::new(t, UnitQuaternion::from_euler_angles(r.x, r.y, r.z), s);
            let analytic = x.adjoint();
            let x_mat = x.matrix();
            let x_inv_mat = x.inverse(None).matrix();

            let mut numeric = Matrix7::zeros();
            for k in 0..7 {
                let mut e_plus = Vector7::zeros();
                e_plus[k] = FD_STEP;
                let g_plus = (Sim3Tangent { data: e_plus }).exp(None).matrix();
                let mut e_minus = Vector7::zeros();
                e_minus[k] = -FD_STEP;
                let g_minus = (Sim3Tangent { data: e_minus }).exp(None).matrix();

                let conj_plus = x_mat * g_plus * x_inv_mat;
                let conj_minus = x_mat * g_minus * x_inv_mat;
                let d = (conj_plus - conj_minus) / (2.0 * FD_STEP);
                numeric.set_column(k, &vee(&d));
            }

            let err = (analytic - numeric).norm();
            assert!(
                err < FD_TOL,
                "adjoint mismatch at t={t:?} r={r:?} s={s}: error={err}"
            );
        }
    }

    #[test]
    fn sim3_right_jacobian_continuous_near_zero_and_joint_limit() {
        // Sweep theta and sigma norms down toward zero (independently and
        // jointly) and assert the analytic Jacobian stays close to the
        // matrix-exp finite-difference oracle throughout — no discontinuity
        // at the small-angle boundary.
        for scale in [1e-1, 1e-3, 1e-5, 1e-7, 1e-9] {
            let xi = Sim3Tangent::new(
                Vector3::new(0.3, -0.2, 0.1) * scale,
                Vector3::new(0.2, 0.1, -0.3) * scale,
                0.15 * scale,
            );
            let analytic = xi.right_jacobian();
            let numeric = fd_right_jacobian(&xi);
            let err = (analytic - numeric).norm();
            assert!(err < FD_TOL, "discontinuity at scale={scale}: error={err}");
        }
    }
}