Address review comments

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 <gtsam/geometry/Pose3.h>
-#include <gtsam/geometry/Pose2.h>
-#include <gtsam/geometry/concepts.h>
 #include <gtsam/base/concepts.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/geometry/concepts.h>
 
 #include <cmath>
 #include <iostream>
 #include <string>
 
-#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>();

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

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;
 

gtsam/geometry/tests/testSO3.cpp

@@ -19,7 +19,6 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/testLie.h>
 #include <gtsam/geometry/SO3.h>
-#include <iostream>
 
 using namespace std::placeholders;
 using namespace std;

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