namespace mrpt::gtsam_wrappers

Overview

namespace gtsam_wrappers {

// global functions

gtsam::Point3 toPoint3(const mrpt::math::TPoint3D& p);
mrpt::math::TPoint3D toTPoint3(const gtsam::Point3& p);
mrpt::math::TTwist3D toTTwist3D(const gtsam::Velocity3& v);
std::array<double, 3> toVelArray(const gtsam::Velocity3& v);
mrpt::math::CMatrixDouble66 to_mrpt_se3_cov6(const gtsam::Matrix6& se3_cov);
gtsam::Matrix6 to_gtsam_se3_cov6_reordering(const mrpt::math::CMatrixDouble66& se3_cov);
gtsam::Matrix6 to_gtsam_se3_cov6_isotropic(const mrpt::math::CMatrixDouble66& se3_cov);
gtsam::Pose3 toPose3(const mrpt::poses::CPose3D& p);
mrpt::math::TPose3D toTPose3D(const gtsam::Rot3& p);
gtsam::Pose3 toPose3(const mrpt::math::TPose3D& p);
mrpt::math::TPose3D toTPose3D(const gtsam::Pose3& p);
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_wrappers

Global Functions

mrpt::math::TTwist3D toTTwist3D(const gtsam::Velocity3& v)

Linear velocity only (the angular part is zero), in the same frame as the input: e.g.

the navigation (world) frame for gtsam::NavState velocities.

mrpt::math::CMatrixDouble66 to_mrpt_se3_cov6(const gtsam::Matrix6& se3_cov)

Reorders a covariance between the GTSAM (rx,ry,rz,tx,ty,tz) and MRPT (x,y,z,yaw,pitch,roll) layouts.

A plain reordering, exact only at the identity rotation: for poses, see mrpt::gtsam_wrappers::to_mrpt_se3_pdf() and mrpt::gtsam_wrappers::to_gtsam_se3_cov6().

gtsam::Matrix6 to_gtsam_se3_cov6_reordering(const mrpt::math::CMatrixDouble66& se3_cov)

Reorders a covariance between the GTSAM (rx,ry,rz,tx,ty,tz) and MRPT (x,y,z,yaw,pitch,roll) layouts.

A plain reordering, exact only at the identity rotation: for poses, see mrpt::gtsam_wrappers::to_mrpt_se3_pdf() and mrpt::gtsam_wrappers::to_gtsam_se3_cov6().

gtsam::Matrix6 to_gtsam_se3_cov6_isotropic(const mrpt::math::CMatrixDouble66& se3_cov)

Like to_gtsam_se3_cov6_reordering(), for diagonal covariances (it throws otherwise).

Despite its name, it does not check isotropy: the result is exact for any pose only if the translational variances are all equal and the rotational ones too, and the pose has pitch=0 (e.g. 4% error at pitch 30 deg); with different variances per axis, errors reach tens of percent for rotated poses. For poses, use mrpt::gtsam_wrappers::to_gtsam_se3_cov6().

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 the body-frame increment xi = Logmap(mean^-1 * pose) with respect to MRPT’s (x,y,z,yaw,pitch,roll), evaluated at the mean.

The rotation part is the body-frame angular velocity of R = Rz(yaw)*Ry(pitch)*Rx(roll), the translation part is R^T times the global translation increment.

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, with a (x,y,z,yaw,pitch,roll) covariance, into a GTSAM Pose3 and its covariance in GTSAM local coordinates (rx,ry,rz,tx,ty,tz), i.e.

for the body-frame increment xi = Logmap(mean^-1 * pose). This is a first-order (linearized) conversion.

See also:

to_mrpt_se3_pdf()

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

Inverse of to_gtsam_se3_cov6() : converts a GTSAM Pose3 and its covariance in GTSAM local coordinates (rx,ry,rz,tx,ty,tz) into an MRPT Gaussian SE(3) pose with a (x,y,z,yaw,pitch,roll) covariance.

First-order conversion. New in MRPT 3.7.0

Parameters:

std::exception

At pitch = +-90 deg, where (yaw,pitch,roll) is singular. Unlike mrpt::gtsam_wrappers::to_mrpt_se3_cov6(), which only reorders the entries and is therefore exact only at the identity rotation, this one takes the pose into account.