diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 6d9256f93b..f6d0c43cb6 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -183,18 +183,37 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) { // Get angular velocity omega and translational velocity v from twist xi const Vector3 w = xi.head<3>(), v = xi.tail<3>(); + // Instantiate functor for Dexp-related operations: + const bool nearZero = (w.dot(w) <= 1e-5); + const so3::DexpFunctor local(w, nearZero); + // Compute rotation using Expmap - Matrix3 Jr; - Rot3 R = Rot3::Expmap(w, Hxi ? &Jr : nullptr); +#ifdef GTSAM_USE_QUATERNIONS + const Rot3 R = traits::Expmap(w); +#else + const Rot3 R(local.expmap()); +#endif - // Compute translation and optionally its Jacobian Q in w - // The Jacobian in v is the right Jacobian Jr of SO(3), which we already have. - Matrix3 Q; - const Vector3 t = ExpmapTranslation(w, v, Hxi ? &Q : nullptr); + // The translation t = local.leftJacobian() * v. + // Here we call applyLeftJacobian, which is faster if you don't need + // Jacobians, and returns Jacobian of t with respect to w if asked. + // NOTE(Frank): t = applyLeftJacobian(v) does the same as the intuitive formulas + // t_parallel = w * w.dot(v); // translation parallel to axis + // w_cross_v = w.cross(v); // translation orthogonal to axis + // t = (w_cross_v - Rot3::Expmap(w) * w_cross_v + t_parallel) / theta2; + // but functor does not need R, deals automatically with the case where theta2 + // is near zero, and also gives us the machinery for the Jacobians. + Matrix3 H; + const Vector3 t = local.applyLeftJacobian(v, Hxi ? &H : nullptr); if (Hxi) { - *Hxi << Jr, Z_3x3, // - Q, Jr; + // The Jacobian of expmap is given by the right Jacobian of SO(3): + const Matrix3 Jr = local.rightJacobian(); + // We are creating a Pose3, so we still need to chain H with R^T, the + // Jacobian of Pose3::Create with respect to t. + const Matrix3 Q = R.matrix().transpose() * H; + *Hxi << Jr, Z_3x3, // Jr here *is* the Jacobian of expmap + Q, Jr; // Here Jr = R^T * J_l, with J_l the Jacobian of t in v. } return Pose3(R, t); @@ -260,45 +279,18 @@ Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi, double nearZeroThreshold) { const auto w = xi.head<3>(); const auto v = xi.tail<3>(); - Matrix3 Q; - ExpmapTranslation(w, v, Q, {}, nearZeroThreshold); - return Q; -} - -/* ************************************************************************* */ -// NOTE(Frank): t = applyLeftJacobian(v) does the same as the intuitive formulas -// t_parallel = w * w.dot(v); // translation parallel to axis -// w_cross_v = w.cross(v); // translation orthogonal to axis -// t = (w_cross_v - Rot3::Expmap(w) * w_cross_v + t_parallel) / theta2; -// but functor does not need R, deals automatically with the case where theta2 -// is near zero, and also gives us the machinery for the Jacobians. -Vector3 Pose3::ExpmapTranslation(const Vector3& w, const Vector3& v, - OptionalJacobian<3, 3> Q, - OptionalJacobian<3, 3> J, - double nearZeroThreshold) { - const double theta2 = w.dot(w); - bool nearZero = (theta2 <= nearZeroThreshold); // Instantiate functor for Dexp-related operations: + bool nearZero = (w.dot(w) <= nearZeroThreshold); so3::DexpFunctor local(w, nearZero); - // Call applyLeftJacobian which is faster than local.leftJacobian() * v if you - // don't need Jacobians, and returns Jacobian of t with respect to w if asked. + // Call applyLeftJacobian to get its Jacobian Matrix3 H; - Vector t = local.applyLeftJacobian(v, Q ? &H : nullptr); - - // We return Jacobians for use in Expmap, so we multiply with X, that - // translates from left to right for our right expmap convention: - if (Q) { - Matrix3 X = local.rightJacobian() * local.leftJacobianInverse(); - *Q = X * H; - } - - if (J) { - *J = local.rightJacobian(); // = X * local.leftJacobian(); - } + local.applyLeftJacobian(v, H); - return t; + // Multiply with R^T to account for the Pose3::Create Jacobian. + const Matrix3 R = local.expmap(); + return R.transpose() * H; } /* ************************************************************************* */ diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index e4896ae26f..af14ac82cc 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -220,27 +220,10 @@ class GTSAM_EXPORT Pose3: public LieGroup { * (see Chirikjian11book2, pg 44, eq 10.95. * The closed-form formula is identical to formula 102 in Barfoot14tro where * Q_l of the SE3 Expmap left derivative matrix is given. - * This is the Jacobian of ExpmapTranslation and computed there. */ static Matrix3 ComputeQforExpmapDerivative( const Vector6& xi, double nearZeroThreshold = 1e-5); - /** - * Compute the translation part of the exponential map, with Jacobians. - * @param w 3D angular velocity - * @param v 3D velocity - * @param Q Optionally, compute 3x3 Jacobian wrpt w - * @param J Optionally, compute 3x3 Jacobian wrpt v = right Jacobian of SO(3) - * @param nearZeroThreshold threshold for small values - * @note This function returns Jacobians Q and J corresponding to the bottom - * blocks of the SE(3) exponential, and translated from left to right from the - * applyLeftJacobian Jacobians. - */ - static Vector3 ExpmapTranslation(const Vector3& w, const Vector3& v, - OptionalJacobian<3, 3> Q = {}, - OptionalJacobian<3, 3> J = {}, - double nearZeroThreshold = 1e-5); - using LieGroup::inverse; // version with derivative /** diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index 41f7ce8107..a1947953ba 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -92,7 +92,7 @@ ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle, init(nearZeroApprox); } -SO3 ExpmapFunctor::expmap() const { return SO3(I_3x3 + A * W + B * WW); } +Matrix3 ExpmapFunctor::expmap() const { return I_3x3 + A * W + B * WW; } DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox) : ExpmapFunctor(omega, nearZeroApprox), omega(omega) { @@ -193,7 +193,7 @@ Vector3 DexpFunctor::applyLeftJacobianInverse(const Vector3& v, template <> GTSAM_EXPORT SO3 SO3::AxisAngle(const Vector3& axis, double theta) { - return so3::ExpmapFunctor(axis, theta).expmap(); + return SO3(so3::ExpmapFunctor(axis, theta).expmap()); } //****************************************************************************** @@ -251,11 +251,11 @@ template <> GTSAM_EXPORT SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) { if (H) { - so3::DexpFunctor impl(omega); - *H = impl.dexp(); - return impl.expmap(); + so3::DexpFunctor local(omega); + *H = local.dexp(); + return SO3(local.expmap()); } else { - return so3::ExpmapFunctor(omega).expmap(); + return SO3(so3::ExpmapFunctor(omega).expmap()); } } diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index 36329a19d5..1bdcd82c75 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -150,7 +150,7 @@ struct GTSAM_EXPORT ExpmapFunctor { ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApprox = false); /// Rodrigues formula - SO3 expmap() const; + Matrix3 expmap() const; protected: void init(bool nearZeroApprox = false); diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index c6fd521613..41777ae3aa 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -163,22 +163,22 @@ TEST(SO3, ExpmapFunctor) { // axis angle version so3::ExpmapFunctor f1(axis, angle); - SO3 actual1 = f1.expmap(); + SO3 actual1(f1.expmap()); CHECK(assert_equal(expected, actual1.matrix(), 1e-5)); // axis angle version, negative angle so3::ExpmapFunctor f2(axis, angle - 2*M_PI); - SO3 actual2 = f2.expmap(); + SO3 actual2(f2.expmap()); CHECK(assert_equal(expected, actual2.matrix(), 1e-5)); // omega version so3::ExpmapFunctor f3(axis * angle); - SO3 actual3 = f3.expmap(); + SO3 actual3(f3.expmap()); CHECK(assert_equal(expected, actual3.matrix(), 1e-5)); // omega version, negative angle so3::ExpmapFunctor f4(axis * (angle - 2*M_PI)); - SO3 actual4 = f4.expmap(); + SO3 actual4(f4.expmap()); CHECK(assert_equal(expected, actual4.matrix(), 1e-5)); } diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index bd482132e4..116efe90a2 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -117,20 +117,32 @@ NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi) { // Get angular velocity w and components rho (for t) and nu (for v) from xi Vector3 w = xi.head<3>(), rho = xi.segment<3>(3), nu = xi.tail<3>(); + // Instantiate functor for Dexp-related operations: + const bool nearZero = (w.dot(w) <= 1e-5); + const so3::DexpFunctor local(w, nearZero); + // Compute rotation using Expmap - Matrix3 Jr; - Rot3 R = Rot3::Expmap(w, Hxi ? &Jr : nullptr); +#ifdef GTSAM_USE_QUATERNIONS + const Rot3 R = traits::Expmap(w); +#else + const Rot3 R(local.expmap()); +#endif - // Compute translations and optionally their Jacobians Q in w - // The Jacobians with respect to rho and nu are equal to Jr - Matrix3 Qt, Qv; - Vector3 t = Pose3::ExpmapTranslation(w, rho, Hxi ? &Qt : nullptr); - Vector3 v = Pose3::ExpmapTranslation(w, nu, Hxi ? &Qv : nullptr); + // Compute translation and velocity. See Pose3::Expmap + Matrix3 H_t_w, H_v_w; + const Vector3 t = local.applyLeftJacobian(rho, Hxi ? &H_t_w : nullptr); + const Vector3 v = local.applyLeftJacobian(nu, Hxi ? &H_v_w : nullptr); if (Hxi) { - *Hxi << Jr, Z_3x3, Z_3x3, // - Qt, Jr, Z_3x3, // - Qv, Z_3x3, Jr; + const Matrix3 Jr = local.rightJacobian(); + // We are creating a NavState, so we still need to chain H_t_w and H_v_w + // with R^T, the Jacobian of Navstate::Create with respect to both t and v. + const Matrix3 M = R.matrix(); + *Hxi << Jr, Z_3x3, Z_3x3, // Jr here *is* the Jacobian of expmap + M.transpose() * H_t_w, Jr, Z_3x3, // + M.transpose() * H_v_w, Z_3x3, Jr; + // In the last two rows, Jr = R^T * J_l, see Barfoot eq. (8.83). + // J_l is the Jacobian of applyLeftJacobian in the second argument. } return NavState(R, t, v); @@ -231,11 +243,22 @@ Matrix9 NavState::LogmapDerivative(const NavState& state) { const Vector3 w = xi.head<3>(); Vector3 rho = xi.segment<3>(3); Vector3 nu = xi.tail<3>(); - - Matrix3 Qt, Qv; - const Rot3 R = Rot3::Expmap(w); - Pose3::ExpmapTranslation(w, rho, Qt); - Pose3::ExpmapTranslation(w, nu, Qv); + + // Instantiate functor for Dexp-related operations: + const bool nearZero = (w.dot(w) <= 1e-5); + const so3::DexpFunctor local(w, nearZero); + + // Call applyLeftJacobian to get its Jacobians + Matrix3 H_t_w, H_v_w; + local.applyLeftJacobian(rho, H_t_w); + local.applyLeftJacobian(nu, H_v_w); + + // Multiply with R^T to account for NavState::Create Jacobian. + const Matrix3 R = local.expmap(); + const Matrix3 Qt = R.transpose() * H_t_w; + const Matrix3 Qv = R.transpose() * H_v_w; + + // Now compute the blocks of the LogmapDerivative Jacobian const Matrix3 Jw = Rot3::LogmapDerivative(w); const Matrix3 Qt2 = -Jw * Qt * Jw; const Matrix3 Qv2 = -Jw * Qv * Jw; @@ -247,7 +270,6 @@ Matrix9 NavState::LogmapDerivative(const NavState& state) { return J; } - //------------------------------------------------------------------------------ NavState NavState::ChartAtOrigin::Retract(const Vector9& xi, ChartJacobian Hxi) {