From aaba18c61d5230aa24ba4f7d0f1038dd96dc860a Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 19 Dec 2014 15:39:34 +0100 Subject: [PATCH 1/5] Added expected files. --- wrap/tests/expected/geometry_wrapper.cpp | 22 +++++++++++----------- wrap/tests/expected2/geometry_wrapper.cpp | 22 +++++++++++----------- 2 files changed, 22 insertions(+), 22 deletions(-) diff --git a/wrap/tests/expected/geometry_wrapper.cpp b/wrap/tests/expected/geometry_wrapper.cpp index 527600b47..585918f20 100644 --- a/wrap/tests/expected/geometry_wrapper.cpp +++ b/wrap/tests/expected/geometry_wrapper.cpp @@ -363,7 +363,7 @@ void Test_arg_EigenConstRef_24(int nargout, mxArray *out[], int nargin, const mx typedef boost::shared_ptr Shared; checkArguments("arg_EigenConstRef",nargout,nargin-1,1); Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); - Matrix& value = *unwrap_shared_ptr< Matrix >(in[1], "ptr_Matrix"); + Matrix value = unwrap< Matrix >(in[1]); obj->arg_EigenConstRef(value); } @@ -707,7 +707,7 @@ void MyTemplatePoint2_templatedMethod_57(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)); } @@ -736,7 +736,7 @@ void MyTemplatePoint2_templatedMethod_60(int nargout, mxArray *out[], int nargin typedef boost::shared_ptr Shared; checkArguments("templatedMethodVector",nargout,nargin-1,1); Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); - Vector& t = *unwrap_shared_ptr< Vector >(in[1], "ptr_Vector"); + Vector t = unwrap< Vector >(in[1]); out[0] = wrap< Vector >(obj->templatedMethod(t)); } @@ -795,7 +795,7 @@ void MyTemplateMatrix_accept_T_65(int nargout, mxArray *out[], int nargin, const typedef boost::shared_ptr Shared; checkArguments("accept_T",nargout,nargin-1,1); Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); - Matrix& value = *unwrap_shared_ptr< Matrix >(in[1], "ptr_Matrix"); + Matrix value = unwrap< Matrix >(in[1]); obj->accept_T(value); } @@ -804,7 +804,7 @@ void MyTemplateMatrix_accept_Tptr_66(int nargout, mxArray *out[], int nargin, co typedef boost::shared_ptr Shared; checkArguments("accept_Tptr",nargout,nargin-1,1); Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); - boost::shared_ptr value = unwrap_shared_ptr< Matrix >(in[1], "ptr_Matrix"); + Matrix value = unwrap< Matrix >(in[1]); obj->accept_Tptr(value); } @@ -842,7 +842,7 @@ void MyTemplateMatrix_return_T_69(int nargout, mxArray *out[], int nargin, const typedef boost::shared_ptr Shared; checkArguments("return_T",nargout,nargin-1,1); Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); - boost::shared_ptr value = unwrap_shared_ptr< Matrix >(in[1], "ptr_Matrix"); + Matrix value = unwrap< Matrix >(in[1]); out[0] = wrap< Matrix >(obj->return_T(value)); } @@ -851,7 +851,7 @@ void MyTemplateMatrix_return_Tptr_70(int nargout, mxArray *out[], int nargin, co typedef boost::shared_ptr Shared; checkArguments("return_Tptr",nargout,nargin-1,1); Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); - boost::shared_ptr value = unwrap_shared_ptr< Matrix >(in[1], "ptr_Matrix"); + Matrix value = unwrap< Matrix >(in[1]); { SharedMatrix* ret = new SharedMatrix(obj->return_Tptr(value)); out[0] = wrap_shared_ptr(ret,"Matrix"); @@ -863,8 +863,8 @@ void MyTemplateMatrix_return_ptrs_71(int nargout, mxArray *out[], int nargin, co typedef boost::shared_ptr Shared; checkArguments("return_ptrs",nargout,nargin-1,2); Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); - boost::shared_ptr p1 = unwrap_shared_ptr< Matrix >(in[1], "ptr_Matrix"); - boost::shared_ptr p2 = unwrap_shared_ptr< Matrix >(in[2], "ptr_Matrix"); + Matrix p1 = unwrap< Matrix >(in[1]); + Matrix p2 = unwrap< Matrix >(in[2]); pair< SharedMatrix, SharedMatrix > pairResult = obj->return_ptrs(p1,p2); { SharedMatrix* ret = new SharedMatrix(pairResult.first); @@ -881,7 +881,7 @@ void MyTemplateMatrix_templatedMethod_72(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_MyTemplateMatrix"); - Matrix& t = *unwrap_shared_ptr< Matrix >(in[1], "ptr_Matrix"); + Matrix t = unwrap< Matrix >(in[1]); out[0] = wrap< Matrix >(obj->templatedMethod(t)); } @@ -910,7 +910,7 @@ void MyTemplateMatrix_templatedMethod_75(int nargout, mxArray *out[], int nargin typedef boost::shared_ptr Shared; checkArguments("templatedMethodVector",nargout,nargin-1,1); Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); - Vector& t = *unwrap_shared_ptr< Vector >(in[1], "ptr_Vector"); + Vector t = unwrap< Vector >(in[1]); out[0] = wrap< Vector >(obj->templatedMethod(t)); } diff --git a/wrap/tests/expected2/geometry_wrapper.cpp b/wrap/tests/expected2/geometry_wrapper.cpp index 1c8403ac9..4b41f3958 100644 --- a/wrap/tests/expected2/geometry_wrapper.cpp +++ b/wrap/tests/expected2/geometry_wrapper.cpp @@ -335,7 +335,7 @@ void Test_arg_EigenConstRef_22(int nargout, mxArray *out[], int nargin, const mx typedef boost::shared_ptr Shared; checkArguments("arg_EigenConstRef",nargout,nargin-1,1); Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); - Matrix& value = *unwrap_shared_ptr< Matrix >(in[1], "ptr_Matrix"); + Matrix value = unwrap< Matrix >(in[1]); obj->arg_EigenConstRef(value); } @@ -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< Matrix >(in[1]); + Matrix t = unwrap< Matrix >(in[1]); out[0] = wrap< Matrix >(obj->templatedMethod(t)); } @@ -708,7 +708,7 @@ void MyTemplatePoint2_templatedMethod_58(int nargout, mxArray *out[], int nargin typedef boost::shared_ptr Shared; checkArguments("templatedMethodVector",nargout,nargin-1,1); Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); - Vector& t = *unwrap_shared_ptr< Vector >(in[1], "ptr_Vector"); + Vector t = unwrap< Vector >(in[1]); out[0] = wrap< Vector >(obj->templatedMethod(t)); } @@ -767,7 +767,7 @@ void MyTemplateMatrix_accept_T_63(int nargout, mxArray *out[], int nargin, const typedef boost::shared_ptr Shared; checkArguments("accept_T",nargout,nargin-1,1); Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); - Matrix& value = *unwrap_shared_ptr< Matrix >(in[1], "ptr_Matrix"); + Matrix value = unwrap< Matrix >(in[1]); obj->accept_T(value); } @@ -776,7 +776,7 @@ void MyTemplateMatrix_accept_Tptr_64(int nargout, mxArray *out[], int nargin, co typedef boost::shared_ptr Shared; checkArguments("accept_Tptr",nargout,nargin-1,1); Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); - boost::shared_ptr value = unwrap_shared_ptr< Matrix >(in[1], "ptr_Matrix"); + Matrix value = unwrap< Matrix >(in[1]); obj->accept_Tptr(value); } @@ -814,7 +814,7 @@ void MyTemplateMatrix_return_T_67(int nargout, mxArray *out[], int nargin, const typedef boost::shared_ptr Shared; checkArguments("return_T",nargout,nargin-1,1); Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); - boost::shared_ptr value = unwrap_shared_ptr< Matrix >(in[1], "ptr_Matrix"); + Matrix value = unwrap< Matrix >(in[1]); out[0] = wrap< Matrix >(obj->return_T(value)); } @@ -823,7 +823,7 @@ void MyTemplateMatrix_return_Tptr_68(int nargout, mxArray *out[], int nargin, co typedef boost::shared_ptr Shared; checkArguments("return_Tptr",nargout,nargin-1,1); Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); - boost::shared_ptr value = unwrap_shared_ptr< Matrix >(in[1], "ptr_Matrix"); + Matrix value = unwrap< Matrix >(in[1]); { SharedMatrix* ret = new SharedMatrix(obj->return_Tptr(value)); out[0] = wrap_shared_ptr(ret,"Matrix"); @@ -835,8 +835,8 @@ void MyTemplateMatrix_return_ptrs_69(int nargout, mxArray *out[], int nargin, co typedef boost::shared_ptr Shared; checkArguments("return_ptrs",nargout,nargin-1,2); Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); - boost::shared_ptr p1 = unwrap_shared_ptr< Matrix >(in[1], "ptr_Matrix"); - boost::shared_ptr p2 = unwrap_shared_ptr< Matrix >(in[2], "ptr_Matrix"); + Matrix p1 = unwrap< Matrix >(in[1]); + Matrix p2 = unwrap< Matrix >(in[2]); pair< SharedMatrix, SharedMatrix > pairResult = obj->return_ptrs(p1,p2); { SharedMatrix* ret = new SharedMatrix(pairResult.first); @@ -853,7 +853,7 @@ void MyTemplateMatrix_templatedMethod_70(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_MyTemplateMatrix"); - Matrix& t = *unwrap_shared_ptr< Matrix >(in[1], "ptr_Matrix"); + Matrix t = unwrap< Matrix >(in[1]); out[0] = wrap< Matrix >(obj->templatedMethod(t)); } @@ -882,7 +882,7 @@ void MyTemplateMatrix_templatedMethod_73(int nargout, mxArray *out[], int nargin typedef boost::shared_ptr Shared; checkArguments("templatedMethodVector",nargout,nargin-1,1); Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); - Vector& t = *unwrap_shared_ptr< Vector >(in[1], "ptr_Vector"); + Vector t = unwrap< Vector >(in[1]); out[0] = wrap< Vector >(obj->templatedMethod(t)); } From 9f2e6562c2ae8c83619ee8f9acedcd6c19340f7c Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 19 Dec 2014 15:40:21 +0100 Subject: [PATCH 2/5] test virtual, cleaned up other test --- wrap/tests/testClass.cpp | 87 +++++++++++++++++++++++----------------- 1 file changed, 51 insertions(+), 36 deletions(-) diff --git a/wrap/tests/testClass.cpp b/wrap/tests/testClass.cpp index ea86a174c..a133e15ac 100644 --- a/wrap/tests/testClass.cpp +++ b/wrap/tests/testClass.cpp @@ -160,7 +160,6 @@ TEST( Class, Grammar ) { EXPECT_LONGS_EQUAL(ReturnType::EIGEN, rv4.type1.category); } - //****************************************************************************** TEST( Class, TemplateSubstition ) { @@ -171,10 +170,10 @@ TEST( Class, TemplateSubstition ) { Template t; ClassGrammar g(cls, t); - string markup( - string("template class Point2 { \n") - + string(" T myMethod(const T& t) const; \n") - + string("};")); + string markup(string("template" + "class Point2 {" + " T myMethod(const T& t) const;" + "};")); EXPECT(parse(markup.c_str(), g, space_p).full); @@ -190,58 +189,74 @@ TEST( Class, TemplateSubstition ) { EXPECT(assert_equal("T", rv2.type1.name())); EXPECT_LONGS_EQUAL(ReturnType::CLASS, rv2.type1.category); + // Generate some expected values for qualified types + Qualified q_void("void", Qualified::VOID); + Qualified q_double("double", Qualified::BASIS); + Qualified q_Matrix("Matrix", Qualified::EIGEN); + Qualified q_Point3("Point3", Qualified::CLASS); + 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)); + EXPECT(t[0]==q_void); + EXPECT(t[1]==q_double); + EXPECT(t[2]==q_Matrix); + EXPECT(t[3]==q_Point3); - vector classes = cls.expandTemplate(t.argName(), - t.argValues()); + 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)); + EXPECT( classes[0].method("myMethod").returnValue(0).type1 == q_void); + EXPECT( classes[1].method("myMethod").returnValue(0).type1 == q_double); + EXPECT( classes[2].method("myMethod").returnValue(0).type1 == q_Matrix); + EXPECT( classes[3].method("myMethod").returnValue(0).type1 == q_Point3); // 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)); + EXPECT( classes[0].method("myMethod").argumentList(0)[0].type == q_void); + EXPECT( classes[1].method("myMethod").argumentList(0)[0].type == q_double); + EXPECT( classes[2].method("myMethod").argumentList(0)[0].type == q_Matrix); + EXPECT( classes[3].method("myMethod").argumentList(0)[0].type == q_Point3); } +//****************************************************************************** TEST(Class, Template) { + using classic::space_p; - using classic::space_p; + // Create type grammar that will place result in cls + Class cls; + Template t; + ClassGrammar g(cls, t); - // 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; " + "};")); - 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); + EXPECT(parse(markup.c_str(), g, space_p).full); } -/* ************************************************************************* */ +//****************************************************************************** +TEST( Class, Virtualness ) { + using classic::space_p; + Class cls; + Template t; + ClassGrammar g(cls, t); + string markup("virtual class Point3 {};"); + EXPECT(parse(markup.c_str(), g, space_p).full); + EXPECT(cls.isVirtual); +} +//****************************************************************************** int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ +//****************************************************************************** From 959a002693abe7fde31e9bfb97b0d75c20ddd519 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 19 Dec 2014 15:40:43 +0100 Subject: [PATCH 3/5] Fixed and double versions --- gtsam.h | 41 +++++++++++++++++++++----------------- gtsam/nonlinear/Values.cpp | 18 +++++++++++++++++ gtsam/nonlinear/Values.h | 14 +++++++++++++ 3 files changed, 55 insertions(+), 18 deletions(-) diff --git a/gtsam.h b/gtsam.h index 25c34ebd1..da29b99b6 100644 --- a/gtsam.h +++ b/gtsam.h @@ -156,12 +156,6 @@ virtual class Value { size_t dim() const; }; -class Vector3 { - Vector3(Vector v); -}; -class Vector6 { - Vector6(Vector v); -}; #include class LieScalar { // Standard constructors @@ -758,10 +752,6 @@ class CalibratedCamera { gtsam::CalibratedCamera retract(const Vector& d) const; Vector localCoordinates(const gtsam::CalibratedCamera& T2) const; - // Group - gtsam::CalibratedCamera compose(const gtsam::CalibratedCamera& c) const; - gtsam::CalibratedCamera inverse() const; - // Action on Point3 gtsam::Point2 project(const gtsam::Point3& point) const; static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint); @@ -1727,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); @@ -1758,6 +1749,16 @@ class Values { template T at(size_t j); + + /// Fixed size versions, for n in 1..9 + void insertFixed(size_t j, Vector t, size_t n); + + /// Fixed size versions, for n in 1..9 + Vector atFixed(size_t j, size_t n); + + /// version for double + void insertDouble(size_t j, double c); + double atDouble(size_t j) const; }; // Actually a FastList @@ -2199,10 +2200,14 @@ typedef gtsam::RangeFactor RangeFactorPosePoint2; typedef gtsam::RangeFactor RangeFactorPosePoint3; typedef gtsam::RangeFactor RangeFactorPose2; typedef gtsam::RangeFactor RangeFactorPose3; -typedef gtsam::RangeFactor RangeFactorCalibratedCameraPoint; -typedef gtsam::RangeFactor RangeFactorSimpleCameraPoint; -typedef gtsam::RangeFactor RangeFactorCalibratedCamera; -typedef gtsam::RangeFactor RangeFactorSimpleCamera; + +// Commented out by Frank Dec 2014: not poses! +// If needed, we need a RangeFactor that takes a camera, extracts the pose +// Should be easy with Expressions +//typedef gtsam::RangeFactor RangeFactorCalibratedCameraPoint; +//typedef gtsam::RangeFactor RangeFactorSimpleCameraPoint; +//typedef gtsam::RangeFactor RangeFactorCalibratedCamera; +//typedef gtsam::RangeFactor RangeFactorSimpleCamera; #include @@ -2395,7 +2400,7 @@ class ConstantBias { #include class PoseVelocity{ - PoseVelocity(const gtsam::Pose3& pose, const gtsam::Vector3 velocity); + PoseVelocity(const gtsam::Pose3& pose, Vector velocity); }; class ImuFactorPreintegratedMeasurements { // Standard Constructor @@ -2434,7 +2439,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; }; @@ -2480,7 +2485,7 @@ virtual class AHRSFactor : gtsam::NonlinearFactor { #include 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 @@ -2531,7 +2536,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); }; diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp index b92e54143..b9a3f714b 100644 --- a/gtsam/nonlinear/Values.cpp +++ b/gtsam/nonlinear/Values.cpp @@ -112,6 +112,24 @@ namespace gtsam { return result; } + /* ************************************************************************* */ + Vector Values::atFixed(Key j, size_t n) { + switch (n) { + case 1: return at(j); + case 2: return at(j); + case 3: return at(j); + case 4: return at(j); + case 5: return at(j); + case 6: return at(j); + case 7: return at(j); + case 8: return at(j); + case 9: return at(j); + default: + throw runtime_error( + "Values::at fixed size can only handle n in 1..9"); + } + } + /* ************************************************************************* */ const Value& Values::at(Key j) const { // Find the item diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index d00baa0d9..dcbc2a433 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -180,6 +180,13 @@ namespace gtsam { template const ValueType& at(Key j) const; + /// Special version for small fixed size vectors, for matlab/python + /// throws truntime error if n<1 || n>9 + Vector atFixed(Key j, size_t n); + + /// version for double + double atDouble(size_t key) const { return at(key);} + /** Retrieve a variable by key \c j. This version returns a reference * to the base Value class, and needs to be casted before use. * @param j Retrieve the value associated with this key @@ -258,6 +265,13 @@ namespace gtsam { template 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); + + /// version for double + void insertDouble(Key j, double c) { insert(j,c); } + /// overloaded insert version that also specifies a chart template void insert(Key j, const ValueType& val); From fac54f87b9c5e6d859c05a399955e972d99a1156 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 19 Dec 2014 16:18:10 +0100 Subject: [PATCH 4/5] Added Vector1 from develop --- gtsam/base/Vector.h | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 1be8408d2..dbc34d09a 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -34,6 +34,7 @@ namespace gtsam { typedef Eigen::VectorXd Vector; // Commonly used fixed size vectors +typedef Eigen::Matrix Vector1; typedef Eigen::Vector2d Vector2; typedef Eigen::Vector3d Vector3; typedef Eigen::Matrix Vector4; From c29e6ca2e7d0954bf6cc51c144b17df239d8f54d Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 19 Dec 2014 16:19:02 +0100 Subject: [PATCH 5/5] Fixed subtle (imperative!) bug where a forward declaration was partially parsed as a class, only then as a forward declaration. --- wrap/Module.cpp | 1 + wrap/tests/geometry.h | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/wrap/Module.cpp b/wrap/Module.cpp index 55fd13715..092c732f7 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -156,6 +156,7 @@ void Module::parseMarkup(const std::string& data) { >> (*(basic.namespace_p >> str_p("::")) >> basic.className_p)[assign_a(fwDec.name)] >> ch_p(';') [push_back_a(forward_declarations, fwDec)] + [assign_a(cls,cls0)] // also clear class to avoid partial parse [assign_a(fwDec, fwDec0)]; Rule module_content_p = basic.comments_p | include_p | class_p diff --git a/wrap/tests/geometry.h b/wrap/tests/geometry.h index 78e2a1dff..69bc7e3be 100644 --- a/wrap/tests/geometry.h +++ b/wrap/tests/geometry.h @@ -1,7 +1,7 @@ // comments! class VectorNotEigen; -class ns::OtherClass; +virtual class ns::OtherClass; namespace gtsam {