Struct BetweenFactorSE3 

pub struct BetweenFactorSE3 {
    pub relative_pose: SE3,
}

Between factor for SE(3) poses (3D pose graph constraint).

Represents a relative pose measurement between two SE(3) poses in 3D. This is the fundamental building block for 3D SLAM, structure from motion, and bundle adjustment.

Mathematical Formulation

Given two poses T_i and T_j in SE(3), and a measurement T_ij, the residual is:

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

where:

• ⊕ is SE(3) composition
• log is the SE(3) logarithm map (converts to tangent space se(3))
• The residual is a 6D vector [v_x, v_y, v_z, ω_x, ω_y, ω_z] in the tangent space

Tangent Space

The 6D tangent space se(3) consists of:

• Translation: [v_x, v_y, v_z] - Linear velocity
• Rotation: [ω_x, ω_y, ω_z] - Angular velocity (axis-angle)

Jacobian Computation

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

J = ∂r/∂[T_i, T_j]

This is a 6×12 matrix (6 residual dimensions, 12 DOF from two SE(3) poses).

Use Cases

• 3D SLAM: Visual odometry, loop closure constraints
• Pose graph optimization: Relative pose constraints
• Bundle adjustment: Camera pose relationships
• Multi-view geometry: Relative camera poses

Example

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

// Measurement: relative transformation between two poses
let relative_pose = SE3::from_translation_quaternion(
    Vector3::new(1.0, 0.0, 0.0),        // 1m forward
    Quaternion::new(1.0, 0.0, 0.0, 0.0) // No rotation
);
let between = BetweenFactorSE3::new(relative_pose);

// Current pose estimates (in [tx, ty, tz, qw, qx, qy, qz] format)
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]);

// Compute residual
let (residual, jacobian) = between.linearize(&[pose_i, pose_j], true);
println!("Residual dimension: {}", residual.len());  // 6
if let Some(jac) = jacobian {
    println!("Jacobian shape: {} x {}", jac.nrows(), jac.ncols());  // 6x12
}

Fields

relative_pose: SE3

The measured relative pose transformation between the two connected poses

Implementations

impl BetweenFactorSE3

pub fn new(relative_pose: SE3) -> Self

Create a new SE(3) between factor from a relative pose measurement.

Arguments

• relative_pose - The measured relative SE(3) transformation

Returns

A new BetweenFactorSE3 instance

Example

use apex_solver::core::factors::BetweenFactorSE3;
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)
);

let factor = BetweenFactorSE3::new(relative);

Trait Implementations

impl Clone for BetweenFactorSE3

fn clone(&self) -> BetweenFactorSE3

Returns a duplicate of the value.

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

Performs copy-assignment from source.

impl Debug for BetweenFactorSE3

fn fmt(&self, f: &mut Formatter<'_>) -> Result

Formats the value using the given formatter.

impl Factor for BetweenFactorSE3

fn linearize( &self, params: &[DVector<f64>], compute_jacobian: bool, ) -> (DVector<f64>, Option<DMatrix<f64>>)

Compute residual and Jacobian for SE(3) between factor.

Arguments

• params - Two SE(3) poses in format: [tx, ty, tz, qw, qx, qy, qz]

Returns

• Residual: 6×1 vector [v_x, v_y, v_z, ω_x, ω_y, ω_z] in tangent space
• Jacobian: 6×12 matrix [∂r/∂pose_i, ∂r/∂pose_j]

Algorithm

Uses analytical Jacobians computed via chain rule through:

1. Inverse: T_i⁻¹
2. Composition: T_i⁻¹ ⊕ T_j
3. Composition: (T_i⁻¹ ⊕ T_j) ⊕ T_ij
4. Logarithm: log(...)

fn get_dimension(&self) -> usize

Get the dimension of the residual vector.