diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index 867844fbb..140ab1bdf 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -141,7 +141,9 @@ private: friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(leftCamPose_); + ar & boost::serialization::make_nvp("StereoCamera", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(leftCamPose_); ar & BOOST_SERIALIZATION_NVP(K_); } diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index d94c162af..a9db7dcbe 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -111,6 +111,8 @@ namespace gtsam { friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NoiseModelFactor2", + boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); } }; @@ -143,6 +145,7 @@ namespace gtsam { */ GeneralSFMFactor2(const Point2& measured, const SharedNoiseModel& model, Key poseKey, Key landmarkKey, Key calibKey) : Base(model, poseKey, landmarkKey, calibKey), measured_(measured) {} + GeneralSFMFactor2():measured_(0.0,0.0) {} ///< default constructor virtual ~GeneralSFMFactor2() {} ///< destructor @@ -197,6 +200,8 @@ namespace gtsam { friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NoiseModelFactor3", + boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); } }; diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h index d0c0bda6a..5be783aaf 100644 --- a/gtsam/slam/StereoFactor.h +++ b/gtsam/slam/StereoFactor.h @@ -74,7 +74,7 @@ public: /** * equals */ - virtual bool equals(const NonlinearFactor& f, double tol) const { + virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { const GenericStereoFactor* p = dynamic_cast (&f); return p && Base::equals(f) && measured_.equals(p->measured_, tol); } @@ -102,6 +102,8 @@ private: friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NoiseModelFactor2", + boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); ar & BOOST_SERIALIZATION_NVP(K_); } diff --git a/tests/testSerializationSLAM.cpp b/tests/testSerializationSLAM.cpp index c0cdb6a93..4f9664e20 100644 --- a/tests/testSerializationSLAM.cpp +++ b/tests/testSerializationSLAM.cpp @@ -17,13 +17,17 @@ */ #include - -#include -#include +//#include #include -#include #include +#include +//#include +#include +//#include +#include #include +#include +#include #include #include #include @@ -42,6 +46,7 @@ #include #include #include +#include #include #include @@ -64,6 +69,7 @@ typedef PriorFactor PriorFactorCal3_S2; typedef PriorFactor PriorFactorCal3DS2; typedef PriorFactor PriorFactorCalibratedCamera; typedef PriorFactor PriorFactorSimpleCamera; +typedef PriorFactor PriorFactorStereoCamera; typedef BetweenFactor BetweenFactorLieVector; typedef BetweenFactor BetweenFactorLieMatrix; @@ -87,6 +93,7 @@ typedef NonlinearEquality NonlinearEqualityCal3_S2; typedef NonlinearEquality NonlinearEqualityCal3DS2; typedef NonlinearEquality NonlinearEqualityCalibratedCamera; typedef NonlinearEquality NonlinearEqualitySimpleCamera; +typedef NonlinearEquality NonlinearEqualityStereoCamera; typedef RangeFactor RangeFactorPosePoint2; typedef RangeFactor RangeFactorPosePoint3; @@ -106,6 +113,13 @@ typedef BearingRangeFactor BearingRangeFactor3D; typedef GenericProjectionFactor GenericProjectionFactorCal3_S2; typedef GenericProjectionFactor GenericProjectionFactorCal3DS2; +typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; +//typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3DS2; + +typedef gtsam::GeneralSFMFactor2 GeneralSFMFactor2Cal3_S2; + +typedef gtsam::GenericStereoFactor GenericStereoFactor3D; + // Convenience for named keys using symbol_shorthand::X; @@ -136,8 +150,10 @@ BOOST_CLASS_EXPORT(gtsam::Pose2); BOOST_CLASS_EXPORT(gtsam::Pose3); BOOST_CLASS_EXPORT(gtsam::Cal3_S2); BOOST_CLASS_EXPORT(gtsam::Cal3DS2); +BOOST_CLASS_EXPORT(gtsam::Cal3_S2Stereo); BOOST_CLASS_EXPORT(gtsam::CalibratedCamera); BOOST_CLASS_EXPORT(gtsam::SimpleCamera); +BOOST_CLASS_EXPORT(gtsam::StereoCamera); /* Create GUIDs for factors */ @@ -158,6 +174,7 @@ BOOST_CLASS_EXPORT_GUID(PriorFactorCal3_S2, "gtsam::PriorFactorCal3_S2"); BOOST_CLASS_EXPORT_GUID(PriorFactorCal3DS2, "gtsam::PriorFactorCal3DS2"); BOOST_CLASS_EXPORT_GUID(PriorFactorCalibratedCamera, "gtsam::PriorFactorCalibratedCamera"); BOOST_CLASS_EXPORT_GUID(PriorFactorSimpleCamera, "gtsam::PriorFactorSimpleCamera"); +BOOST_CLASS_EXPORT_GUID(PriorFactorStereoCamera, "gtsam::PriorFactorStereoCamera"); BOOST_CLASS_EXPORT_GUID(BetweenFactorLieVector, "gtsam::BetweenFactorLieVector"); BOOST_CLASS_EXPORT_GUID(BetweenFactorLieMatrix, "gtsam::BetweenFactorLieMatrix"); @@ -181,6 +198,7 @@ BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCal3_S2, "gtsam::NonlinearEqualityCal3_ BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCal3DS2, "gtsam::NonlinearEqualityCal3DS2"); BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCalibratedCamera, "gtsam::NonlinearEqualityCalibratedCamera"); BOOST_CLASS_EXPORT_GUID(NonlinearEqualitySimpleCamera, "gtsam::NonlinearEqualitySimpleCamera"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityStereoCamera, "gtsam::NonlinearEqualityStereoCamera"); BOOST_CLASS_EXPORT_GUID(RangeFactorPosePoint2, "gtsam::RangeFactorPosePoint2"); BOOST_CLASS_EXPORT_GUID(RangeFactorPosePoint3, "gtsam::RangeFactorPosePoint3"); @@ -198,6 +216,13 @@ BOOST_CLASS_EXPORT_GUID(BearingRangeFactor2D, "gtsam::BearingRangeFactor2D"); BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3_S2, "gtsam::GenericProjectionFactorCal3_S2"); BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3DS2, "gtsam::GenericProjectionFactorCal3DS2"); +BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3_S2, "gtsam::GeneralSFMFactorCal3_S2"); +//BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3DS2, "gtsam::GeneralSFMFactorCal3DS2"); + +BOOST_CLASS_EXPORT_GUID(GeneralSFMFactor2Cal3_S2, "gtsam::GeneralSFMFactor2Cal3_S2"); + +BOOST_CLASS_EXPORT_GUID(GenericStereoFactor3D, "gtsam::GenericStereoFactor3D"); + /* ************************************************************************* */ TEST (Serialization, smallExample_linear) { @@ -261,12 +286,18 @@ TEST (Serialization, factors) { Pose3 pose3(rot3, point3); Cal3_S2 cal3_s2(1.0, 2.0, 3.0, 4.0, 5.0); Cal3DS2 cal3ds2(1.0, 2.0, 3.0, 4.0, 5.0,6.0, 7.0, 8.0, 9.0); + Cal3_S2Stereo cal3_s2stereo(1.0, 2.0, 3.0, 4.0, 5.0, 1.0); CalibratedCamera calibratedCamera(pose3); SimpleCamera simpleCamera(pose3, cal3_s2); + StereoCamera stereoCamera(pose3, boost::make_shared(cal3_s2stereo)); - Symbol a01('a',1), a02('a',2), a03('a',3), a04('a',4), a05('a',5), a06('a',6), a07('a',7), a08('a',8), a09('a',9), a10('a',10), a11('a',11), a12('a',12), a13('a',13); - Symbol b01('b',1), b02('b',2), b03('b',3), b04('b',4), b05('b',5), b06('b',6), b07('b',7), b08('b',8), b09('b',9), b10('b',10), b11('b',11), b12('b',12), b13('b',13); + Symbol a01('a',1), a02('a',2), a03('a',3), a04('a',4), a05('a',5), + a06('a',6), a07('a',7), a08('a',8), a09('a',9), a10('a',10), + a11('a',11), a12('a',12), a13('a',13), a14('a',14), a15('a',15); + Symbol b01('b',1), b02('b',2), b03('b',3), b04('b',4), b05('b',5), + b06('b',6), b07('b',7), b08('b',8), b09('b',9), b10('b',10), + b11('b',11), b12('b',12), b13('b',13), b14('b',14), b15('b',15); Values values; values.insert(a01, lieVector); @@ -282,6 +313,7 @@ TEST (Serialization, factors) { values.insert(a11, cal3ds2); values.insert(a12, calibratedCamera); values.insert(a13, simpleCamera); + values.insert(a14, stereoCamera); SharedNoiseModel model1 = noiseModel::Isotropic::Sigma(1, 0.3); @@ -308,6 +340,7 @@ TEST (Serialization, factors) { PriorFactorCal3DS2 priorFactorCal3DS2(a11, cal3ds2, model9); PriorFactorCalibratedCamera priorFactorCalibratedCamera(a12, calibratedCamera, model6); PriorFactorSimpleCamera priorFactorSimpleCamera(a13, simpleCamera, model11); + PriorFactorStereoCamera priorFactorStereoCamera(a14, stereoCamera, model11); BetweenFactorLieVector betweenFactorLieVector(a01, b01, lieVector, model4); BetweenFactorLieMatrix betweenFactorLieMatrix(a02, b02, lieMatrix, model6); @@ -331,6 +364,7 @@ TEST (Serialization, factors) { NonlinearEqualityCal3DS2 nonlinearEqualityCal3DS2(a11, cal3ds2); NonlinearEqualityCalibratedCamera nonlinearEqualityCalibratedCamera(a12, calibratedCamera); NonlinearEqualitySimpleCamera nonlinearEqualitySimpleCamera(a13, simpleCamera); + NonlinearEqualityStereoCamera nonlinearEqualityStereoCamera(a14, stereoCamera); RangeFactorPosePoint2 rangeFactorPosePoint2(a08, a03, 2.0, model1); RangeFactorPosePoint3 rangeFactorPosePoint3(a09, a05, 2.0, model1); @@ -348,7 +382,11 @@ TEST (Serialization, factors) { GenericProjectionFactorCal3_S2 genericProjectionFactorCal3_S2(point2, model2, a09, a05, boost::make_shared(cal3_s2)); GenericProjectionFactorCal3DS2 genericProjectionFactorCal3DS2(point2, model2, a09, a05, boost::make_shared(cal3ds2)); + GeneralSFMFactorCal3_S2 generalSFMFactorCal3_S2(point2, model2, a13, a05); + GeneralSFMFactor2Cal3_S2 generalSFMFactor2Cal3_S2(point2, model2, a09, a05, a10); + + GenericStereoFactor3D genericStereoFactor3D(stereoPoint2, model3, a09, a05, boost::make_shared(cal3_s2stereo)); NonlinearFactorGraph graph; @@ -362,9 +400,10 @@ TEST (Serialization, factors) { graph.add(priorFactorPose2); graph.add(priorFactorPose3); graph.add(priorFactorCal3_S2); - graph.add(priorFactorCal3_S2); + graph.add(priorFactorCal3DS2); graph.add(priorFactorCalibratedCamera); graph.add(priorFactorSimpleCamera); + graph.add(priorFactorStereoCamera); graph.add(betweenFactorLieVector); graph.add(betweenFactorLieMatrix); @@ -388,6 +427,7 @@ TEST (Serialization, factors) { graph.add(nonlinearEqualityCal3DS2); graph.add(nonlinearEqualityCalibratedCamera); graph.add(nonlinearEqualitySimpleCamera); + graph.add(nonlinearEqualityStereoCamera); graph.add(rangeFactorPosePoint2); graph.add(rangeFactorPosePoint3); @@ -405,6 +445,12 @@ TEST (Serialization, factors) { graph.add(genericProjectionFactorCal3_S2); graph.add(genericProjectionFactorCal3DS2); + graph.add(generalSFMFactorCal3_S2); + + graph.add(generalSFMFactor2Cal3_S2); + + graph.add(genericStereoFactor3D); + // text EXPECT(equalsObj(a01)); @@ -425,6 +471,7 @@ TEST (Serialization, factors) { EXPECT(equalsObj(priorFactorCal3DS2)); EXPECT(equalsObj(priorFactorCalibratedCamera)); EXPECT(equalsObj(priorFactorSimpleCamera)); + EXPECT(equalsObj(priorFactorStereoCamera)); EXPECT(equalsObj(betweenFactorLieVector)); EXPECT(equalsObj(betweenFactorLieMatrix)); @@ -448,6 +495,7 @@ TEST (Serialization, factors) { EXPECT(equalsObj(nonlinearEqualityCal3DS2)); EXPECT(equalsObj(nonlinearEqualityCalibratedCamera)); EXPECT(equalsObj(nonlinearEqualitySimpleCamera)); + EXPECT(equalsObj(nonlinearEqualityStereoCamera)); EXPECT(equalsObj(rangeFactorPosePoint2)); EXPECT(equalsObj(rangeFactorPosePoint3)); @@ -465,6 +513,12 @@ TEST (Serialization, factors) { EXPECT(equalsObj(genericProjectionFactorCal3_S2)); EXPECT(equalsObj(genericProjectionFactorCal3DS2)); + EXPECT(equalsObj(generalSFMFactorCal3_S2)); + + EXPECT(equalsObj(generalSFMFactor2Cal3_S2)); + + EXPECT(equalsObj(genericStereoFactor3D)); + // xml EXPECT(equalsXML(a01)); @@ -485,6 +539,7 @@ TEST (Serialization, factors) { EXPECT(equalsXML(priorFactorCal3DS2)); EXPECT(equalsXML(priorFactorCalibratedCamera)); EXPECT(equalsXML(priorFactorSimpleCamera)); + EXPECT(equalsXML(priorFactorStereoCamera)); EXPECT(equalsXML(betweenFactorLieVector)); EXPECT(equalsXML(betweenFactorLieMatrix)); @@ -508,6 +563,7 @@ TEST (Serialization, factors) { EXPECT(equalsXML(nonlinearEqualityCal3DS2)); EXPECT(equalsXML(nonlinearEqualityCalibratedCamera)); EXPECT(equalsXML(nonlinearEqualitySimpleCamera)); + EXPECT(equalsXML(nonlinearEqualityStereoCamera)); EXPECT(equalsXML(rangeFactorPosePoint2)); EXPECT(equalsXML(rangeFactorPosePoint3)); @@ -524,6 +580,12 @@ TEST (Serialization, factors) { EXPECT(equalsXML(genericProjectionFactorCal3_S2)); EXPECT(equalsXML(genericProjectionFactorCal3DS2)); + + EXPECT(equalsXML(generalSFMFactorCal3_S2)); + + EXPECT(equalsXML(generalSFMFactor2Cal3_S2)); + + EXPECT(equalsXML(genericStereoFactor3D)); }