Struct BetweenFactorSE2 

pub struct BetweenFactorSE2 {
    pub relative_pose: SE2,
}

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

Represents a relative pose measurement between two SE(2) poses in 2D. This is the fundamental building block for 2D SLAM, pose graph optimization, and trajectory estimation.

Mathematical Formulation

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

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

where:

• ⊕ is SE(2) composition
• log is the SE(2) logarithm map (converts to tangent space)
• The residual is a 3D vector [dx, dy, dtheta] in the tangent space

Jacobian Computation

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

J = ∂r/∂[T_i, T_j]

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

Use Cases

• 2D SLAM: Odometry measurements, loop closure constraints
• Pose graph optimization: Relative pose constraints
• 2D trajectory estimation

Example

use apex_solver::core::factors::{Factor, BetweenFactorSE2};
use nalgebra::DVector;

// Measurement: robot moved 1m forward and rotated 0.1 rad
let between = BetweenFactorSE2::new(1.0, 0.0, 0.1);

// Current pose estimates
let pose_i = DVector::from_vec(vec![0.0, 0.0, 0.0]);     // Origin
let pose_j = DVector::from_vec(vec![0.95, 0.05, 0.12]);  // Slightly off

// Compute residual (should be small if poses are consistent)
let (residual, jacobian) = between.linearize(&[pose_i, pose_j], true);
println!("Residual: {:?}", residual);  // Shows deviation from measurement

Fields

relative_pose: SE2

The measured relative pose transformation between the two connected poses

Implementations

impl BetweenFactorSE2

pub fn new(dx: f64, dy: f64, dtheta: f64) -> Self

Create a new SE(2) between factor from translation and rotation components.

Arguments

• dx - Relative translation in x direction (meters)
• dy - Relative translation in y direction (meters)
• dtheta - Relative rotation angle (radians)

Returns

A new BetweenFactorSE2 instance

Example

use apex_solver::core::factors::BetweenFactorSE2;

// Robot moved 2m forward, 0.5m left, rotated 0.1 rad counterclockwise
let factor = BetweenFactorSE2::new(2.0, 0.5, 0.1);

pub fn from_se2(relative_pose: SE2) -> Self

Create a new SE(2) between factor from an existing SE2 transformation.

Arguments

• relative_pose - The measured relative pose

Returns

A new BetweenFactorSE2 instance

Example

use apex_solver::core::factors::BetweenFactorSE2;
use apex_solver::manifold::se2::SE2;

let relative = SE2::from_xy_angle(1.0, 0.0, 0.0);
let factor = BetweenFactorSE2::from_se2(relative);

Trait Implementations

impl Clone for BetweenFactorSE2

fn clone(&self) -> BetweenFactorSE2

Returns a duplicate of the value.

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

Performs copy-assignment from source.

impl Debug for BetweenFactorSE2

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

Formats the value using the given formatter.

impl Factor for BetweenFactorSE2

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

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

Arguments

• params - Two SE(2) poses in G2O format: [x, y, theta]

Returns

• Residual: 3×1 vector [dx_error, dy_error, dtheta_error]
• Jacobian: 3×6 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.