diff --git a/gtsam.h b/gtsam.h index 24a717c3c..d681ddb0d 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 @@ -2259,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; @@ -2275,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; 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/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 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)); 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)); 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