diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index cb8de111e..9f81204d6 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -22,6 +22,7 @@ #include #include #include + #include "gtsam/geometry/Rot3.h" namespace gtsam { @@ -112,7 +113,7 @@ Matrix6 Pose3::adjointMap(const Vector6& xi) { /* ************************************************************************* */ Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y, - OptionalJacobian<6, 6> Hxi, OptionalJacobian<6, 6> H_y) { + OptionalJacobian<6, 6> Hxi, OptionalJacobian<6, 6> H_y) { if (Hxi) { Hxi->setZero(); for (int i = 0; i < 6; ++i) { @@ -159,7 +160,7 @@ bool Pose3::equals(const Pose3& pose, double tol) const { /* ************************************************************************* */ Pose3 Pose3::interpolateRt(const Pose3& T, double t) const { return Pose3(interpolate(R_, T.R_, t), - interpolate(t_, T.t_, t)); + interpolate(t_, T.t_, t)); } /* ************************************************************************* */ @@ -239,30 +240,14 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) { #endif } -/* ************************************************************************* */ -namespace pose3 { -struct GTSAM_EXPORT ExpmapFunctor : public so3::DexpFunctor { - ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false) - : so3::DexpFunctor(omega, nearZeroApprox) { - } - - // Compute the bottom-left 3x3 block of the SE(3) Expmap derivative. - Matrix3 computeQ(const Vector3& v) const { - // X translate from left to right for our right expmap convention: - Matrix X = rightJacobian() * leftJacobianInverse(); - Matrix3 H; - applyLeftJacobian(v, H); - return X * H; - } -}; -} // namespace pose3 - /* ************************************************************************* */ Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi, double nearZeroThreshold) { const auto w = xi.head<3>(); const auto v = xi.tail<3>(); - return pose3::ExpmapFunctor(w).computeQ(v); + Matrix3 Q; + ExpmapTranslation(w, v, Q, {}, nearZeroThreshold); + return Q; } /* ************************************************************************* */ @@ -273,13 +258,22 @@ Vector3 Pose3::ExpmapTranslation(const Vector3& w, const Vector3& v, const double theta2 = w.dot(w); bool nearZero = (theta2 <= nearZeroThreshold); - if (Q) *Q = pose3::ExpmapFunctor(w, nearZero).computeQ(v); - - if (nearZero) { - return v + 0.5 * w.cross(v); + if (Q) { + // Instantiate functor for Dexp-related operations: + so3::DexpFunctor local(w, nearZero); + // X translate from left to right for our right expmap convention: + Matrix3 X = local.rightJacobian() * local.leftJacobianInverse(); + Matrix3 H; + Vector t = local.applyLeftJacobian(v, H); + *Q = X * H; + return t; + } else if (nearZero) { + // (I_3x3 + B * W + C * WW) * v with B->0.5, C->1/6 + Vector3 Wv = w.cross(v); + return v + 0.5 * Wv + w.cross(Wv) * so3::DexpFunctor::one_sixth; } else { - // NOTE(Frank): t can also be computed by calling applyLeftJacobian(v), but - // formulas below convey geometric insight and creating functor is not free. + // NOTE(Frank): if Q is not asked we use formulas below instead of calling + // applyLeftJacobian(v), as they convey geometric insight and are faster. Vector3 t_parallel = w * w.dot(v); // translation parallel to axis Vector3 w_cross_v = w.cross(v); // translation orthogonal to axis Rot3 rotation = R.value_or(Rot3::Expmap(w)); @@ -314,7 +308,6 @@ const Point3& Pose3::translation(OptionalJacobian<3, 6> Hself) const { } /* ************************************************************************* */ - const Rot3& Pose3::rotation(OptionalJacobian<3, 6> Hself) const { if (Hself) { *Hself << I_3x3, Z_3x3; @@ -332,14 +325,14 @@ Matrix4 Pose3::matrix() const { /* ************************************************************************* */ Pose3 Pose3::transformPoseFrom(const Pose3& aTb, OptionalJacobian<6, 6> Hself, - OptionalJacobian<6, 6> HaTb) const { + OptionalJacobian<6, 6> HaTb) const { const Pose3& wTa = *this; return wTa.compose(aTb, Hself, HaTb); } /* ************************************************************************* */ Pose3 Pose3::transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> Hself, - OptionalJacobian<6, 6> HwTb) const { + OptionalJacobian<6, 6> HwTb) const { if (Hself) *Hself = -wTb.inverse().AdjointMap() * AdjointMap(); if (HwTb) *HwTb = I_6x6; const Pose3& wTa = *this; @@ -348,7 +341,7 @@ Pose3 Pose3::transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> Hself, /* ************************************************************************* */ Point3 Pose3::transformFrom(const Point3& point, OptionalJacobian<3, 6> Hself, - OptionalJacobian<3, 3> Hpoint) const { + OptionalJacobian<3, 3> Hpoint) const { // Only get matrix once, to avoid multiple allocations, // as well as multiple conversions in the Quaternion case const Matrix3 R = R_.matrix(); @@ -372,7 +365,7 @@ Matrix Pose3::transformFrom(const Matrix& points) const { /* ************************************************************************* */ Point3 Pose3::transformTo(const Point3& point, OptionalJacobian<3, 6> Hself, - OptionalJacobian<3, 3> Hpoint) const { + OptionalJacobian<3, 3> Hpoint) const { // Only get transpose once, to avoid multiple allocations, // as well as multiple conversions in the Quaternion case const Matrix3 Rt = R_.transpose(); @@ -478,7 +471,7 @@ std::optional Pose3::Align(const Point3Pairs &abPointPairs) { std::optional Pose3::Align(const Matrix& a, const Matrix& b) { if (a.rows() != 3 || b.rows() != 3 || a.cols() != b.cols()) { throw std::invalid_argument( - "Pose3:Align expects 3*N matrices of equal shape."); + "Pose3:Align expects 3*N matrices of equal shape."); } Point3Pairs abPointPairs; for (Eigen::Index j = 0; j < a.cols(); j++) { diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 4ff4f9a85..3b3de6792 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -900,17 +900,6 @@ TEST(Pose3, ExpmapDerivative4) { } } -TEST( Pose3, ExpmapDerivativeQr) { - Vector6 w = Vector6::Random(); - w.head<3>().normalize(); - w.head<3>() = w.head<3>() * 0.9e-2; - Matrix3 actualQr = Pose3::ComputeQforExpmapDerivative(w, 0.01); - Matrix expectedH = numericalDerivative21 >(&Pose3::Expmap, w, {}); - Matrix3 expectedQr = expectedH.bottomLeftCorner<3, 3>(); - EXPECT(assert_equal(expectedQr, actualQr, 1e-6)); -} - /* ************************************************************************* */ TEST( Pose3, LogmapDerivative) { Matrix6 actualH;