Struct BetweenFactor 

pub struct BetweenFactor<T>
where
    T: LieGroup + Clone + Send + Sync,
{
    pub relative_pose: T,
}

Generic between factor for Lie group pose constraints.

Represents a relative pose measurement between two poses of any Lie group manifold type. This is a generic implementation that works with SE(2), SE(3), SO(2), SO(3), and Rⁿ using static dispatch for zero runtime overhead.

Type Parameter

• T - The Lie group manifold type (e.g., SE2, SE3, SO2, SO3, Rn)

Mathematical Formulation

Given two poses T_i and T_j in a Lie group, and a measurement T_ij, the residual is:

r = log(T_ij⁻¹ ⊕ T_i⁻¹ ⊕ T_j)

where:

• ⊕ is the Lie group composition operation
• log is the logarithm map (converts from manifold to tangent space)
• The residual dimensionality depends on the manifold’s degrees of freedom (DOF)

Residual Dimensions by Manifold Type

• SE(3): 6D residual [v_x, v_y, v_z, ω_x, ω_y, ω_z] - translation + rotation
• SE(2): 3D residual [dx, dy, dθ] - 2D translation + rotation
• SO(3): 3D residual [ω_x, ω_y, ω_z] - 3D rotation only
• SO(2): 1D residual [dθ] - 2D rotation only
• Rⁿ: nD residual - Euclidean space

Jacobian Computation

The Jacobian is computed analytically using the chain rule and Lie group derivatives:

J = ∂r/∂[T_i, T_j]

The Jacobian dimensions are DOF × (2 × DOF) where DOF is the manifold’s degrees of freedom:

• SE(3): 6×12 matrix
• SE(2): 3×6 matrix
• SO(3): 3×6 matrix
• SO(2): 1×2 matrix

Use Cases

• 3D SLAM: Visual odometry, loop closure constraints (SE3)
• 2D SLAM: Robot navigation, mapping (SE2)
• Pose graph optimization: Relative pose constraints (SE2, SE3)
• Orientation tracking: IMU fusion, attitude estimation (SO2, SO3)
• General manifold optimization: Custom manifolds (Rⁿ)

Examples

SE(3) - 3D Pose Graph

use apex_solver::factors::{Factor, BetweenFactor};
use apex_solver::manifold::se3::SE3;
use nalgebra::{Vector3, Quaternion, DVector};

let relative_pose = SE3::from_translation_quaternion(
    Vector3::new(1.0, 0.0, 0.0),
    Quaternion::new(1.0, 0.0, 0.0, 0.0),
);
let between = BetweenFactor::new(relative_pose);

let pose_i = DVector::from_vec(vec![0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]);
let pose_j = DVector::from_vec(vec![0.95, 0.05, 0.0, 1.0, 0.0, 0.0, 0.0]);

let mut residual = vec![0.0f64; between.residual_dim()];
let (rows, cols) = between.jacobian_shape();
let mut jac_buf = vec![0.0f64; rows * cols];
let jac_mut = faer::mat::MatMut::from_column_major_slice_mut(&mut jac_buf, rows, cols);
between.linearize(&[pose_i.as_slice(), pose_j.as_slice()], &mut residual, Some(jac_mut));

SE(2) - 2D Pose Graph

use apex_solver::factors::{Factor, BetweenFactor};
use apex_solver::manifold::se2::SE2;
use nalgebra::DVector;

let relative_pose = SE2::from_xy_angle(1.0, 0.0, 0.1);
let between = BetweenFactor::new(relative_pose);

let pose_i = DVector::from_vec(vec![0.0, 0.0, 0.0]);
let pose_j = DVector::from_vec(vec![0.95, 0.05, 0.12]);

let mut residual = vec![0.0f64; between.residual_dim()];
between.linearize(&[pose_i.as_slice(), pose_j.as_slice()], &mut residual, None);

Performance

This generic implementation uses static dispatch (monomorphization), meaning:

• Zero runtime overhead compared to type-specific implementations
• Compiler optimizes each instantiation (BetweenFactor<SE3>, BetweenFactor<SE2>, etc.)
• All type checking happens at compile time
• No dynamic dispatch or virtual function calls

Fields

relative_pose: T

The measured relative pose transformation between the two connected poses

Implementations

impl<T> BetweenFactor<T>

where T: LieGroup + Clone + Send + Sync,

pub fn new(relative_pose: T) -> Self

Create a new between factor from a relative pose measurement.

This is a generic constructor that works with any Lie group manifold type. The type parameter T is typically inferred from the relative_pose argument.

Arguments

• relative_pose - The measured relative transformation between two poses

Returns

A new BetweenFactor<T> instance

Examples

SE(3) Between Factor

use apex_solver::factors::BetweenFactor;
use apex_solver::manifold::se3::SE3;

// Create relative pose: move 2m in x, rotate 90° around z-axis
let relative = SE3::from_translation_euler(
    2.0, 0.0, 0.0,                      // translation (x, y, z)
    0.0, 0.0, std::f64::consts::FRAC_PI_2  // rotation (roll, pitch, yaw)
);

// Type is inferred as BetweenFactor<SE3>
let factor = BetweenFactor::new(relative);

SE(2) Between Factor

use apex_solver::factors::BetweenFactor;
use apex_solver::manifold::se2::SE2;

// Create relative 2D pose
let relative = SE2::from_xy_angle(1.0, 0.5, 0.1);

// Type is inferred as BetweenFactor<SE2>
let factor = BetweenFactor::new(relative);

Trait Implementations

impl<T> Clone for BetweenFactor<T>

where T: LieGroup + Clone + Send + Sync + Clone,

fn clone(&self) -> BetweenFactor<T>

Returns a duplicate of the value.

fn clone_from(&mut self, source: &Self)

Performs copy-assignment from source.

impl<T> Factor for BetweenFactor<T>

where T: LieGroup + Clone + Send + Sync,

fn linearize( &self, params: &[&[f64]], residual: &mut [f64], jacobian: Option<MatMut<'_, f64>>, )

Write residual and (optionally) Jacobian into pre-allocated buffers.

fn residual_dim(&self) -> usize

Number of residual rows (length of the residual buffer).

fn jacobian_shape(&self) -> (usize, usize)

(rows, cols) of the Jacobian — rows == residual_dim(), cols == sum of variable DOFs.

impl<T> PartialEq for BetweenFactor<T>

where T: LieGroup + Clone + Send + Sync + PartialEq,

fn eq(&self, other: &BetweenFactor<T>) -> bool

Equality operator ==.

fn ne(&self, other: &Rhs) -> bool

Inequality operator !=.

impl<T> StructuralPartialEq for BetweenFactor<T>

where T: LieGroup + Clone + Send + Sync + PartialEq,