apex_solver/core/

variable.rs

//! Variables for optimization on manifolds.
//!
//! This module provides the `Variable` struct, which represents optimization variables that
//! live on manifolds (Lie groups like SE2, SE3, SO2, SO3, or Euclidean spaces Rn). Variables
//! support manifold operations (plus/minus in tangent space), constraints (fixed indices,
//! bounds), and covariance estimation.
//!
//! # Key Concepts
//!
//! ## Manifolds vs. Vector Spaces
//!
//! Unlike standard optimization that operates in Euclidean space (Rⁿ), many robotics problems
//! involve variables on manifolds:
//! - **Poses**: SE(2) or SE(3) - rigid body transformations
//! - **Rotations**: SO(2) or SO(3) - rotation matrices/quaternions
//! - **Landmarks**: R³ - 3D points in Euclidean space
//!
//! Manifolds require special handling:
//! - Updates happen in the tangent space (local linearization)
//! - Plus operation (⊞): manifold × tangent → manifold
//! - Minus operation (⊟): manifold × manifold → tangent
//!
//! ## Tangent Space Updates
//!
//! During optimization, updates are computed as tangent vectors and applied via the plus
//! operation:
//!
//! ```text
//! x_new = x_old ⊞ δx
//! ```
//!
//! where `δx` is a tangent vector (e.g., 6D for SE(3), 3D for SE(2)).
//!
//! ## Constraints
//!
//! Variables support two types of constraints:
//! - **Fixed indices**: Specific DOF held constant during optimization
//! - **Bounds**: Box constraints (min/max) on tangent space components
//!
//! ## Covariance
//!
//! After optimization, the `Variable` can store a covariance matrix representing uncertainty
//! in the tangent space. For SE(3), this is a 6×6 matrix; for SE(2), a 3×3 matrix.
//!
//! # Example: SE(2) Variable
//!
//! ```
//! use apex_solver::core::variable::Variable;
//! use apex_solver::manifold::se2::{SE2, SE2Tangent};
//! use apex_solver::manifold::Tangent;
//! use nalgebra::DVector;
//!
//! // Create a 2D pose variable
//! let initial_pose = SE2::from_xy_angle(1.0, 2.0, 0.5);
//! let mut var = Variable::new(initial_pose);
//!
//! // Apply a tangent space update: [dx, dy, dtheta]
//! let delta = SE2Tangent::from_slice(&[0.1, 0.2, 0.05]);
//! let updated_pose = var.plus(&delta);
//! var.set_value(updated_pose);
//! ```
//!
//! # Example: Variable with Constraints
//!
//! ```
//! use apex_solver::core::variable::Variable;
//! use apex_solver::manifold::rn::Rn;
//! use nalgebra::DVector;
//!
//! // Create a 3D point variable
//! let mut landmark = Variable::new(Rn::new(DVector::from_vec(vec![0.0, 0.0, 0.0])));
//!
//! // Fix the z-coordinate (index 2)
//! landmark.fixed_indices.insert(2);
//!
//! // Constrain x to [-10, 10]
//! landmark.bounds.insert(0, (-10.0, 10.0));
//!
//! // Apply update (will respect constraints)
//! let update = DVector::from_vec(vec![15.0, 5.0, 100.0]); // Large update
//! landmark.update_variable(update);
//!
//! let result = landmark.to_vector();
//! assert_eq!(result[0], 10.0);   // Clamped to upper bound
//! assert_eq!(result[1], 5.0);    // Unconstrained
//! assert_eq!(result[2], 0.0);    // Fixed at original value
//! ```

use std::any::Any;
use std::collections::{HashMap, HashSet};

use apex_manifolds::{LieGroup, Tangent};
use faer::Mat;
use nalgebra::DVector;

