From e49c9fa1005494dd137102d9f85d2b500847550b Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Thu, 4 Dec 2014 13:28:20 -0500 Subject: [PATCH] 1. remove LieVector in IMUKittiExampleGPS.m 2. Add tests the priorFactor in matlab 3. template substition tests in testsClass.cpp --- gtsam.h | 2 +- matlab/gtsam_examples/IMUKittiExampleGPS.m | 4 +- matlab/gtsam_tests/testPriorFactor.m | 18 +++++++ matlab/gtsam_tests/test_gtsam.m | 3 ++ wrap/matlab.h | 17 +++++++ wrap/tests/testClass.cpp | 57 ++++++++++++++++++++++ 6 files changed, 98 insertions(+), 3 deletions(-) create mode 100644 matlab/gtsam_tests/testPriorFactor.m diff --git a/gtsam.h b/gtsam.h index 96d51117a..6d75bb114 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2153,7 +2153,7 @@ class NonlinearISAM { #include #include -template +template virtual class PriorFactor : gtsam::NoiseModelFactor { PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); T prior() const; diff --git a/matlab/gtsam_examples/IMUKittiExampleGPS.m b/matlab/gtsam_examples/IMUKittiExampleGPS.m index e205d918c..32f61e47f 100644 --- a/matlab/gtsam_examples/IMUKittiExampleGPS.m +++ b/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; diff --git a/matlab/gtsam_tests/testPriorFactor.m b/matlab/gtsam_tests/testPriorFactor.m new file mode 100644 index 000000000..4d567a6ce --- /dev/null +++ b/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); diff --git a/matlab/gtsam_tests/test_gtsam.m b/matlab/gtsam_tests/test_gtsam.m index e08019610..669ee7530 100644 --- a/matlab/gtsam_tests/test_gtsam.m +++ b/matlab/gtsam_tests/test_gtsam.m @@ -1,5 +1,8 @@ % Test runner script - runs each test +display 'Starting: testPriorFactor' +testPriorFactor + display 'Starting: testValues' testValues diff --git a/wrap/matlab.h b/wrap/matlab.h index 23f391903..f2080fab3 100644 --- a/wrap/matlab.h +++ b/wrap/matlab.h @@ -425,3 +425,20 @@ boost::shared_ptr unwrap_shared_ptr(const mxArray* obj, const string& pro boost::shared_ptr* spp = *reinterpret_cast**> (mxGetData(mxh)); return *spp; } + +// throw an error if unwrap_shared_ptr is attempted for an Eigen Vector +template <> +Vector unwrap_shared_ptr(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(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(); +} + diff --git a/wrap/tests/testClass.cpp b/wrap/tests/testClass.cpp index 756b6668d..f7d9a3ade 100644 --- a/wrap/tests/testClass.cpp +++ b/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 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 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;