struct mp2p_icp::GravityPrior

Overview

A gravity (“verticality”) observation for the optimal-transformation solvers.

It constrains the two TILT degrees of freedom only, leaving rotation about the gravity axis (yaw) and all three translations exactly free. Use it to keep a solution gravity-aligned without the parameterization hazards of encoding tilt into a 6x6 SE(3) prior information matrix: a diagonal there only isolates roll/pitch when yaw is near zero, and it inevitably couples into translation.

The residual is the horizontal component of the predicted “up” vector,

\[r(R) = B^\top \, (R \, u_{body}) \in \mathbb{R}^2\]

where \(B\) (3x2) is an orthonormal basis of the plane orthogonal to up_map, weighted isotropically by \(1/\sigma^2\). \(r = 0\) exactly when \(R\,u_{body}\) is parallel to up_map, i.e. when the solution is level; for small tilt \(\|r\|\) is the tilt angle in radians.

Isotropic weighting is what makes the cost invariant to rotation about gravity. The induced 6x6 information \(J^\top W J\) then has rank 2, a zero translation block, and null space exactly \(span(u_{body})\), i.e. rotation about the gravity axis - so yaw stays free at ANY attitude, not only near yaw=0.

#include <GravityPrior.h>

struct GravityPrior
{
    // fields

    mrpt::math::TVector3D up_body {0, 0, 1};
    mrpt::math::TVector3D up_map {0, 0, 1};
    double sigma_rad = 0.02;
};

Fields

mrpt::math::TVector3D up_body {0, 0, 1}

Measured gravity “up” direction in the BODY/vehicle frame, e.g. the normalized accelerometer specific force while quasi-static. Need not be normalized by the caller.

mrpt::math::TVector3D up_map {0, 0, 1}

Gravity “up” direction expressed in the MAP/global frame. Use (0,0,1) for a gravity-aligned map, or the direction captured at map origin if the map frame is not exactly level. Need not be normalized.

double sigma_rad = 0.02

Standard deviation [rad] of the tilt observation. Smaller means a stronger, closer-to-mandatory leveling constraint.