Merge pull request #1136 from borglab/fix/wrap-parametermatrix

Wrap ParameterMatrix as argument for Values
borglab · Mar 19, 2022 · 2de090f · 2de090f
2 parents e5fb4cd + e271a8a
commit 2de090f
Showing 1 changed file with 62 additions and 1 deletion.
diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i
@@ -25,6 +25,7 @@ namespace gtsam {
 #include <gtsam/geometry/Unit3.h>
 #include <gtsam/navigation/ImuBias.h>
 #include <gtsam/navigation/NavState.h>
+#include <gtsam/basis/ParameterMatrix.h>
 
 #include <gtsam/nonlinear/GraphvizFormatting.h>
 class GraphvizFormatting : gtsam::DotWriter {
@@ -228,6 +229,21 @@ class Values {
   void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
   void insert(size_t j, const gtsam::NavState& nav_state);
   void insert(size_t j, double c);
+  void insert(size_t j, const gtsam::ParameterMatrix<1>& X);
+  void insert(size_t j, const gtsam::ParameterMatrix<2>& X);
+  void insert(size_t j, const gtsam::ParameterMatrix<3>& X);
+  void insert(size_t j, const gtsam::ParameterMatrix<4>& X);
+  void insert(size_t j, const gtsam::ParameterMatrix<5>& X);
+  void insert(size_t j, const gtsam::ParameterMatrix<6>& X);
+  void insert(size_t j, const gtsam::ParameterMatrix<7>& X);
+  void insert(size_t j, const gtsam::ParameterMatrix<8>& X);
+  void insert(size_t j, const gtsam::ParameterMatrix<9>& X);
+  void insert(size_t j, const gtsam::ParameterMatrix<10>& X);
+  void insert(size_t j, const gtsam::ParameterMatrix<11>& X);
+  void insert(size_t j, const gtsam::ParameterMatrix<12>& X);
+  void insert(size_t j, const gtsam::ParameterMatrix<13>& X);
+  void insert(size_t j, const gtsam::ParameterMatrix<14>& X);
+  void insert(size_t j, const gtsam::ParameterMatrix<15>& X);
 
   void update(size_t j, const gtsam::Point2& point2);
   void update(size_t j, const gtsam::Point3& point3);
@@ -254,6 +270,21 @@ class Values {
   void update(size_t j, Vector vector);
   void update(size_t j, Matrix matrix);
   void update(size_t j, double c);
+  void update(size_t j, const gtsam::ParameterMatrix<1>& X);
+  void update(size_t j, const gtsam::ParameterMatrix<2>& X);
+  void update(size_t j, const gtsam::ParameterMatrix<3>& X);
+  void update(size_t j, const gtsam::ParameterMatrix<4>& X);
+  void update(size_t j, const gtsam::ParameterMatrix<5>& X);
+  void update(size_t j, const gtsam::ParameterMatrix<6>& X);
+  void update(size_t j, const gtsam::ParameterMatrix<7>& X);
+  void update(size_t j, const gtsam::ParameterMatrix<8>& X);
+  void update(size_t j, const gtsam::ParameterMatrix<9>& X);
+  void update(size_t j, const gtsam::ParameterMatrix<10>& X);
+  void update(size_t j, const gtsam::ParameterMatrix<11>& X);
+  void update(size_t j, const gtsam::ParameterMatrix<12>& X);
+  void update(size_t j, const gtsam::ParameterMatrix<13>& X);
+  void update(size_t j, const gtsam::ParameterMatrix<14>& X);
+  void update(size_t j, const gtsam::ParameterMatrix<15>& X);
 
   void insert_or_assign(size_t j, const gtsam::Point2& point2);
   void insert_or_assign(size_t j, const gtsam::Point3& point3);
@@ -280,6 +311,21 @@ class Values {
   void insert_or_assign(size_t j, Vector vector);
   void insert_or_assign(size_t j, Matrix matrix);
   void insert_or_assign(size_t j, double c);
+  void insert_or_assign(size_t j, const gtsam::ParameterMatrix<1>& X);
+  void insert_or_assign(size_t j, const gtsam::ParameterMatrix<2>& X);
+  void insert_or_assign(size_t j, const gtsam::ParameterMatrix<3>& X);
+  void insert_or_assign(size_t j, const gtsam::ParameterMatrix<4>& X);
+  void insert_or_assign(size_t j, const gtsam::ParameterMatrix<5>& X);
+  void insert_or_assign(size_t j, const gtsam::ParameterMatrix<6>& X);
+  void insert_or_assign(size_t j, const gtsam::ParameterMatrix<7>& X);
+  void insert_or_assign(size_t j, const gtsam::ParameterMatrix<8>& X);
+  void insert_or_assign(size_t j, const gtsam::ParameterMatrix<9>& X);
+  void insert_or_assign(size_t j, const gtsam::ParameterMatrix<10>& X);
+  void insert_or_assign(size_t j, const gtsam::ParameterMatrix<11>& X);
+  void insert_or_assign(size_t j, const gtsam::ParameterMatrix<12>& X);
+  void insert_or_assign(size_t j, const gtsam::ParameterMatrix<13>& X);
+  void insert_or_assign(size_t j, const gtsam::ParameterMatrix<14>& X);
+  void insert_or_assign(size_t j, const gtsam::ParameterMatrix<15>& X);
 
   template <T = {gtsam::Point2,
                  gtsam::Point3,
@@ -305,7 +351,22 @@ class Values {
                  gtsam::NavState,
                  Vector,
                  Matrix,
-                 double}>
+                 double,
+                 gtsam::ParameterMatrix<1>,
+                 gtsam::ParameterMatrix<2>,
+                 gtsam::ParameterMatrix<3>,
+                 gtsam::ParameterMatrix<4>,
+                 gtsam::ParameterMatrix<5>,
+                 gtsam::ParameterMatrix<6>,
+                 gtsam::ParameterMatrix<7>,
+                 gtsam::ParameterMatrix<8>,
+                 gtsam::ParameterMatrix<9>,
+                 gtsam::ParameterMatrix<10>,
+                 gtsam::ParameterMatrix<11>,
+                 gtsam::ParameterMatrix<12>,
+                 gtsam::ParameterMatrix<13>,
+                 gtsam::ParameterMatrix<14>,
+                 gtsam::ParameterMatrix<15>}>
   T at(size_t j);
 };