diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 6d9256f93b..9c3bd77339 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -183,16 +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(v); +#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) { + // The Jacobian of expmap is given by the right Jacobian of SO(3): + const Matrix3 Jr = local.rightJacobian(); + // We multiply H, the derivative of applyLeftJacobian in omega, with + // X = Jr * Jl^{-1}, + // which translates from left to right for our right expmap convention: + const Matrix3 X = Jr * local.leftJacobianInverse(); + const Matrix3 Q = X * H; *Hxi << Jr, Z_3x3, // Q, Jr; } @@ -260,45 +281,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 X, translates from left to right for our expmap convention: + const Matrix3 X = local.rightJacobian() * local.leftJacobianInverse(); + return X * 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/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index bd482132e4..d033c96e88 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -117,20 +117,29 @@ 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(v); +#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) { + // See Pose3::Expamp for explanation of the Jacobians + const Matrix3 Jr = local.rightJacobian(); + const Matrix3 X = Jr * local.leftJacobianInverse(); *Hxi << Jr, Z_3x3, Z_3x3, // - Qt, Jr, Z_3x3, // - Qv, Z_3x3, Jr; + X * H_t_w, Jr, Z_3x3, // + X * H_v_w, Z_3x3, Jr; } return NavState(R, t, v); @@ -231,11 +240,21 @@ 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 X, translates from left to right for our expmap convention: + const Matrix3 X = local.rightJacobian() * local.leftJacobianInverse(); + const Matrix3 Qt = X * H_t_w; + const Matrix3 Qv = X * H_v_w; + const Matrix3 Jw = Rot3::LogmapDerivative(w); const Matrix3 Qt2 = -Jw * Qt * Jw; const Matrix3 Qv2 = -Jw * Qv * Jw; @@ -247,7 +266,6 @@ Matrix9 NavState::LogmapDerivative(const NavState& state) { return J; } - //------------------------------------------------------------------------------ NavState NavState::ChartAtOrigin::Retract(const Vector9& xi, ChartJacobian Hxi) {