From 66959f8423cabd8be4ffc4b0fae2157f4056e773 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 30 Dec 2018 14:34:06 -0500 Subject: [PATCH 1/6] Added noise model to make test succeed --- matlab/gtsam_tests/testJacobianFactor.m | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/matlab/gtsam_tests/testJacobianFactor.m b/matlab/gtsam_tests/testJacobianFactor.m index bba6ca5ac..1c214c3bc 100644 --- a/matlab/gtsam_tests/testJacobianFactor.m +++ b/matlab/gtsam_tests/testJacobianFactor.m @@ -54,7 +54,8 @@ S13 = [ +0.00,-8.94427 ]; d=[2.23607;-1.56525]; -expectedCG = GaussianConditional(x2,d,R11,l1,S12,x1,S13); +unit2 = noiseModel.Unit.Create(2); +expectedCG = GaussianConditional(x2,d,R11,l1,S12,x1,S13,unit2); % check if the result matches CHECK('actualCG.equals(expectedCG,1e-5)',actualCG.equals(expectedCG,1e-4)); From 2c8f55a2d8ff6a54dae83490b63e6a251000de44 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 30 Dec 2018 14:34:55 -0500 Subject: [PATCH 2/6] Adding GenericValue for Values serialization, as suggested by Callum Robinson and Mike Sheffler in issue #398 --- gtsam.h | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/gtsam.h b/gtsam.h index 24a717c3c..01b23e6cc 100644 --- a/gtsam.h +++ b/gtsam.h @@ -228,6 +228,12 @@ virtual class Value { size_t dim() const; }; +#include +template +virtual class GenericValue : gtsam::Value { + void serializable() const; +}; + #include class LieScalar { // Standard constructors From cdf2a6335b288e4282f74855039dfe5020e9e4a9 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 30 Dec 2018 16:23:43 -0500 Subject: [PATCH 3/6] Added serialization for two more factors --- gtsam.h | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/gtsam.h b/gtsam.h index 01b23e6cc..d681ddb0d 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2265,6 +2265,9 @@ virtual class NonlinearEquality : gtsam::NoiseModelFactor { template virtual class RangeFactor : gtsam::NoiseModelFactor { RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel); + + // enabling serialization functionality + void serialize() const; }; typedef gtsam::RangeFactor RangeFactorPosePoint2; @@ -2281,6 +2284,9 @@ typedef gtsam::RangeFactor RangeFactor template virtual class RangeFactorWithTransform : gtsam::NoiseModelFactor { RangeFactorWithTransform(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel, const POSE& body_T_sensor); + + // enabling serialization functionality + void serialize() const; }; typedef gtsam::RangeFactorWithTransform RangeFactorWithTransformPosePoint2; From fe1daec086aa5b20966d4ab9615be137e665adc9 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 30 Dec 2018 16:24:19 -0500 Subject: [PATCH 4/6] Changed include error as in http://boost.2283326.n4.nabble.com/boost-serialization-Serializing-Dynamically-Loaded-Libraries-quot-Unregistered-Void-Cast-quot-td2570981.html --- gtsam/base/serialization.h | 2 +- wrap/Module.cpp | 4 ++-- wrap/tests/expected/geometry_wrapper.cpp | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/base/serialization.h b/gtsam/base/serialization.h index ebd893ad1..954d3e86b 100644 --- a/gtsam/base/serialization.h +++ b/gtsam/base/serialization.h @@ -24,7 +24,6 @@ #include // includes for standard serialization types -#include #include #include #include @@ -39,6 +38,7 @@ #include #include #include +#include namespace gtsam { diff --git a/wrap/Module.cpp b/wrap/Module.cpp index 9eee686cb..5cf3b5d6c 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -273,9 +273,9 @@ void Module::generate_matlab_wrapper(const string& toolboxPath) const { // Include boost.serialization archive headers before other class headers if (hasSerialiable) { - wrapperFile.oss << "#include \n"; wrapperFile.oss << "#include \n"; - wrapperFile.oss << "#include \n\n"; + wrapperFile.oss << "#include \n"; + wrapperFile.oss << "#include \n\n"; } // Generate includes while avoiding redundant includes diff --git a/wrap/tests/expected/geometry_wrapper.cpp b/wrap/tests/expected/geometry_wrapper.cpp index 7e0cb0e47..dec78b80c 100644 --- a/wrap/tests/expected/geometry_wrapper.cpp +++ b/wrap/tests/expected/geometry_wrapper.cpp @@ -1,9 +1,9 @@ #include #include -#include #include #include +#include #include #include From 63acd1a50c9362190d351279c671d76cd9d82f80 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 30 Dec 2018 18:35:33 -0500 Subject: [PATCH 5/6] Add bearing and range factor tests --- matlab/gtsam_tests/testSerialization.m | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/matlab/gtsam_tests/testSerialization.m b/matlab/gtsam_tests/testSerialization.m index 9f49328cd..baacb198b 100644 --- a/matlab/gtsam_tests/testSerialization.m +++ b/matlab/gtsam_tests/testSerialization.m @@ -52,14 +52,26 @@ priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]); graph.add(PriorFactorPose2(i1, priorMean, priorNoise)); % add directly to graph -% Between Factors - FAIL: unregistered class +% Between Factors odometry = Pose2(2.0, 0.0, 0.0); odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); graph.add(BetweenFactorPose2(i1, i2, odometry, odometryNoise)); graph.add(BetweenFactorPose2(i2, i3, odometry, odometryNoise)); -% BearingRange Factors - FAIL: unregistered class +% Range Factors +rNoise = noiseModel.Diagonal.Sigmas([0.2]); +graph.add(RangeFactorPosePoint2(i1, j1, sqrt(4+4), rNoise)); +graph.add(RangeFactorPosePoint2(i2, j1, 2, rNoise)); +graph.add(RangeFactorPosePoint2(i3, j2, 2, rNoise)); + +% Bearing Factors degrees = pi/180; +bNoise = noiseModel.Diagonal.Sigmas([0.1]); +graph.add(BearingFactor2D(i1, j1, Rot2(45*degrees), bNoise)); +graph.add(BearingFactor2D(i2, j1, Rot2(90*degrees), bNoise)); +graph.add(BearingFactor2D(i3, j2, Rot2(90*degrees), bNoise)); + +% BearingRange Factors brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]); graph.add(BearingRangeFactor2D(i1, j1, Rot2(45*degrees), sqrt(4+4), brNoise)); graph.add(BearingRangeFactor2D(i2, j1, Rot2(90*degrees), 2, brNoise)); From 3b3e39381712d69f3252a27af6fa0da65acec68b Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 30 Dec 2018 18:37:38 -0500 Subject: [PATCH 6/6] Add serialization functions that call base classes, to avoid "unregistered void cast" errors (in MATLAB, as flagged by issue #398), as mentioned in http://tb-nguyen.blogspot.com/2009/08/more-on-using-boost-serialization-and.html --- gtsam/nonlinear/ExpressionFactor.h | 7 +++++++ gtsam/sam/BearingFactor.h | 8 ++++++++ gtsam/sam/BearingRangeFactor.h | 8 ++++++++ gtsam/sam/RangeFactor.h | 8 ++++++++ 4 files changed, 31 insertions(+) diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h index 34ba8e1ff..04d82fe9a 100644 --- a/gtsam/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -266,6 +266,13 @@ class ExpressionFactor2 : public ExpressionFactor { virtual Expression expression() const { return expression(this->keys_[0], this->keys_[1]); } + + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "ExpressionFactor", boost::serialization::base_object >(*this)); + } }; // ExpressionFactor2 diff --git a/gtsam/sam/BearingFactor.h b/gtsam/sam/BearingFactor.h index f190e683c..a9ed5ef4b 100644 --- a/gtsam/sam/BearingFactor.h +++ b/gtsam/sam/BearingFactor.h @@ -60,6 +60,14 @@ struct BearingFactor : public ExpressionFactor2 { std::cout << s << "BearingFactor" << std::endl; Base::print(s, kf); } + + private: + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "Base", boost::serialization::base_object(*this)); + } }; // BearingFactor /// traits diff --git a/gtsam/sam/BearingRangeFactor.h b/gtsam/sam/BearingRangeFactor.h index 2dd1fecb8..44740f8ff 100644 --- a/gtsam/sam/BearingRangeFactor.h +++ b/gtsam/sam/BearingRangeFactor.h @@ -73,6 +73,14 @@ class BearingRangeFactor Base::print(s, kf); } + + private: + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "Base", boost::serialization::base_object(*this)); + } }; // BearingRangeFactor /// traits diff --git a/gtsam/sam/RangeFactor.h b/gtsam/sam/RangeFactor.h index a5bcac822..40a9cf758 100644 --- a/gtsam/sam/RangeFactor.h +++ b/gtsam/sam/RangeFactor.h @@ -65,6 +65,14 @@ class RangeFactor : public ExpressionFactor2 { std::cout << s << "RangeFactor" << std::endl; Base::print(s, kf); } + + private: + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "Base", boost::serialization::base_object(*this)); + } }; // \ RangeFactor /// traits