Pose & Priors

factors::pose — relative constraints and anchors. No sensor model: these express graph topology.

Throughout, $⊞$ is the right perturbation $X ⊞ δ = X Exp (δ)$ , $⊟$ its inverse $Y ⊟ X = Log (X^{- 1} Y)$ , and $J_{r}$ , $J_{l}$ the right and left Jacobians of the group. The identity used everywhere below is

$Log (X Exp (ϵ)) \approx Log (X) + J_{r}^{- 1} (Log (X)) ϵ$

`BetweenFactor<T>`

A relative measurement between two variables of the same Lie group. Odometry edges and loop closures are both this factor, the latter usually with a robust loss.

Blocks $[T_{i}, T_{j}]$ — any T: LieGroup. Residual $dof (T)$ , Jacobian $dof (T) \times 2 dof (T)$ .

Error

With measurement $\tilde{T}_{ij}$ ,

$r = Log (\tilde{T}_{ij}^{- 1} T_{i}^{- 1} T_{j})$

zero exactly when the estimated relative transform equals the measured one.

Jacobian

Write $A = T_{i}^{- 1} T_{j}$ . Perturbing the second variable, $\tilde{T}^{- 1} A Exp (δ_{j})$ , gives directly

$\frac{\partial r}{\partial δ _{j}} = J_{r}^{- 1} (r)$

Perturbing the first, $T_{i} \leftarrow T_{i} Exp (δ_{i})$ , moves the inverse to the left of $A$ , and it has to be commuted through:

$\overset{^{- 1} Exp (- δ_{i}) A = T}{T}^{- 1} A Exp (- Ad_{A^{- 1}} δ_{i})$

$\frac{\partial r}{\partial δ _{i}} = - J_{r}^{- 1} (r) Ad_{T_{j}^{- 1} T_{i}}$

For SE(3), $Ad_{T} = [R 0 [t]_{\times} R R]$ and

$J_{r}^{- 1} (ρ, θ) = [J_{r}^{- 1} (θ) 0 Q_{r} J_{r}^{- 1} (θ)], J_{r}^{- 1} (θ) = I + \frac{1}{2} [θ]_{\times} + (\frac{1}{θ ^{2}} - \frac{1 + c o s θ}{2 θ s i n θ}) [θ]_{\times}^{2}$

The implementation never writes these forms out: it chains the Jacobians that between, compose and log report, so it stays correct for every group.

#![allow(unused)]
fn main() {
use apex_solver::factors::pose::BetweenFactor;

problem.add_residual_block_with_noise(
    &[key_i, key_j],
    Box::new(BetweenFactor::new(measured_relative_pose)),
    Some(Box::new(HuberLoss::new(1.345)?)),
    NoiseModel::from_sigmas(&[0.05, 0.05, 0.05, 0.01, 0.01, 0.02])?,
);
}

`PriorFactor<T>`

Anchors one variable to a known value in the tangent space. Prefer this to Problem::fix_variable: fixed indices are honoured when the step is applied, but the linear system still treats those coordinates as free, so variables sharing factors with a “fixed” one are systematically under-corrected.

Blocks $[X]$ . Residual $dof (T)$ , Jacobian square.

Error and Jacobian

$r = Log (T_{prior}^{- 1} X), \frac{\partial r}{\partial δ} = J_{r}^{- 1} (r)$

This is a BetweenFactor with an identity measurement and a constant first argument, and is implemented as exactly that — the two share one chain and cannot drift apart. Because the residual never touches the parameters, there is no quaternion double-cover ambiguity.

`EuclideanPriorFactor`

$r = x - x_{prior}, \frac{\partial r}{\partial x} = I_{n}$

Registration rejects any manifold but Rn — on a Lie group this would difference raw parameters, which is not a tangent-space error. Use it to anchor velocities, biases and intrinsics.

`PoseRotationPrior` / `PoseTranslationPrior`

Constrain one half of an SE(3) pose and leave the other free.

$Rotation: r = Log (R_{meas}^{⊤} R), \frac{\partial r}{\partial ( δ ρ , δ θ )} = [0_{3 \times 3} J_{r}^{- 1} (r)]$

$Translation: r = t - \tilde{t}, \frac{\partial r}{\partial ( δ ρ , δ θ )} = [R 0_{3 \times 3}]$

The translation block is $R$ , not $I$ , because the right perturbation moves the origin by $R δ ρ$ in the world frame.

These are not a decomposition of PriorFactor<SE3>: the translation rows here are the world-frame difference, whereas the prior’s are the coupled SE(3) logarithm $ρ$ , which mixes rotation into translation through $J_{l}^{- 1} (θ)$ . Stacking both does not reproduce the full prior.

PoseRotationPrior is the usual rotation-only loop-closure initializer; PoseTranslationPrior is how a GNSS position fix attaches to an SE(3) pose.

Keyboard shortcuts

Apex Solver Cookbook