/// Generic Variable struct that uses static dispatch with any manifold type.
///
/// This struct represents optimization variables that live on manifolds and provides
/// type-safe operations for updating variables with tangent space perturbations.
///
/// # Type Parameters
/// * `M` - The manifold type that implements the LieGroup trait
///
/// # Examples
/// ```
/// use apex_solver::core::variable::Variable;
/// use apex_solver::manifold::se2::SE2;
/// use apex_solver::manifold::rn::Rn;
///
/// // Create a Variable for SE2 manifold
/// let se2_value = SE2::from_xy_angle(1.0, 2.0, 0.5);
/// let se2_var = Variable::new(se2_value);
///
/// // Create a Variable for Euclidean space
/// let rn_value = Rn::from_vec(vec![1.0, 2.0, 3.0]);
/// let rn_var = Variable::new(rn_value);
/// ```
#[derive(Clone, Debug)]
pub struct Variable<M: LieGroup> {
    /// The manifold value
    pub value: M,
    /// Indices that should remain fixed during optimization
    pub fixed_indices: HashSet<usize>,
    /// Bounds constraints on the tangent space representation
    pub bounds: HashMap<usize, (f64, f64)>,
    /// Covariance matrix in the tangent space (uncertainty estimation)
    ///
    /// This is `None` if covariance has not been computed.
    /// When present, it's a square matrix of size `tangent_dim x tangent_dim`
    /// representing the uncertainty in the optimized variable's tangent space.
    ///
    /// For example, for SE3 this would be a 6×6 matrix representing uncertainty
    /// in [translation_x, translation_y, translation_z, rotation_x, rotation_y, rotation_z].
    pub covariance: Option<faer::Mat<f64>>,
}

impl<M> Variable<M>
where
    M: LieGroup + Clone + 'static,
    M::TangentVector: Tangent<M>,
{
    /// Create a new Variable from a manifold value.
    ///
    /// # Arguments
    /// * `value` - The initial manifold value
    ///
    /// # Examples
    /// ```
    /// use apex_solver::core::variable::Variable;
    /// use apex_solver::manifold::se2::SE2;
    ///
    /// let se2_value = SE2::from_xy_angle(1.0, 2.0, 0.5);
    /// let variable = Variable::new(se2_value);
    /// ```
    pub fn new(value: M) -> Self {
        Variable {
            value,
            fixed_indices: HashSet::new(),
            bounds: HashMap::new(),
            covariance: None,
        }
    }

    /// Set the manifold value.
    ///
    /// # Arguments
    /// * `value` - The new manifold value
    pub fn set_value(&mut self, value: M) {
        self.value = value;
    }

    /// Get the degrees of freedom (tangent space dimension) of the variable.
    ///
    /// This returns the dimension of the tangent space, which is the number of
    /// parameters that can be optimized for this manifold type.
    ///
    /// # Returns
    /// The tangent space dimension (degrees of freedom)
    pub fn get_size(&self) -> usize {
        self.value.tangent_dim()
    }

    /// Plus operation: apply tangent space perturbation to the manifold value.
    ///
    /// This method takes a tangent vector and returns a new manifold value by applying
    /// the manifold's plus operation (typically the exponential map).
    ///
    /// # Arguments
    /// * `tangent` - The tangent vector to apply as a perturbation
    ///
    /// # Returns
    /// A new manifold value after applying the tangent perturbation
    ///
    /// # Examples
    /// ```
    /// use apex_solver::core::variable::Variable;
    /// use apex_solver::manifold::se2::{SE2, SE2Tangent};
    /// use apex_solver::manifold::Tangent;
    /// use nalgebra as na;
    ///
    /// let se2_value = SE2::from_xy_angle(1.0, 2.0, 0.0);
    /// let variable = Variable::new(se2_value);
    ///
    /// // Create a tangent vector: [dx, dy, dtheta]
    /// let tangent = SE2Tangent::from_slice(&[0.1, 0.1, 0.1]);
    /// let new_value = variable.plus(&tangent);
    /// ```
    pub fn plus(&self, tangent: &M::TangentVector) -> M {
        self.value.plus(tangent, None, None)
    }

    /// Minus operation: compute tangent space difference between two manifold values.
    ///
    /// This method computes the tangent vector that would transform this variable's
    /// value to the other variable's value using the manifold's minus operation
    /// (typically the logarithmic map).
    ///
    /// # Arguments
    /// * `other` - The other variable to compute the difference to
    ///
    /// # Returns
    /// A tangent vector representing the difference in tangent space
    ///
    /// # Examples
    /// ```
    /// use apex_solver::core::variable::Variable;
    /// use apex_solver::manifold::se2::SE2;
    ///
    /// let se2_1 = SE2::from_xy_angle(2.0, 3.0, 0.5);
    /// let se2_2 = SE2::from_xy_angle(1.0, 2.0, 0.0);
    /// let var1 = Variable::new(se2_1);
    /// let var2 = Variable::new(se2_2);
    ///
    /// let difference = var1.minus(&var2);
    /// ```
    pub fn minus(&self, other: &Self) -> M::TangentVector {
        self.value.minus(&other.value, None, None)
    }

    /// Get the covariance matrix for this variable (if computed).
    ///
    /// Returns `None` if covariance has not been computed.
    ///
    /// # Returns
    /// Reference to the covariance matrix in tangent space
    pub fn get_covariance(&self) -> Option<&Mat<f64>> {
        self.covariance.as_ref()
    }

    /// Set the covariance matrix for this variable.
    ///
    /// The covariance matrix should be square with dimension equal to
    /// the tangent space dimension of this variable.
    ///
    /// # Arguments
    /// * `cov` - Covariance matrix in tangent space
    pub fn set_covariance(&mut self, cov: Mat<f64>) {
        self.covariance = Some(cov);
    }

    /// Clear the covariance matrix.
    pub fn clear_covariance(&mut self) {
        self.covariance = None;
    }
}

