Attempt To Fix Compilation Issue with GTSAM Tag 4.3a0

gpmp2/geometry/DynamicLieTraits.h

@@ -74,6 +74,11 @@ struct DynamicLieGroupTraits {
                        ChartJacobian H = {}) {
     return m.inverse(H);
   }
+
+  static Eigen::MatrixXd AdjointMap(const Class& m) {
+    return Class::AdjointMap(m);
+  }
+
   /// @}
 };
 

gpmp2/geometry/DynamicVector.h

@@ -24,14 +24,14 @@ namespace gpmp2 {
  */
 class GPMP2_EXPORT DynamicVector {
  private:
-  size_t dim_;
-  Eigen::VectorXd vector_;
+  size_t dim_{0};
+  Eigen::VectorXd vector_{};
 
  public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
   /// Default constructor
-  DynamicVector() {}
+  DynamicVector() : dim_(0), vector_() {}
 
   // eigen constructor
   // non-explicit conversion enabled
@@ -89,6 +89,7 @@ class GPMP2_EXPORT DynamicVector {
 
   // equals
   bool equals(const DynamicVector& m2, double tol) const {
+    if(dim_ != m2.dim_) return false;
     return gtsam::equal_with_abs_tol(this->vector_, m2.vector_, tol);
   }
 

gpmp2/geometry/ProductDynamicLieGroup.h

@@ -20,8 +20,8 @@ namespace gpmp2 {
 template <typename G, typename H>
 class ProductDynamicLieGroup : public std::pair<G, H> {
  private:
-  GTSAM_CONCEPT_ASSERT((gtsam::IsLieGroup<G>));
-  GTSAM_CONCEPT_ASSERT((gtsam::IsLieGroup<H>));
+  static_assert(gtsam::IsLieGroup<G>::value, "G must satisfy the LieGroup concept");
+  static_assert(gtsam::IsLieGroup<H>::value, "H must satisfy the LieGroup concept");
   typedef std::pair<G, H> Base;
 
  protected:
@@ -241,6 +241,19 @@ class ProductDynamicLieGroup : public std::pair<G, H> {
     }
   }
 
+  static Eigen::MatrixXd AdjointMap(const ProductDynamicLieGroup& m) {
+    const size_t d1 = m.dim1();
+    const size_t d2 = m.dim2();
+    const size_t d  = d1 + d2;
+
+    Eigen::MatrixXd Adj = Eigen::MatrixXd::Zero(d, d);
+
+    Adj.topLeftCorner(d1, d1) = gtsam::traits<G>::AdjointMap(m.first);
+    Adj.bottomRightCorner(d2, d2) = gtsam::traits<H>::AdjointMap(m.second);
+
+    return Adj;
+  }
+
   ProductDynamicLieGroup expmap(const TangentVector& v) const {
     return compose(ProductDynamicLieGroup::Expmap(v));
   }

gpmp2/geometry/tests/testDynamicVector.cpp

@@ -25,10 +25,11 @@ GTSAM_CONCEPT_LIE_INST(DynamicVector)
 
 /* ************************************************************************** */
 TEST(DynamicVector, Concept) {
-  GTSAM_CONCEPT_ASSERT((IsGroup<DynamicVector>));
-  GTSAM_CONCEPT_ASSERT((IsManifold<DynamicVector>));
-  GTSAM_CONCEPT_ASSERT((IsLieGroup<DynamicVector>));
-  GTSAM_CONCEPT_ASSERT((IsVectorSpace<DynamicVector>));
+  static_assert(IsManifold<DynamicVector>::value,    "DynamicVector must satisfy the Manifold concept");
+  static_assert(IsVectorSpace<DynamicVector>::value, "DynamicVector must satisfy the VectorSpace concept");
+  static_assert(IsLieGroup<DynamicVector>::value,    "DynamicVector must satisfy the LieGroup concept");
+  // TODO : IsGroup<DynamicVector>::value does not exist in GTSAM
+  // static_assert(IsGroup<DynamicVector>::value, "DynamicVector must satisfy the Group concept");
 }
 
 /* ************************************************************************** */

gpmp2/geometry/tests/testPose2Vector.cpp

@@ -22,9 +22,10 @@ using namespace gpmp2;
 
 /* ************************************************************************** */
 TEST(Pose2Vector, Lie) {
-  GTSAM_CONCEPT_ASSERT((IsGroup<Pose2Vector>));
-  GTSAM_CONCEPT_ASSERT((IsManifold<Pose2Vector>));
-  GTSAM_CONCEPT_ASSERT((IsLieGroup<Pose2Vector>));
+    // TODO : IsGroup<DynamicVector>::value does not exist in GTSAM
+  // static_assert(IsGroup<DynamicVector>::value, "DynamicVector must satisfy the Group concept");
+  static_assert(IsManifold<Pose2Vector>::value, "Pose2Vector must satisfy the Manifold concept");
+  static_assert(IsLieGroup<Pose2Vector>::value, "DynamicVector must satisfy the LieGroup concept");
 }
 
 /* ************************************************************************** */

gpmp2/geometry/tests/testProductDynamicLieGroup.cpp

@@ -47,9 +47,10 @@ struct traits<Product> : internal::DynamicLieGroupTraits<Product> {
 
 //******************************************************************************
 TEST(ProductDynamicLieGroup, ProductLieGroup) {
-  GTSAM_CONCEPT_ASSERT((IsGroup<Product>));
-  GTSAM_CONCEPT_ASSERT((IsManifold<Product>));
-  GTSAM_CONCEPT_ASSERT((IsLieGroup<Product>));
+  // TODO : IsGroup<DynamicVector>::value does not exist in GTSAM
+  // static_assert(IsGroup<DynamicVector>::value, "DynamicVector must satisfy the Group concept");
+  static_assert(IsManifold<Product>::value, "Product must satisfy the Manifold concept");
+  static_assert(IsLieGroup<Product>::value, "Product must satisfy the IsLieGroup concept");
   Product pair1(Point2(0, 0), Pose2());
   Vector5 d;
   d << 1, 2, 0.1, 0.2, 0.3;

gpmp2/gp/GaussianProcessInterpolatorLie.h

@@ -26,7 +26,7 @@ namespace gpmp2 {
 template <typename T>
 class GaussianProcessInterpolatorLie {
  private:
-  GTSAM_CONCEPT_ASSERT((gtsam::IsLieGroup<T>));
+  static_assert(gtsam::IsLieGroup<T>::value, "T must satisfy the LieGroup concept");
   typedef GaussianProcessInterpolatorLie<T> This;
 
   size_t dof_;

gpmp2/gp/GaussianProcessPriorLie.h

@@ -24,7 +24,7 @@ template <typename T>
 class GaussianProcessPriorLie
     : public gtsam::NoiseModelFactorN<T, gtsam::Vector, T, gtsam::Vector> {
  private:
-  GTSAM_CONCEPT_ASSERT((gtsam::IsLieGroup<T>));
+  static_assert(gtsam::IsLieGroup<T>::value, "T must satisfy the LieGroup concept");
   typedef GaussianProcessPriorLie<T> This;
   typedef gtsam::NoiseModelFactorN<T, gtsam::Vector, T, gtsam::Vector> Base;