ICP pose prior

Overview

The ICP solver applies the information matrix of its pose prior to log(P_prior^-1 * P_cur), i.e. in the SE(3) tangent of the prior mean: translation in the frame of the mean, then rotation-vector components. A motion model gives its pose in the [x y z yaw pitch roll] layout of CPose3DPDFGaussianInf instead, and converting between both depends on the mean (mp2p_icp::information_ypr_to_tangent()). These keep the information in the tangent of the mean it is applied at.

Against an mp2p_icp without MP2P_ICP_HAS_COVARIANCE_YPR_CONVERSIONS, whose Censi3D covariances reached the motion model already in that tangent, they leave the information unchanged, as before.

// global functions

mrpt::poses::CPose3DPDFGaussianInf mola::icp_prior_from_motion_model(const mrpt::poses::CPose3DPDFGaussianInf& motionModelPose);

void mola::set_icp_prior_mean(
    mrpt::poses::CPose3DPDFGaussianInf& prior,
    const mrpt::poses::CPose3D& newMean
    );

Global Functions

mrpt::poses::CPose3DPDFGaussianInf mola::icp_prior_from_motion_model(const mrpt::poses::CPose3DPDFGaussianInf& motionModelPose)

The ICP prior for a motion model pose given in the [x y z yaw pitch roll] layout: same mean, information converted to the tangent of the mean.

void mola::set_icp_prior_mean(
    mrpt::poses::CPose3DPDFGaussianInf& prior,
    const mrpt::poses::CPose3D& newMean
    )

Moves the mean of an ICP prior, re-expressing its information in the tangent of the new mean. For a prior built with icp_prior_from_motion_model(), the result is the motion model information converted at the new mean.