variable dimension product groups

gtsam/base/Manifold.h

@@ -74,11 +74,11 @@ template<class Class, int N>
 struct GetDimensionImpl {
   // Get dimension at compile-time for fixed-size manifolds, and at
   // run-time for dynamic-size manifolds.
-  static int GetDimension(const Class& m) {
+  static size_t GetDimension(const Class& m) {
     if constexpr (N == Eigen::Dynamic) {
       return m.dim();
     } else {
-      return N;
+      return static_cast<size_t>(N);
     }
   }
 };

gtsam/base/VectorSpace.h

@@ -29,7 +29,7 @@ struct VectorSpaceImpl {
   typedef Eigen::Matrix<double, N, 1> TangentVector;
   typedef OptionalJacobian<N, N> ChartJacobian;
   typedef Eigen::Matrix<double, N, N> Jacobian;
-  static int GetDimension(const Class&) { return N;}
+  static size_t GetDimension(const Class&) { return static_cast<size_t>(N);}
 
   static TangentVector Local(const Class& origin, const Class& other,
       ChartJacobian H1 = {}, ChartJacobian H2 = {}) {
@@ -107,10 +107,10 @@ struct VectorSpaceImpl<Class,Eigen::Dynamic> {
   /// @{
   typedef Eigen::VectorXd TangentVector;
   typedef OptionalJacobian<Eigen::Dynamic,Eigen::Dynamic> ChartJacobian;
-  static int GetDimension(const Class& m) { return m.dim();}
+  static size_t GetDimension(const Class& m) { return m.dim();}
 
   static Eigen::MatrixXd Eye(const Class& m) {
-    int dim = GetDimension(m);
+    size_t dim = GetDimension(m);
     return Eigen::MatrixXd::Identity(dim, dim);
   }
 
@@ -395,12 +395,12 @@ struct DynamicTraits {
   typedef OptionalJacobian<dimension, dimension> ChartJacobian;
   typedef Dynamic ManifoldType;
 
-  static int GetDimension(const Dynamic& m) {
-    return m.rows() * m.cols();
+  static size_t GetDimension(const Dynamic& m) {
+    return static_cast<size_t>(m.rows() * m.cols());
   }
 
   static Jacobian Eye(const Dynamic& m) {
-    int dim = GetDimension(m);
+    size_t dim = GetDimension(m);
     return Eigen::Matrix<double, dimension, dimension>::Identity(dim, dim);
   }
 
@@ -432,9 +432,23 @@ struct DynamicTraits {
     return result;
   }
 
-  static Dynamic Expmap(const TangentVector& /*v*/, ChartJacobian H = {}) {
-    static_cast<void>(H);
-    throw std::runtime_error("Expmap not defined for dynamic types");
+  static Dynamic Expmap(const TangentVector& v, ChartJacobian H = {}) {
+    if constexpr (M == Eigen::Dynamic && N == Eigen::Dynamic) {
+      static_cast<void>(v);
+      static_cast<void>(H);
+      throw std::runtime_error("Expmap not defined for fully dynamic matrices");
+    } else {
+      const int rows = (M == Eigen::Dynamic) ? v.size() / N : M;
+      const int cols = (N == Eigen::Dynamic) ? v.size() / M : N;
+      if (rows * cols != v.size()) {
+        throw std::invalid_argument(
+            "Dynamic Expmap tangent dimension does not match matrix shape");
+      }
+      Dynamic result(rows, cols);
+      Eigen::Map<Dynamic>(result.data(), rows, cols) = v;
+      if (H) *H = Jacobian::Identity(v.size(), v.size());
+      return result;
+    }
   }
 
   static Dynamic Inverse(const Dynamic& m, ChartJacobian H = {}) {

gtsam/linear/NoiseModel.h

@@ -160,8 +160,7 @@ namespace gtsam {
       static_assert(IsManifold<T>::value,
                     "noiseModel::matchesDimension requires a manifold type.");
       if constexpr (traits<T>::dimension == Eigen::Dynamic) {
-        return model.dim() ==
-               static_cast<size_t>(traits<T>::GetDimension(measured));
+        return model.dim() == traits<T>::GetDimension(measured);
       } else {
         return model.dim() == static_cast<size_t>(traits<T>::dimension);
       }
@@ -673,7 +672,7 @@ namespace gtsam {
         static_assert(IsManifold<T>::value,
                       "noiseModel::Unit::Create requires a manifold type.");
         if constexpr (traits<T>::dimension == Eigen::Dynamic) {
-          return Create(static_cast<size_t>(traits<T>::GetDimension(measured)));
+          return Create(traits<T>::GetDimension(measured));
         } else {
           static const shared_ptr kDefault =
               Create(static_cast<size_t>(traits<T>::dimension));

gtsam/navigation/ManifoldEKF.h

@@ -73,7 +73,7 @@ class ManifoldEKF {
     n_ = traits<M>::GetDimension(X0);
     if constexpr (Dim == Eigen::Dynamic) {
       // Validate dimensions of initial covariance P0.
-      if (P0.rows() != n_ || P0.cols() != n_) {
+      if (!isMatrixOfSize(P0, n_, n_)) {
         throw std::invalid_argument(
             "ManifoldEKF: Initial covariance P0 dimensions (" +
             std::to_string(P0.rows()) + "x" + std::to_string(P0.cols()) +
@@ -97,7 +97,7 @@ class ManifoldEKF {
   const Covariance& covariance() const { return P_; }
 
   /// @return runtime dimension of the manifold.
-  int dimension() const { return n_; }
+  size_t dimension() const { return n_; }
 
   /**
    * Basic predict step: Updates state and covariance given the predicted next
@@ -115,8 +115,7 @@ class ManifoldEKF {
    */
   void predict(const M& X_next, const Jacobian& F, const Covariance& Q) {
     if constexpr (Dim == Eigen::Dynamic) {
-      if (F.rows() != n_ || F.cols() != n_ || Q.rows() != n_ ||
-          Q.cols() != n_) {
+      if (!isMatrixOfSize(F, n_, n_) || !isMatrixOfSize(Q, n_, n_)) {
         throw std::invalid_argument(
             "ManifoldEKF::predict: Dynamic F/Q dimensions must match state "
             "dimension " +
@@ -257,29 +256,37 @@ class ManifoldEKF {
   /// Validate inputs to update.
   void validateInputs(const gtsam::Vector& prediction, const Matrix& H,
                       const gtsam::Vector& z, const Matrix& R) {
-    const int m = static_cast<int>(prediction.size());
-    if (static_cast<int>(z.size()) != m) {
+    const size_t m = static_cast<size_t>(prediction.size());
+    if (static_cast<size_t>(z.size()) != m) {
       throw std::invalid_argument(
           "ManifoldEKF::updateWithVector: prediction and z must have same "
           "length.");
     }
-    if (H.rows() != m || H.cols() != n_) {
+    if (!isMatrixOfSize(H, m, n_)) {
       throw std::invalid_argument(
           "ManifoldEKF::updateWithVector: H must be m x n where m = "
           "measurement size and n = state dimension.");
     }
-    if (R.rows() != m || R.cols() != m) {
+    if (!isMatrixOfSize(R, m, m)) {
       throw std::invalid_argument(
           "ManifoldEKF::updateWithVector: R must be m x m where m = "
           "measurement size.");
     }
   }
 
+  /// Check whether a matrix has the expected runtime dimensions.
+  template <typename MatrixType>
+  static bool isMatrixOfSize(const MatrixType& matrix, size_t rows,
+                             size_t cols) {
+    return static_cast<size_t>(matrix.rows()) == rows &&
+           static_cast<size_t>(matrix.cols()) == cols;
+  }
+
  protected:
   M X_;           ///< Manifold state estimate.
   Covariance P_;  ///< Covariance (Eigen::Matrix<double, Dim, Dim>).
   Jacobian I_;    ///< Identity matrix sized to the state dimension.
-  int n_;         ///< Runtime tangent space dimension of M.
+  size_t n_;      ///< Runtime tangent space dimension of M.
 
  private:
   // Detection helper: check if traits<T>::Retract(x, v, Jacobian*) is valid.

gtsam/navigation/navigation.i

@@ -676,7 +676,7 @@ virtual class ManifoldEKF {
   // Accessors
   M state() const;
   gtsam::Matrix covariance() const;
-  int dimension() const;
+  size_t dimension() const;
 
   // Predict with provided next state and Jacobian
   void predict(const M& X_next, gtsam::Matrix F, gtsam::Matrix Q);

gtsam/nonlinear/ExtendedKalmanFilter-inl.h

@@ -70,7 +70,7 @@ namespace gtsam {
     // Create a Jacobian Prior Factor directly P_initial.
     // Since x0 is set to the provided mean, the b vector in the prior will be zero
     // TODO(Frank): is there a reason why noiseModel is not simply P_initial?
-    int n = traits<T>::GetDimension(x_initial);
+    size_t n = traits<T>::GetDimension(x_initial);
     priorFactor_ = JacobianFactor::shared_ptr(
         new JacobianFactor(key_initial, P_initial->R(), Vector::Zero(n),
             noiseModel::Unit::Create(n)));

gtsam/slam/PoseTranslationPrior.h

@@ -66,8 +66,8 @@ class PoseTranslationPrior : public NoiseModelFactorN<POSE> {
   Vector evaluateError(const Pose& pose, OptionalMatrixType H) const override {
     const Translation& newTrans = pose.translation();
     const Rotation& R = pose.rotation();
-    const int tDim = traits<Translation>::GetDimension(newTrans);
-    const int xDim = traits<Pose>::GetDimension(pose);
+    const size_t tDim = traits<Translation>::GetDimension(newTrans);
+    const size_t xDim = traits<Pose>::GetDimension(pose);
     if (H) {
       *H = Matrix::Zero(tDim, xDim);
       std::pair<size_t, size_t> transInterval = POSE::translationInterval();
@@ -109,4 +109,3 @@ class PoseTranslationPrior : public NoiseModelFactorN<POSE> {
 
 
 
-

tests/testNonlinearOptimizer.cpp

@@ -658,7 +658,7 @@ struct traits<MyType> {
     return (a - b).array().abs().maxCoeff() < tol;
   }
   static void Print(const MyType&, const string&) {}
-  static int GetDimension(const MyType&) { return dimension; }
+  static size_t GetDimension(const MyType&) { return dimension; }
   static MyType Retract(const MyType& a, const TangentVector& v,
                         ChartJacobian H1 = {}, ChartJacobian H2 = {}) {
     if (H1) *H1 = Matrix3::Identity();