1. remove LieVector in IMUKittiExampleGPS.m 2. Add tests the priorFactor in matlab 3. template substition tests in testsClass.cpp

gtsam.h

@@ -2153,7 +2153,7 @@ class NonlinearISAM {
 #include <gtsam/geometry/StereoPoint2.h>
 
 #include <gtsam/slam/PriorFactor.h>
-template<T = { gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
+template<T = { Vector, Matrix, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
 virtual class PriorFactor : gtsam::NoiseModelFactor {
   PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
   T prior() const;

matlab/gtsam_examples/IMUKittiExampleGPS.m

@@ -33,7 +33,7 @@ GPSskip = 10; % Skip this many GPS measurements each time
 
 %% Get initial conditions for the estimated trajectory
 currentPoseGlobal = Pose3(Rot3, GPS_data(firstGPSPose).Position); % initial pose is the reference frame (navigation frame)
-currentVelocityGlobal = LieVector([0;0;0]); % the vehicle is stationary at the beginning
+currentVelocityGlobal = [0;0;0]; % the vehicle is stationary at the beginning
 currentBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1));
 sigma_init_x = noiseModel.Isotropic.Precisions([ 0.0; 0.0; 0.0; 1; 1; 1 ]);
 sigma_init_v = noiseModel.Isotropic.Sigma(3, 1000.0);
@@ -72,7 +72,7 @@ for measurementIndex = firstGPSPose:length(GPS_data)
     newValues.insert(currentVelKey, currentVelocityGlobal);
     newValues.insert(currentBiasKey, currentBias);
     newFactors.add(PriorFactorPose3(currentPoseKey, currentPoseGlobal, sigma_init_x));
-    newFactors.add(PriorFactorLieVector(currentVelKey, currentVelocityGlobal, sigma_init_v));
+    newFactors.add(PriorFactorVector(currentVelKey, currentVelocityGlobal, sigma_init_v));
     newFactors.add(PriorFactorConstantBias(currentBiasKey, currentBias, sigma_init_b));
   else
     t_previous = GPS_data(measurementIndex-1, 1).Time;

matlab/gtsam_tests/testPriorFactor.m

@@ -0,0 +1,18 @@
+% test wrapping of Values
+import gtsam.*;
+
+values = Values;
+
+key = 5; 
+priorPose3 = Pose3;
+model = noiseModel.Unit.Create(6);
+factor = PriorFactorPose3(key, priorPose3, model);
+values.insert(key, priorPose3);
+EXPECT('error', factor.error(values) == 0);
+
+key = 3; 
+priorVector = [0,0,0]';
+model = noiseModel.Unit.Create(3);
+factor = PriorFactorVector(key, priorVector, model);
+values.insert(key, priorVector);
+EXPECT('error', factor.error(values) == 0);

matlab/gtsam_tests/test_gtsam.m

@@ -1,5 +1,8 @@
 % Test runner script - runs each test 
 
+display 'Starting: testPriorFactor'
+testPriorFactor
+
 display 'Starting: testValues'
 testValues
 

wrap/matlab.h

@@ -425,3 +425,20 @@ boost::shared_ptr<Class> unwrap_shared_ptr(const mxArray* obj, const string& pro
   boost::shared_ptr<Class>* spp = *reinterpret_cast<boost::shared_ptr<Class>**> (mxGetData(mxh));
   return *spp;
 }
+
+// throw an error if unwrap_shared_ptr is attempted for an Eigen Vector
+template <>
+Vector unwrap_shared_ptr<Vector>(const mxArray* obj, const string& propertyName) {
+  bool unwrap_shared_ptr_Vector_attempted = false;
+  BOOST_STATIC_ASSERT(unwrap_shared_ptr_Vector_attempted, "Vector cannot be unwrapped as a shared pointer");
+  return Vector();
+}
+
+// throw an error if unwrap_shared_ptr is attempted for an Eigen Matrix
+template <>
+Matrix unwrap_shared_ptr<Matrix>(const mxArray* obj, const string& propertyName) {
+  bool unwrap_shared_ptr_Matrix_attempted = false;
+  BOOST_STATIC_ASSERT(unwrap_shared_ptr_Matrix_attempted, "Matrix cannot be unwrapped as a shared pointer");
+  return Matrix();
+}
+

wrap/tests/testClass.cpp

@@ -160,6 +160,63 @@ TEST( Class, Grammar ) {
   EXPECT_LONGS_EQUAL(ReturnType::EIGEN, rv4.type1.category);
 }
 
+
+//******************************************************************************
+TEST( Class, TemplateSubstition ) {
+
+  using classic::space_p;
+
+  // Create type grammar that will place result in cls
+  Class cls;
+  Template t;
+  ClassGrammar g(cls, t);
+
+  string markup(
+      string("template<T = {void, double, Matrix, Point3}> class Point2 {                \n")
+          + string(" T myMethod(const T& t) const;   \n") 
+          + string("};"));
+
+  EXPECT(parse(markup.c_str(), g, space_p).full);
+
+  // Method 2
+  Method m2 = cls.method("myMethod");
+  EXPECT(assert_equal("myMethod", m2.name()));
+  EXPECT(m2.isConst());
+  LONGS_EQUAL(1, m2.nrOverloads());
+
+  ReturnValue rv2 = m2.returnValue(0);
+  EXPECT(!rv2.isPair);
+  EXPECT(!rv2.type1.isPtr);
+  EXPECT(assert_equal("T", rv2.type1.name()));
+  EXPECT_LONGS_EQUAL(ReturnType::CLASS, rv2.type1.category);
+
+  EXPECT_LONGS_EQUAL(4, t.nrValues());
+  EXPECT(t.argName()=="T");
+  EXPECT(t[0]==Qualified("void",Qualified::VOID));
+  EXPECT(t[1]==Qualified("double",Qualified::BASIS));
+  EXPECT(t[2]==Qualified("Matrix",Qualified::EIGEN));
+  EXPECT(t[3]==Qualified("Point3",Qualified::CLASS));
+
+  vector<Class> classes = cls.expandTemplate(t.argName(),
+    t.argValues());
+
+  // check the number of new classes is four
+  EXPECT_LONGS_EQUAL(4, classes.size());
+  
+  // check return types 
+  EXPECT(classes[0].method("myMethod").returnValue(0).type1 == Qualified("void",Qualified::VOID));
+  EXPECT(classes[1].method("myMethod").returnValue(0).type1 == Qualified("double",Qualified::BASIS));
+  EXPECT(classes[2].method("myMethod").returnValue(0).type1 == Qualified("Matrix",Qualified::EIGEN));
+  EXPECT(classes[3].method("myMethod").returnValue(0).type1 == Qualified("Point3",Qualified::CLASS));
+
+  // check the argument types
+  EXPECT(classes[0].method("myMethod").argumentList(0)[0].type == Qualified("void",Qualified::VOID));
+  EXPECT(classes[1].method("myMethod").argumentList(0)[0].type == Qualified("double",Qualified::BASIS));
+  EXPECT(classes[2].method("myMethod").argumentList(0)[0].type == Qualified("Matrix",Qualified::EIGEN));
+  EXPECT(classes[3].method("myMethod").argumentList(0)[0].type == Qualified("Point3",Qualified::CLASS));
+
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;