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:
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. |