Struct PoseGraph 

pub struct PoseGraph { /* private fields */ }

Nodes, edges, and the optimisation over them.

Implementations

impl PoseGraph

pub fn new(poses: Vec<Se3>) -> Self

A graph with these nodes and no edges.

pub fn poses(&self) -> &[Se3]

The nodes, in order.

pub fn edges(&self) -> &[Edge]

The edges, in the order they were added.

pub fn push(&mut self, edge: Edge) -> Result<(), GraphError>

Adds an edge, refusing one that names a node that is not there.

pub fn shape(&self, anchor: usize) -> Shape

What shape the edges make, which decides what a solve can do.

Reported rather than discovered by failing: a survey with a node nothing joins to the anchor is one the normal equations cannot factorise, and finding that out from GraphError::Singular after the solve is a worse way to learn it than being told before.

pub fn residual(&self, edge: &Edge) -> Vector6<f64>

The disagreement on one edge: log(T_i⁻¹·T_j·Z⁻¹).

Zero when the two nodes sit exactly as the measurement says. The ordering — the measurement inverted on the right — is what puts the residual in the same frame and the same left-perturbation convention as the information matrix beside it.

pub fn cost(&self) -> f64

Σ rᵀΛr over the edges.

pub fn optimise( &mut self, params: &OptimiseParams, ) -> Result<Report, GraphError>

Levenberg-damped Gauss–Newton until the steps stop mattering.

impl PoseGraph

pub fn diagnose(&self, anchor: usize) -> Result<Diagnosis, GraphError>

What the survey looks like from above, at the poses it currently has.

Node spreads come from the pseudo-inverse of H = ΣJᵀΛJ, whose diagonal blocks are each station’s marginal covariance relative to the anchor. Pseudo- rather than plain inverse, and that is the whole point: a survey with a direction nothing constrains has a singular H, which is not an error to be damped away but the finding. A damped inverse would answer “this station is known to a centimetre” where the truth is that nothing in the survey knows where it is.

The null space is left out of the sum rather than inverted into infinities. Building V·diag(∞)·Vᵀ and reading blocks out of it does not give infinity where it should — an eigenvector component that is exactly zero turns ∞·0 into NaN, and the NaNs spread into the blocks of stations the survey determines perfectly well. Which coordinates are unconstrained is asked separately, of how much of each lies in the null space.

The cutoff is relative, at 1e-12 of the largest eigenvalue — the same shape of judgement the conditioning report makes about a single registration, one level up.

Trait Implementations

impl Clone for PoseGraph

fn clone(&self) -> PoseGraph

Returns a duplicate of the value.

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

Performs copy-assignment from source.

impl Debug for PoseGraph

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

Formats the value using the given formatter.

impl Default for PoseGraph

fn default() -> PoseGraph

Returns the “default value” for a type.