// Extension implementation for Rn manifold (special case since it's Euclidean)
use apex_manifolds::rn::Rn;

impl Variable<Rn> {
    /// Convert the Rn variable to a vector representation.
    pub fn to_vector(&self) -> DVector<f64> {
        self.value.data().clone()
    }

    /// Create an Rn variable from a vector representation.
    pub fn from_vector(values: DVector<f64>) -> Self {
        Self::new(Rn::new(values))
    }

    /// Update the Rn variable with bounds and fixed constraints.
    pub fn update_variable(&mut self, mut tangent_delta: DVector<f64>) {
        // bound
        for (&idx, &(lower, upper)) in &self.bounds {
            tangent_delta[idx] = tangent_delta[idx].max(lower).min(upper);
        }

        // fix
        for &index_to_fix in &self.fixed_indices {
            tangent_delta[index_to_fix] = self.value.data()[index_to_fix];
        }

        self.value = Rn::new(tangent_delta);
    }
}

// ============================================================================
// ManifoldVariable — object-safe trait for heterogeneous variable collections
// ============================================================================

/// Object-safe trait covering all manifold variable types.
///
/// Replaces `VariableEnum` as the common type for `HashMap<String, Box<dyn ManifoldVariable>>`.
/// One blanket impl covers all 8 manifold types; no per-manifold match arms needed.
///
/// # Hot-path contract
/// `as_param_slice`, `dof`, and `apply_tangent_step` must never heap-allocate.
pub trait ManifoldVariable: Send + Sync + 'static {
    // ── Hot path (zero allocation) ──────────────────────────────────────
    /// Zero-copy borrow of the parameter storage backing this variable.
    fn as_param_slice(&self) -> &[f64];
    /// Tangent space dimension (degrees of freedom).
    fn dof(&self) -> usize;
    /// Apply a tangent-space step in-place: x ← x ⊞ δx.
    /// Fixed indices are zeroed before the retraction.
    fn apply_tangent_step(&mut self, step: &[f64]);

    // ── Build-time configuration ─────────────────────────────────────────
    fn set_fixed_indices(&mut self, indices: HashSet<usize>);
    fn get_fixed_indices(&self) -> &HashSet<usize>;
    fn set_bounds(&mut self, bounds: HashMap<usize, (f64, f64)>);
    fn get_bounds(&self) -> &HashMap<usize, (f64, f64)>;
    /// Static name of the underlying manifold type ("SE3", "SO3", …).
    fn manifold_type_name(&self) -> &'static str;

    // ── Fixed/free column layout (ISSUE-0003) ───────────────────────────
    // Fixed tangent coordinates must never occupy a column in the linear
    // solve — solving the full system and zeroing the fixed entries of the
    // step afterward is not equivalent, since free and fixed columns
    // generally couple through JᵀJ. These map a variable's local tangent
    // indices to a compacted "free-column" space instead.
    /// Number of un-fixed tangent coordinates.
    fn free_dof(&self) -> usize {
        self.dof() - self.get_fixed_indices().len()
    }

    /// Map a local tangent index (`0..dof()`) to its offset within this
    /// variable's free-column block, or `None` if that index is fixed.
    fn local_free_offset(&self, local: usize) -> Option<usize> {
        let fixed = self.get_fixed_indices();
        if fixed.is_empty() {
            // Fast path: identity mapping, no HashSet touch.
            return Some(local);
        }
        if fixed.contains(&local) {
            return None;
        }
        Some(local - fixed.iter().filter(|&&i| i < local).count())
    }

    /// Expand a free-space step (length `free_dof()`) into a `dof()`-length
    /// buffer, writing `0.0` at every fixed local index in ascending order.
    fn expand_free_step(&self, free_step: &[f64], out: &mut [f64]) {
        let fixed = self.get_fixed_indices();
        let dof = self.dof();
        if fixed.is_empty() {
            out[..dof].copy_from_slice(&free_step[..dof]);
            return;
        }
        let mut free_idx = 0usize;
        for (local, slot) in out.iter_mut().enumerate().take(dof) {
            *slot = if fixed.contains(&local) {
                0.0
            } else {
                let v = free_step.get(free_idx).copied().unwrap_or(0.0);
                free_idx += 1;
                v
            };
        }
    }

    // ── Covariance ───────────────────────────────────────────────────────
    fn set_covariance(&mut self, cov: faer::Mat<f64>);
    fn covariance(&self) -> Option<&faer::Mat<f64>>;
    fn clear_covariance(&mut self);

    // ── I/O (NOT hot path — allocates) ───────────────────────────────────
    fn to_dvector(&self) -> DVector<f64>;

    // ── Downcast support ─────────────────────────────────────────────────
    fn as_any(&self) -> &dyn Any;
    fn as_any_mut(&mut self) -> &mut dyn Any;
    fn clone_box(&self) -> Box<dyn ManifoldVariable>;
}

