struct mp2p_icp::OptimalTF_GN_Parameters

Overview

#include <optimal_tf_gauss_newton.h>

struct OptimalTF_GN_Parameters
{
    // fields

    std::optional<mrpt::poses::CPose3D> linearizationPoint;
    std::optional<mrpt::poses::CPose3DPDFGaussianInf> prior;
    std::optional<GravityPrior> gravityPrior;
    double minDelta = 1e-7;
    double maxCost = 0;
    PairWeights pairWeights;
    uint32_t maxInnerLoopIterations = 6;
    RobustKernel kernel = RobustKernel::None;
    double kernelScale = 1.0;
    double kernelPriorRefBlend = 0.0;
    double cov2cov_alpha = 1.0;
    bool cov2cov_auto_balance_with_prior = true;
    bool verbose = false;
};

Fields

std::optional<mrpt::poses::CPose3D> linearizationPoint

The linerization point (the current relative pose guess)

std::optional<mrpt::poses::CPose3DPDFGaussianInf> prior

Optional prior guess of the SE(3) solution, including a mean value and an inverse covariance (information) matrix, i.e. zeros in the diagonal mean that those prior coordinates should be ignored, a large value means the solution must be close to those coordinates.

The information matrix is applied to the residual log(P_prior^-1 · P_cur) and is therefore expressed in the SE(3) Lie tangent: entries [0..2] are x,y,z and [3..5] are the rotation-VECTOR components (w_x, w_y, w_z), i.e. approximately (roll, pitch, yaw) axes. This is NOT MRPT’s (x, y, z, yaw, pitch, roll) Euler ordering used by CPose3DPDFGaussian elsewhere: the two swap indices 3 and 5.

std::optional<GravityPrior> gravityPrior

Optional gravity (“verticality”) observation, INDEPENDENT of prior : a yaw-free, rank-2 tilt constraint that never touches translation. See mp2p_icp::GravityPrior for the residual and its properties.

Being a function of the current rotation, its linearization is rebuilt at every Gauss-Newton iteration (unlike a fixed prior information).

double minDelta = 1e-7

Minimum SE(3) change to stop iterating.

double maxCost = 0

Maximum cost function; when reached, stop iterating.

uint32_t maxInnerLoopIterations = 6

Maximum number of iterations trying to solve for the optimal pose

double kernelScale = 1.0

Robust kernel scale: the residual size at which down-weighting sets in, in the units of the (whitened) residual the kernel sees, i.e. in sigmas. Renamed from the former kernelParam, which named the same quantity squared.

double kernelPriorRefBlend = 0.0

Reference used to evaluate the residual that feeds the robust kernel, blended between the current linearization point (β=0) and the residual predicted at the prior mean pose (β=1):

refSqrNorm = (1-β)·‖r_i(x)‖² + β·‖r_i(prior.mean)‖²

β=0 (default) reproduces the classic behavior, where a factor is judged only by how much it diverges from the current iterate. With β>0 each factor is (partially) judged by how much it diverges from the prior Gaussian, so correspondences inconsistent with the prior are down-weighted even if they look fine at a (possibly already corrupted) current iterate. Has no effect when no prior is given, no robust kernel is selected, or β=0. Range: [0,1].

double cov2cov_alpha = 1.0

Generalized (tempered) Bayesian scaling for the cov-to-cov data block: the cov2cov contribution to H and g is multiplied by cov2cov_alpha. α=1 keeps the standard MAP cost; α<1 down-weights the data likelihood (e.g. α=1/N turns it into a “mean residual” form). Useful when many cov-to-cov pairings carry correlated information that the per-pair modeled covariances do not capture, and the prior is otherwise drowned.

bool cov2cov_auto_balance_with_prior = true

If true (default) and a prior is provided, automatically balance the cov2cov data block against the prior using a Birge-ratio-style global scale of the modeled per-pair covariances: κ = max(1, χ²_cc / (3·N_cc − 6)) evaluated at each iteration’s current residuals; the cov2cov block of H and g is then scaled by 1/κ. Has no effect when the prior is absent.