Add LocalCoordinates() to ProductLieGroup and remove unnecessary <Eigen/Core> include to reduce compile memory

gtsam/base/ProductLieGroup.h

@@ -161,6 +161,9 @@ public:
     }
     return v;
   }
+  static TangentVector LocalCoordinates(const ProductLieGroup& p, ChartJacobian Hp = boost::none) {
+    return Logmap(p, Hp);
+  }
   ProductLieGroup expmap(const TangentVector& v) const {
     return compose(ProductLieGroup::Expmap(v));
   }

gtsam_unstable/dynamics/DynamicsPriors.h

@@ -9,20 +9,28 @@
 
 #pragma once
 
-#include <gtsam_unstable/slam/PartialPriorFactor.h>
-
 #include <gtsam_unstable/dynamics/PoseRTV.h>
+#include <gtsam_unstable/slam/PartialPriorFactor.h>
 
 namespace gtsam {
 
+// Indices of relevant variables in the PoseRTV tangent vector:
+// [ rx ry rz tx ty tz vx vy vz ]
+static const size_t kRollIndex = 0;
+static const size_t kPitchIndex = 1;
+static const size_t kHeightIndex = 5;
+static const size_t kVelocityZIndex = 8;
+static const std::vector<size_t> kVelocityIndices = { 6, 7, 8 };
+
 /**
- * Forces the value of the height in a PoseRTV to a specific value
+ * Forces the value of the height (z) in a PoseRTV to a specific value.
  * Dim: 1
  */
 struct DHeightPrior : public gtsam::PartialPriorFactor<PoseRTV> {
   typedef gtsam::PartialPriorFactor<PoseRTV> Base;
+
   DHeightPrior(Key key, double height, const gtsam::SharedNoiseModel& model)
-  : Base(key, 5, height, model)  {  }
+  : Base(key, kHeightIndex, height, model)  {}
 };
 
 /**
@@ -35,11 +43,11 @@ struct DRollPrior : public gtsam::PartialPriorFactor<PoseRTV> {
 
   /** allows for explicit roll parameterization - uses canonical coordinate */
   DRollPrior(Key key, double wx, const gtsam::SharedNoiseModel& model)
-  : Base(key, 0, wx, model)  {  }
+      : Base(key, kRollIndex, wx, model)  {  }
 
   /** Forces roll to zero */
   DRollPrior(Key key, const gtsam::SharedNoiseModel& model)
-  : Base(key, 0, 0.0, model)  {  }
+      : Base(key, kRollIndex, 0.0, model)  {  }
 };
 
 /**
@@ -49,8 +57,9 @@ struct DRollPrior : public gtsam::PartialPriorFactor<PoseRTV> {
  */
 struct VelocityPrior : public gtsam::PartialPriorFactor<PoseRTV> {
   typedef gtsam::PartialPriorFactor<PoseRTV> Base;
+
   VelocityPrior(Key key, const gtsam::Vector& vel, const gtsam::SharedNoiseModel& model)
-      : Base(key, {6, 7, 8}, vel, model) {}
+      : Base(key, kVelocityIndices, vel, model) {}
 };
 
 /**
@@ -65,13 +74,14 @@ struct DGroundConstraint : public gtsam::PartialPriorFactor<PoseRTV> {
    * Primary constructor allows for variable height of the "floor"
    */
   DGroundConstraint(Key key, double height, const gtsam::SharedNoiseModel& model)
-      : Base(key, {5, 8, 0, 1}, Vector::Unit(4,0)*height, model) {}
+      : Base(key, { kHeightIndex, kVelocityZIndex, kRollIndex, kPitchIndex },
+             Vector::Unit(4, 0)*height, model) {}
 
   /**
    * Fully specify vector - use only for debugging
    */
   DGroundConstraint(Key key, const Vector& constraint, const gtsam::SharedNoiseModel& model)
-  : Base(key, {5, 8, 0, 1}, constraint, model) {
+  : Base(key, { kHeightIndex, kVelocityZIndex, kRollIndex, kPitchIndex }, constraint, model) {
     assert(constraint.size() == 4);
   }
 };

gtsam_unstable/dynamics/PoseRTV.h

@@ -80,6 +80,7 @@ public:
   using Base::Dim;
   using Base::retract;
   using Base::localCoordinates;
+  using Base::LocalCoordinates;
   /// @}
 
   /// @name measurement functions

gtsam_unstable/slam/PartialPriorFactor.h

@@ -17,8 +17,6 @@
 
 #pragma once
 
-#include <Eigen/Core>
-
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/base/Lie.h>
 
@@ -110,21 +108,21 @@ namespace gtsam {
     /** Returns a vector of errors for the measured tangent parameters.  */
     Vector evaluateError(const T& p, boost::optional<Matrix&> H = boost::none) const override {
       if (H) {
-        Matrix H_logmap;
+        Matrix H_local;
-        T::Logmap(p, H_logmap);
+        T::LocalCoordinates(p, H_local);
         (*H) = Matrix::Zero(indices_.size(), T::dimension);
         for (size_t i = 0; i < indices_.size(); ++i) {
-          (*H).row(i) = H_logmap.row(indices_.at(i));
+          (*H).row(i) = H_local.row(indices_.at(i));
         }
       }
       // Compute the tangent vector representation of T and select relevant parameters.
-      const Vector& full_logmap = T::Logmap(p);
+      const Vector& full_tangent = T::LocalCoordinates(p);
-      Vector partial_logmap = Vector::Zero(indices_.size());
+      Vector partial_tangent = Vector::Zero(indices_.size());
       for (size_t i = 0; i < indices_.size(); ++i) {
-        partial_logmap(i) = full_logmap(indices_.at(i));
+        partial_tangent(i) = full_tangent(indices_.at(i));
       }
 
-      return partial_logmap - prior_;
+      return partial_tangent - prior_;
     }
 
     // access

gtsam_unstable/slam/tests/testPartialPriorFactor.cpp

@@ -30,7 +30,6 @@ static const int kIndexTx = 3;
 static const int kIndexTy = 4;
 static const int kIndexTz = 5;
 
-
 typedef PartialPriorFactor<Pose2> TestPartialPriorFactor2;
 typedef PartialPriorFactor<Pose3> TestPartialPriorFactor3;
 typedef std::vector<size_t> Indices;