diff --git a/gtsam.h b/gtsam.h index 56bf378b5..37040234d 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1728,6 +1728,7 @@ class Values { void insert(size_t j, const gtsam::Cal3DS2& t); void insert(size_t j, const gtsam::Cal3Bundler& t); void insert(size_t j, const gtsam::EssentialMatrix& t); + void insert(size_t j, const gtsam::SimpleCamera& t); void insert(size_t j, const gtsam::imuBias::ConstantBias& t); void insert(size_t j, Vector t); void insert(size_t j, Matrix t); @@ -2147,7 +2148,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_examples/MonocularVOExample.m b/matlab/gtsam_examples/MonocularVOExample.m index 9fd2df463..11c4253de 100644 --- a/matlab/gtsam_examples/MonocularVOExample.m +++ b/matlab/gtsam_examples/MonocularVOExample.m @@ -40,7 +40,7 @@ end %% Create initial estimate initial = Values; -trueE = EssentialMatrix(aRb,Sphere2(aTb)); +trueE = EssentialMatrix(aRb,Unit3(aTb)); initialE = trueE.retract([0.1, -0.1, 0.1, 0.1, -0.1]'); initial.insert(1, initialE); @@ -52,5 +52,5 @@ result = optimizer.optimize(); %% Print result (as essentialMatrix) and error error = graph.error(result) -E = result.at(1) +E = result.atEssentialMatrix(1) 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..1a6856a9a 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/Argument.cpp b/wrap/Argument.cpp index 1f57917d8..848998eb0 100644 --- a/wrap/Argument.cpp +++ b/wrap/Argument.cpp @@ -78,11 +78,11 @@ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const { string cppType = type.qualifiedName("::"); string matlabUniqueType = type.qualifiedName(); - if (is_ptr) + if (is_ptr && type.category != Qualified::EIGEN) // A pointer: emit an "unwrap_shared_ptr" call which returns a pointer file.oss << "boost::shared_ptr<" << cppType << "> " << name << " = unwrap_shared_ptr< "; - else if (is_ref) + else if (is_ref && type.category != Qualified::EIGEN) // A reference: emit an "unwrap_shared_ptr" call and de-reference the pointer file.oss << cppType << "& " << name << " = *unwrap_shared_ptr< "; else @@ -94,7 +94,7 @@ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const { file.oss << cppType << " " << name << " = unwrap< "; file.oss << cppType << " >(" << matlabName; - if (is_ptr || is_ref) + if( (is_ptr || is_ref) && type.category != Qualified::EIGEN) file.oss << ", \"ptr_" << matlabUniqueType << "\""; file.oss << ");" << endl; } diff --git a/wrap/matlab.h b/wrap/matlab.h index 23f391903..1639876cc 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/expected2/geometry_wrapper.cpp b/wrap/tests/expected2/geometry_wrapper.cpp index 82926e2ce..1c8403ac9 100644 --- a/wrap/tests/expected2/geometry_wrapper.cpp +++ b/wrap/tests/expected2/geometry_wrapper.cpp @@ -679,7 +679,7 @@ void MyTemplatePoint2_templatedMethod_55(int nargout, mxArray *out[], int nargin typedef boost::shared_ptr Shared; checkArguments("templatedMethodMatrix",nargout,nargin-1,1); Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); - Matrix& t = *unwrap_shared_ptr< Matrix >(in[1], "ptr_Matrix"); + Matrix& t = unwrap< Matrix >(in[1]); out[0] = wrap< Matrix >(obj->templatedMethod(t)); } diff --git a/wrap/tests/testClass.cpp b/wrap/tests/testClass.cpp index 756b6668d..ea86a174c 100644 --- a/wrap/tests/testClass.cpp +++ b/wrap/tests/testClass.cpp @@ -160,6 +160,85 @@ 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)); + +} + +TEST(Class, Template) { + + + 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" + " virtual class PriorFactor : gtsam::NoiseModelFactor {" + " PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); " + " T prior() const; " + " void serialize() const; " + "};" )); + + EXPECT(parse(markup.c_str(), g, space_p).full); + +} + /* ************************************************************************* */ int main() { TestResult tr;