diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 6fbd2468f..16dda3d1e 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -11,20 +11,19 @@ /** * @file Pose3.cpp - * @brief 3D Pose + * @brief 3D Pose manifold SO(3) x R^3 and group SE(3) */ -#include -#include -#include #include +#include +#include +#include +#include #include #include #include -#include "gtsam/geometry/Rot3.h" - namespace gtsam { /** instantiate concept checks */ @@ -164,6 +163,8 @@ Pose3 Pose3::interpolateRt(const Pose3& T, double t) const { } /* ************************************************************************* */ +// Expmap is implemented in so3::ExpmapFunctor::expmap, based on Ethan Eade's +// elegant Lie group document, at https://www.ethaneade.org/lie.pdf. 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>(); diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 04504de65..81a9848e2 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -11,7 +11,7 @@ /** *@file Pose3.h - *@brief 3D Pose + * @brief 3D Pose manifold SO(3) x R^3 and group SE(3) */ // \callgraph diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index 6e2e38101..163ae6623 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -132,6 +132,8 @@ GTSAM_EXPORT Matrix99 Dcompose(const SO3& R); // functor also implements dedicated methods to apply dexp and/or inv(dexp). /// Functor implementing Exponential map +/// Math is based on Ethan Eade's elegant Lie group document, at +/// https://www.ethaneade.org/lie.pdf. struct GTSAM_EXPORT ExpmapFunctor { const double theta2, theta; const Matrix3 W, WW; @@ -155,6 +157,8 @@ struct GTSAM_EXPORT ExpmapFunctor { }; /// Functor that implements Exponential map *and* its derivatives +/// Math extends Ethan theme of elegant I + aW + bWW expressions. +/// See https://www.ethaneade.org/lie.pdf expmap (82) and left Jacobian (83). struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor { const Vector3 omega; diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index 200642456..c6fd52161 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -19,7 +19,6 @@ #include #include #include -#include using namespace std::placeholders; using namespace std; diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index c9cd244d3..bd482132e 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -114,7 +114,7 @@ NavState NavState::inverse() const { //------------------------------------------------------------------------------ NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi) { - // Get angular velocity w and components rho and nu from xi + // 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>(); // Compute rotation using Expmap