namespace mola::gtsam_compat

Overview

namespace gtsam_compat {

// global functions

gtsam::Matrix6 jacobian_gtsam_local_wrt_ypr(const mrpt::poses::CPose3D& mean);

void to_gtsam_se3_cov6(
    const mrpt::poses::CPose3DPDFGaussian& in,
    gtsam::Pose3& pose_out,
    gtsam::Matrix6& cov_out
    );

mrpt::poses::CPose3DPDFGaussian to_mrpt_se3_pdf(
    const gtsam::Pose3& pose,
    const gtsam::Matrix6& cov
    );

} // namespace gtsam_compat

Global Functions

gtsam::Matrix6 jacobian_gtsam_local_wrt_ypr(const mrpt::poses::CPose3D& mean)

Jacobian of the GTSAM local coordinates (rx,ry,rz,tx,ty,tz) of xi = Logmap(mean^-1 * pose) with respect to MRPT’s (x,y,z,yaw,pitch,roll), evaluated at the mean.

void to_gtsam_se3_cov6(
    const mrpt::poses::CPose3DPDFGaussian& in,
    gtsam::Pose3& pose_out,
    gtsam::Matrix6& cov_out
    )

Converts an MRPT Gaussian SE(3) pose into a GTSAM Pose3 and its covariance in GTSAM local coordinates (rx,ry,rz,tx,ty,tz). First-order conversion.

mrpt::poses::CPose3DPDFGaussian to_mrpt_se3_pdf(
    const gtsam::Pose3& pose,
    const gtsam::Matrix6& cov
    )

Inverse of to_gtsam_se3_cov6(). First-order conversion.

Parameters:

std::exception

At pitch = +-90 deg, where (yaw,pitch,roll) is singular.