impl Clone for Box<dyn ManifoldVariable> {
    fn clone(&self) -> Self {
        self.clone_box()
    }
}

impl<M> ManifoldVariable for Variable<M>
where
    M: LieGroup + Clone + Send + Sync + 'static,
    M::TangentVector: Tangent<M> + Send + Sync + 'static,
{
    fn as_param_slice(&self) -> &[f64] {
        self.value.as_param_slice()
    }

    fn dof(&self) -> usize {
        self.value.tangent_dim()
    }

    fn apply_tangent_step(&mut self, step: &[f64]) {
        let dof = self.dof();
        // Stack buffer for fixed-size manifolds (all current types have DOF ≤ 10).
        // Fall back to heap Vec only for large dynamic Rn variables.
        if dof <= 16 {
            let mut buf = [0f64; 16];
            buf[..dof].copy_from_slice(&step[..dof]);
            for &idx in &self.fixed_indices {
                if idx < dof {
                    buf[idx] = 0.0;
                }
            }
            let tangent = M::TangentVector::from_slice(&buf[..dof]);
            self.value = self.value.plus(&tangent, None, None);
        } else {
            let mut v = step[..dof].to_vec();
            for &idx in &self.fixed_indices {
                if idx < dof {
                    v[idx] = 0.0;
                }
            }
            let tangent = M::TangentVector::from_slice(&v);
            self.value = self.value.plus(&tangent, None, None);
        }
    }

    fn manifold_type_name(&self) -> &'static str {
        M::NAME
    }

    fn set_fixed_indices(&mut self, indices: HashSet<usize>) {
        self.fixed_indices = indices;
    }

    fn get_fixed_indices(&self) -> &HashSet<usize> {
        &self.fixed_indices
    }

    fn set_bounds(&mut self, bounds: HashMap<usize, (f64, f64)>) {
        self.bounds = bounds;
    }

    fn get_bounds(&self) -> &HashMap<usize, (f64, f64)> {
        &self.bounds
    }

    fn set_covariance(&mut self, cov: faer::Mat<f64>) {
        self.covariance = Some(cov);
    }

    fn covariance(&self) -> Option<&faer::Mat<f64>> {
        self.covariance.as_ref()
    }

    fn clear_covariance(&mut self) {
        self.covariance = None;
    }

    fn to_dvector(&self) -> DVector<f64> {
        DVector::from_column_slice(self.value.as_param_slice())
    }

    fn as_any(&self) -> &dyn Any {
        self
    }

    fn as_any_mut(&mut self) -> &mut dyn Any {
        self
    }

    fn clone_box(&self) -> Box<dyn ManifoldVariable> {
        Box::new(self.clone())
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    use apex_manifolds::{rn::Rn, se2::SE2, se3::SE3, so2::SO2, so3::SO3};
    use nalgebra::{DVector, Quaternion, Vector3};
    use std;

    #[test]
    fn test_variable_creation_rn() {
        let vec_data = DVector::from_vec(vec![1.0, 2.0, 3.0, 4.0, 5.0]);
        let rn_value = Rn::new(vec_data);
        let variable = Variable::new(rn_value);

        // Use get_size for Rn manifold (returns dynamic size)
        assert_eq!(variable.get_size(), 5);
        assert!(variable.fixed_indices.is_empty());
        assert!(variable.bounds.is_empty());
    }

    #[test]
    fn test_variable_creation_se2() {
        let se2 = SE2::from_xy_angle(1.0, 2.0, 0.5);
        let variable = Variable::new(se2);

        assert_eq!(variable.get_size(), SE2::DOF);
        assert!(variable.fixed_indices.is_empty());
        assert!(variable.bounds.is_empty());
    }

    #[test]
    fn test_variable_creation_se3() {
        let se3 = SE3::from_translation_quaternion(
            Vector3::new(1.0, 2.0, 3.0),
            Quaternion::new(1.0, 0.0, 0.0, 0.0),
        );
        let variable = Variable::new(se3);

        assert_eq!(variable.get_size(), SE3::DOF);
        assert!(variable.fixed_indices.is_empty());
        assert!(variable.bounds.is_empty());
    }

    #[test]
    fn test_variable_creation_so2() {
        let so2 = SO2::from_angle(0.5);
        let variable = Variable::new(so2);

        assert_eq!(variable.get_size(), SO2::DOF);
        assert!(variable.fixed_indices.is_empty());
        assert!(variable.bounds.is_empty());
    }

    #[test]
    fn test_variable_creation_so3() {
        let so3 = SO3::from_euler_angles(0.1, 0.2, 0.3);
        let variable = Variable::new(so3);

        assert_eq!(variable.get_size(), SO3::DOF);
        assert!(variable.fixed_indices.is_empty());
        assert!(variable.bounds.is_empty());
    }

    #[test]
    fn test_variable_set_value() {
        let initial_vec = DVector::from_vec(vec![1.0, 2.0, 3.0]);
        let mut variable = Variable::new(Rn::new(initial_vec));

        let new_vec = DVector::from_vec(vec![4.0, 5.0, 6.0, 7.0]);
        variable.set_value(Rn::new(new_vec));
        assert_eq!(variable.get_size(), 4);

        let se2_initial = SE2::from_xy_angle(0.0, 0.0, 0.0);
        let mut se2_variable = Variable::new(se2_initial);

        let se2_new = SE2::from_xy_angle(1.0, 2.0, std::f64::consts::PI / 4.0);
        se2_variable.set_value(se2_new);
        assert_eq!(se2_variable.get_size(), SE2::DOF);
    }

    #[test]
    fn test_variable_plus_minus_operations() {
        // Test SE2 manifold plus/minus operations
        let se2_1 = SE2::from_xy_angle(2.0, 3.0, std::f64::consts::PI / 2.0);
        let se2_2 = SE2::from_xy_angle(1.0, 1.0, std::f64::consts::PI / 4.0);
        let var1 = Variable::new(se2_1);
        let var2 = Variable::new(se2_2);

        let diff_tangent = var1.minus(&var2);
        let var2_updated = var2.plus(&diff_tangent);
        let final_diff = var1.minus(&Variable::new(var2_updated));

        assert!(DVector::from_column_slice(final_diff.as_slice()).norm() < 1e-10);
    }

    #[test]
    fn test_variable_rn_plus_minus_operations() {
        // Test Rn manifold plus/minus operations
        let rn_1 = Rn::new(DVector::from_vec(vec![1.0, 2.0, 3.0]));
        let rn_2 = Rn::new(DVector::from_vec(vec![4.0, 5.0, 6.0]));
        let var1 = Variable::new(rn_1);
        let var2 = Variable::new(rn_2);

        // Test minus operation
        let diff_tangent = var1.minus(&var2);
        assert_eq!(
            diff_tangent.to_vector(),
            DVector::from_vec(vec![-3.0, -3.0, -3.0])
        );

        // Test plus operation
        let var2_updated = var2.plus(&diff_tangent);
        assert_eq!(var2_updated.data(), &DVector::from_vec(vec![1.0, 2.0, 3.0]));

        // Test roundtrip consistency
        let final_diff = var1.minus(&Variable::new(var2_updated));
        assert!(final_diff.to_vector().norm() < 1e-10);
    }

    #[test]
    fn test_variable_update_with_bounds() {
        let vec_data = DVector::from_vec(vec![1.0, 2.0, 3.0, 4.0, 5.0, 6.0]);
        let mut variable = Variable::new(Rn::new(vec_data));

        variable.bounds.insert(0, (-1.0, 1.0));
        variable.bounds.insert(2, (0.0, 5.0));

        let new_values = DVector::from_vec(vec![-5.0, 10.0, -3.0, 20.0, 30.0, 40.0]);
        variable.update_variable(new_values);

        let result_vec = variable.to_vector();
        assert!(result_vec.len() == 6);
    }

    #[test]
    fn test_variable_update_with_fixed_indices() {
        let vec_data = DVector::from_vec(vec![1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0]);
        let mut variable = Variable::new(Rn::new(vec_data.clone()));

        variable.fixed_indices.insert(1);
        variable.fixed_indices.insert(4);

        let delta_values = DVector::from_vec(vec![9.0, 18.0, 27.0, 36.0, 45.0, 54.0, 63.0, 72.0]);
        variable.update_variable(delta_values);

        let result_vec = variable.to_vector();
        assert_eq!(result_vec[1], 2.0);
        assert_eq!(result_vec[4], 5.0);
        assert!(result_vec.len() == 8);
    }

    #[test]
    fn test_variable_combined_bounds_and_fixed() {
        let vec_data = DVector::from_vec(vec![1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0]);
        let mut variable = Variable::new(Rn::new(vec_data.clone()));

        variable.bounds.insert(0, (-2.0, 2.0));
        variable.bounds.insert(3, (-1.0, 1.0));
        variable.fixed_indices.insert(1);
        variable.fixed_indices.insert(5);

        let delta_values = DVector::from_vec(vec![-5.0, 100.0, 30.0, 10.0, 50.0, 600.0, 70.0]);
        variable.update_variable(delta_values);

        let result = variable.to_vector();
        assert_eq!(result[1], 2.0);
        assert_eq!(result[5], 6.0);
        assert!(result.len() == 7);
    }

    #[test]
    fn test_variable_type_safety() {
        let se2_var = Variable::new(SE2::from_xy_angle(1.0, 2.0, 0.5));
        let se3_var = Variable::new(SE3::from_translation_quaternion(
            Vector3::new(1.0, 2.0, 3.0),
            Quaternion::new(1.0, 0.0, 0.0, 0.0),
        ));
        let so2_var = Variable::new(SO2::from_angle(0.5));
        let so3_var = Variable::new(SO3::from_euler_angles(0.1, 0.2, 0.3));
        let rn_var = Variable::new(Rn::new(DVector::from_vec(vec![1.0, 2.0, 3.0])));

        assert_eq!(se2_var.get_size(), SE2::DOF);
        assert_eq!(se3_var.get_size(), SE3::DOF);
        assert_eq!(so2_var.get_size(), SO2::DOF);
        assert_eq!(so3_var.get_size(), SO3::DOF);
        assert_eq!(rn_var.get_size(), 3);
    }

    #[test]
    fn test_variable_vector_conversion_roundtrip() {
        let original_data = DVector::from_vec(vec![1.0, 2.0, 3.0, 4.0, 5.0]);
        let rn_var = Variable::new(Rn::new(original_data.clone()));
        let vec_repr = rn_var.to_vector();
        assert_eq!(vec_repr, original_data);

        let reconstructed_var = Variable::<Rn>::from_vector(vec_repr);
        assert_eq!(reconstructed_var.to_vector(), original_data);
    }

    #[test]
    fn test_variable_manifold_operations_consistency() {
        // Test Rn manifold operations (has vector conversion methods)
        let rn_initial = Rn::new(DVector::from_vec(vec![1.0, 2.0, 3.0]));
        let mut rn_var = Variable::new(rn_initial);
        let rn_new_values = DVector::from_vec(vec![2.0, 3.0, 4.0]);
        rn_var.update_variable(rn_new_values);

        let rn_result = rn_var.to_vector();
        assert_eq!(rn_result, DVector::from_vec(vec![2.0, 3.0, 4.0]));

        // Test SE2 manifold plus/minus operations (core functionality)
        let se2_1 = SE2::from_xy_angle(2.0, 3.0, std::f64::consts::PI / 2.0);
        let se2_2 = SE2::from_xy_angle(1.0, 1.0, std::f64::consts::PI / 4.0);
        let var1 = Variable::new(se2_1);
        let var2 = Variable::new(se2_2);

        let diff_tangent = var1.minus(&var2);
        let var2_updated = var2.plus(&diff_tangent);
        let final_diff = var1.minus(&Variable::new(var2_updated));

        // The final difference should be small (close to identity in tangent space)
        assert!(DVector::from_column_slice(final_diff.as_slice()).norm() < 1e-10);
    }

    #[test]
    fn test_variable_constraints_interaction() {
        let rn_data = DVector::from_vec(vec![0.0, 0.0, 0.0, 0.0, 0.0]);
        let mut rn_var = Variable::new(Rn::new(rn_data));

        rn_var.bounds.insert(0, (-1.0, 1.0));
        rn_var.bounds.insert(2, (-10.0, 10.0));
        rn_var.fixed_indices.insert(1);
        rn_var.fixed_indices.insert(4);

        let large_delta = DVector::from_vec(vec![5.0, 100.0, 15.0, 20.0, 200.0]);
        rn_var.update_variable(large_delta);

        let result = rn_var.to_vector();

        assert_eq!(result[0], 1.0);
        assert_eq!(result[1], 0.0);
        assert_eq!(result[2], 10.0);
        assert_eq!(result[3], 20.0);
        assert_eq!(result[4], 0.0);
    }
}