We will not wrap fixed Vectors and Matrices individually. Methods that take those can just as well be wrapped with Vector or Matrix. Note const and & are ignored for those. I added a small method, insertFixed, that allows one to put in small fixed size matrices for optimization. Did not do retrieval yet.

gtsam.h

@@ -156,12 +156,6 @@ virtual class Value {
   size_t dim() const;
 };
 
-class Vector3 {
-  Vector3(Vector v);
-};
-class Vector6 {
-  Vector6(Vector v);
-};
 #include <gtsam/base/LieScalar.h>
 class LieScalar {
   // Standard constructors
@@ -1723,6 +1717,7 @@ class Values {
   // void insert(size_t j, const gtsam::Value& value);
   // void update(size_t j, const gtsam::Value& val);
   // gtsam::Value at(size_t j) const;
+
   void insert(size_t j, const gtsam::Point2& t);
   void insert(size_t j, const gtsam::Point3& t);
   void insert(size_t j, const gtsam::Rot2& t);
@@ -1737,6 +1732,9 @@ class Values {
   void insert(size_t j, Vector t);
   void insert(size_t j, Matrix t);
 
+  // Fixed size version
+  void insertFixed(size_t j, Vector t, size_t n);
+
   void update(size_t j, const gtsam::Point2& t);
   void update(size_t j, const gtsam::Point3& t);
   void update(size_t j, const gtsam::Rot2& t);
@@ -2394,7 +2392,7 @@ class ConstantBias {
 
 #include <gtsam/navigation/ImuFactor.h>
 class PoseVelocity{
-    PoseVelocity(const gtsam::Pose3& pose, const gtsam::Vector3 velocity);
+    PoseVelocity(const gtsam::Pose3& pose, Vector velocity);
   };
 class ImuFactorPreintegratedMeasurements {
   // Standard Constructor
@@ -2433,7 +2431,7 @@ virtual class ImuFactor : gtsam::NonlinearFactor {
       const gtsam::Pose3& body_P_sensor);
   // Standard Interface
   gtsam::ImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
-  gtsam::PoseVelocity Predict(const gtsam::Pose3& pose_i, const gtsam::Vector3& vel_i, const gtsam::imuBias::ConstantBias& bias,
+  gtsam::PoseVelocity Predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias,
       const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements,
       Vector gravity, Vector omegaCoriolis) const;
 };
@@ -2479,7 +2477,7 @@ virtual class AHRSFactor : gtsam::NonlinearFactor {
 
 #include <gtsam/navigation/CombinedImuFactor.h>
 class PoseVelocityBias{
-    PoseVelocityBias(const gtsam::Pose3& pose, const gtsam::Vector3 velocity, const gtsam::imuBias::ConstantBias& bias);
+    PoseVelocityBias(const gtsam::Pose3& pose, Vector velocity, const gtsam::imuBias::ConstantBias& bias);
   };
 class CombinedImuFactorPreintegratedMeasurements {
   // Standard Constructor
@@ -2530,7 +2528,7 @@ virtual class CombinedImuFactor : gtsam::NonlinearFactor {
 
   // Standard Interface
   gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
-  gtsam::PoseVelocityBias Predict(const gtsam::Pose3& pose_i, const gtsam::Vector3& vel_i, const gtsam::imuBias::ConstantBias& bias_i,
+  gtsam::PoseVelocityBias Predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias_i,
       const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements,
       Vector gravity, Vector omegaCoriolis);
 };

gtsam/nonlinear/Values.cpp

@@ -130,6 +130,24 @@ namespace gtsam {
       throw ValuesKeyAlreadyExists(j);
   }
 
+  /* ************************************************************************* */
+  void Values::insertFixed(Key j, const Vector& v, size_t n) {
+    switch (n) {
+    case 1: insert<Vector1>(j,v); break;
+    case 2: insert<Vector2>(j,v); break;
+    case 3: insert<Vector3>(j,v); break;
+    case 4: insert<Vector4>(j,v); break;
+    case 5: insert<Vector5>(j,v); break;
+    case 6: insert<Vector6>(j,v); break;
+    case 7: insert<Vector7>(j,v); break;
+    case 8: insert<Vector8>(j,v); break;
+    case 9: insert<Vector9>(j,v); break;
+    default:
+      throw runtime_error(
+          "Values::insert fixed size can only handle n in 1..9");
+    }
+  }
+
   /* ************************************************************************* */
   void Values::insert(const Values& values) {
     for(const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) {

gtsam/nonlinear/Values.h

@@ -258,6 +258,10 @@ namespace gtsam {
     template <typename ValueType>
     void insert(Key j, const ValueType& val);
 
+    /// Special version for small fixed size vectors, for matlab/python
+    /// throws truntime error if n<1 || n>9
+    void insertFixed(Key j, const Vector& v, size_t n);
+
     /// overloaded insert version that also specifies a chart
     template <typename ValueType, typename Chart>
     void insert(Key j, const ValueType& val);

gtsam/nonlinear/tests/testValues.cpp

@@ -470,6 +470,15 @@ TEST(Values, Destructors) {
   LONGS_EQUAL(4+2, (long)TestValueData::DestructorCount);
 }
 
+/* ************************************************************************* */
+TEST(Values, FixedSize) {
+  Values values;
+  Vector v(3); v << 5.0, 6.0, 7.0;
+  values.insertFixed(key1, v, 3);
+  Vector3 expected(5.0, 6.0, 7.0);
+  CHECK(assert_equal((Vector)expected, values.at<Vector3>(key1)));
+  CHECK_EXCEPTION(values.insertFixed(key1, v, 12),runtime_error);
+}
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */

gtsam_unstable/nonlinear/ExpressionFactor.h

@@ -32,6 +32,8 @@ namespace gtsam {
 template<class T>
 class ExpressionFactor: public NoiseModelFactor {
 
+protected:
+
   T measurement_; ///< the measurement to be compared with the expression
   Expression<T> expression_; ///< the expression that is AD enabled
   FastVector<int> dims_; ///< dimensions of the Jacobian matrices