diff --git a/gtsam/linear/GaussianISAM.h b/gtsam/linear/GaussianISAM.h index 55208d4d1..b77b26240 100644 --- a/gtsam/linear/GaussianISAM.h +++ b/gtsam/linear/GaussianISAM.h @@ -20,6 +20,7 @@ #include #include +#include namespace gtsam { @@ -43,4 +44,8 @@ namespace gtsam { }; + /// traits + template <> + struct traits : public Testable {}; + } diff --git a/gtsam/nonlinear/PriorFactor.h b/gtsam/nonlinear/PriorFactor.h index 0afbaa588..3198dab07 100644 --- a/gtsam/nonlinear/PriorFactor.h +++ b/gtsam/nonlinear/PriorFactor.h @@ -117,4 +117,9 @@ namespace gtsam { GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) }; + /// traits + template + struct traits > : public Testable > {}; + + } /// namespace gtsam diff --git a/tests/testSerializationSLAM.cpp b/tests/testSerializationSLAM.cpp index e3252a90b..84e521156 100644 --- a/tests/testSerializationSLAM.cpp +++ b/tests/testSerializationSLAM.cpp @@ -18,15 +18,10 @@ #include -#if 0 - #include -//#include #include #include -//#include #include -//#include #include #include #include @@ -34,8 +29,6 @@ #include #include #include -#include -#include #include #include #include @@ -49,6 +42,7 @@ #include #include #include +#include #include @@ -57,8 +51,6 @@ using namespace gtsam; using namespace gtsam::serializationTestHelpers; // Creating as many permutations of factors as possible -typedef PriorFactor PriorFactorLieVector; -typedef PriorFactor PriorFactorLieMatrix; typedef PriorFactor PriorFactorPoint2; typedef PriorFactor PriorFactorStereoPoint2; typedef PriorFactor PriorFactorPoint3; @@ -69,12 +61,9 @@ typedef PriorFactor PriorFactorPose3; typedef PriorFactor PriorFactorCal3_S2; typedef PriorFactor PriorFactorCal3DS2; typedef PriorFactor PriorFactorCalibratedCamera; -typedef PriorFactor PriorFactorSimpleCamera; typedef PriorFactor PriorFactorPinholeCameraCal3_S2; typedef PriorFactor PriorFactorStereoCamera; -typedef BetweenFactor BetweenFactorLieVector; -typedef BetweenFactor BetweenFactorLieMatrix; typedef BetweenFactor BetweenFactorPoint2; typedef BetweenFactor BetweenFactorPoint3; typedef BetweenFactor BetweenFactorRot2; @@ -82,8 +71,6 @@ typedef BetweenFactor BetweenFactorRot3; typedef BetweenFactor BetweenFactorPose2; typedef BetweenFactor BetweenFactorPose3; -typedef NonlinearEquality NonlinearEqualityLieVector; -typedef NonlinearEquality NonlinearEqualityLieMatrix; typedef NonlinearEquality NonlinearEqualityPoint2; typedef NonlinearEquality NonlinearEqualityStereoPoint2; typedef NonlinearEquality NonlinearEqualityPoint3; @@ -94,7 +81,6 @@ typedef NonlinearEquality NonlinearEqualityPose3; typedef NonlinearEquality NonlinearEqualityCal3_S2; typedef NonlinearEquality NonlinearEqualityCal3DS2; typedef NonlinearEquality NonlinearEqualityCalibratedCamera; -typedef NonlinearEquality NonlinearEqualitySimpleCamera; typedef NonlinearEquality NonlinearEqualityPinholeCameraCal3_S2; typedef NonlinearEquality NonlinearEqualityStereoCamera; @@ -148,8 +134,6 @@ BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); /* Create GUIDs for geometry */ /* ************************************************************************* */ -GTSAM_VALUE_EXPORT(gtsam::LieVector); -GTSAM_VALUE_EXPORT(gtsam::LieMatrix); GTSAM_VALUE_EXPORT(gtsam::Point2); GTSAM_VALUE_EXPORT(gtsam::StereoPoint2); GTSAM_VALUE_EXPORT(gtsam::Point3); @@ -170,8 +154,6 @@ GTSAM_VALUE_EXPORT(gtsam::StereoCamera); BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor"); BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor"); -BOOST_CLASS_EXPORT_GUID(PriorFactorLieVector, "gtsam::PriorFactorLieVector"); -BOOST_CLASS_EXPORT_GUID(PriorFactorLieMatrix, "gtsam::PriorFactorLieMatrix"); BOOST_CLASS_EXPORT_GUID(PriorFactorPoint2, "gtsam::PriorFactorPoint2"); BOOST_CLASS_EXPORT_GUID(PriorFactorStereoPoint2, "gtsam::PriorFactorStereoPoint2"); BOOST_CLASS_EXPORT_GUID(PriorFactorPoint3, "gtsam::PriorFactorPoint3"); @@ -182,11 +164,8 @@ BOOST_CLASS_EXPORT_GUID(PriorFactorPose3, "gtsam::PriorFactorPose3"); 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"); BOOST_CLASS_EXPORT_GUID(BetweenFactorPoint2, "gtsam::BetweenFactorPoint2"); BOOST_CLASS_EXPORT_GUID(BetweenFactorPoint3, "gtsam::BetweenFactorPoint3"); BOOST_CLASS_EXPORT_GUID(BetweenFactorRot2, "gtsam::BetweenFactorRot2"); @@ -194,8 +173,6 @@ BOOST_CLASS_EXPORT_GUID(BetweenFactorRot3, "gtsam::BetweenFactorRot3"); BOOST_CLASS_EXPORT_GUID(BetweenFactorPose2, "gtsam::BetweenFactorPose2"); BOOST_CLASS_EXPORT_GUID(BetweenFactorPose3, "gtsam::BetweenFactorPose3"); -BOOST_CLASS_EXPORT_GUID(NonlinearEqualityLieVector, "gtsam::NonlinearEqualityLieVector"); -BOOST_CLASS_EXPORT_GUID(NonlinearEqualityLieMatrix, "gtsam::NonlinearEqualityLieMatrix"); BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPoint2, "gtsam::NonlinearEqualityPoint2"); BOOST_CLASS_EXPORT_GUID(NonlinearEqualityStereoPoint2, "gtsam::NonlinearEqualityStereoPoint2"); BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPoint3, "gtsam::NonlinearEqualityPoint3"); @@ -206,7 +183,6 @@ BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPose3, "gtsam::NonlinearEqualityPose3") BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCal3_S2, "gtsam::NonlinearEqualityCal3_S2"); 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(RangeFactor2D, "gtsam::RangeFactor2D"); @@ -286,8 +262,6 @@ TEST (testSerializationSLAM, smallExample_nonlinear) { /* ************************************************************************* */ TEST (testSerializationSLAM, factors) { - LieVector lieVector((Vector(4) << 1.0, 2.0, 3.0, 4.0).finished()); - LieMatrix lieMatrix((Matrix(2, 3) << 1.0, 2.0, 3.0, 4.0, 5.0 ,6.0).finished()); Point2 point2(1.0, 2.0); StereoPoint2 stereoPoint2(1.0, 2.0, 3.0); Point3 point3(1.0, 2.0, 3.0); @@ -311,8 +285,6 @@ TEST (testSerializationSLAM, factors) { b11('b',11), b12('b',12), b13('b',13), b14('b',14), b15('b',15); Values values; - values.insert(a01, lieVector); - values.insert(a02, lieMatrix); values.insert(a03, point2); values.insert(a04, stereoPoint2); values.insert(a05, point3); @@ -344,8 +316,6 @@ TEST (testSerializationSLAM, factors) { EXPECT(equalsDereferencedXML(robust1)); EXPECT(equalsDereferencedBinary(robust1)); - PriorFactorLieVector priorFactorLieVector(a01, lieVector, model4); - PriorFactorLieMatrix priorFactorLieMatrix(a02, lieMatrix, model6); PriorFactorPoint2 priorFactorPoint2(a03, point2, model2); PriorFactorStereoPoint2 priorFactorStereoPoint2(a04, stereoPoint2, model3); PriorFactorPoint3 priorFactorPoint3(a05, point3, model3); @@ -356,11 +326,8 @@ TEST (testSerializationSLAM, factors) { PriorFactorCal3_S2 priorFactorCal3_S2(a10, cal3_s2, model5); 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); BetweenFactorPoint2 betweenFactorPoint2(a03, b03, point2, model2); BetweenFactorPoint3 betweenFactorPoint3(a05, b05, point3, model3); BetweenFactorRot2 betweenFactorRot2(a06, b06, rot2, model1); @@ -368,8 +335,6 @@ TEST (testSerializationSLAM, factors) { BetweenFactorPose2 betweenFactorPose2(a08, b08, pose2, model3); BetweenFactorPose3 betweenFactorPose3(a09, b09, pose3, model6); - NonlinearEqualityLieVector nonlinearEqualityLieVector(a01, lieVector); - NonlinearEqualityLieMatrix nonlinearEqualityLieMatrix(a02, lieMatrix); NonlinearEqualityPoint2 nonlinearEqualityPoint2(a03, point2); NonlinearEqualityStereoPoint2 nonlinearEqualityStereoPoint2(a04, stereoPoint2); NonlinearEqualityPoint3 nonlinearEqualityPoint3(a05, point3); @@ -380,7 +345,6 @@ TEST (testSerializationSLAM, factors) { NonlinearEqualityCal3_S2 nonlinearEqualityCal3_S2(a10, cal3_s2); NonlinearEqualityCal3DS2 nonlinearEqualityCal3DS2(a11, cal3ds2); NonlinearEqualityCalibratedCamera nonlinearEqualityCalibratedCamera(a12, calibratedCamera); - NonlinearEqualitySimpleCamera nonlinearEqualitySimpleCamera(a13, simpleCamera); NonlinearEqualityStereoCamera nonlinearEqualityStereoCamera(a14, stereoCamera); RangeFactor2D rangeFactor2D(a08, a03, 2.0, model1); @@ -405,8 +369,6 @@ TEST (testSerializationSLAM, factors) { NonlinearFactorGraph graph; - graph += priorFactorLieVector; - graph += priorFactorLieMatrix; graph += priorFactorPoint2; graph += priorFactorStereoPoint2; graph += priorFactorPoint3; @@ -417,11 +379,8 @@ TEST (testSerializationSLAM, factors) { graph += priorFactorCal3_S2; graph += priorFactorCal3DS2; graph += priorFactorCalibratedCamera; - graph += priorFactorSimpleCamera; graph += priorFactorStereoCamera; - graph += betweenFactorLieVector; - graph += betweenFactorLieMatrix; graph += betweenFactorPoint2; graph += betweenFactorPoint3; graph += betweenFactorRot2; @@ -429,8 +388,6 @@ TEST (testSerializationSLAM, factors) { graph += betweenFactorPose2; graph += betweenFactorPose3; - graph += nonlinearEqualityLieVector; - graph += nonlinearEqualityLieMatrix; graph += nonlinearEqualityPoint2; graph += nonlinearEqualityStereoPoint2; graph += nonlinearEqualityPoint3; @@ -441,7 +398,6 @@ TEST (testSerializationSLAM, factors) { graph += nonlinearEqualityCal3_S2; graph += nonlinearEqualityCal3DS2; graph += nonlinearEqualityCalibratedCamera; - graph += nonlinearEqualitySimpleCamera; graph += nonlinearEqualityStereoCamera; graph += rangeFactor2D; @@ -471,8 +427,6 @@ TEST (testSerializationSLAM, factors) { EXPECT(equalsObj(values)); EXPECT(equalsObj(graph)); - EXPECT(equalsObj(priorFactorLieVector)); - EXPECT(equalsObj(priorFactorLieMatrix)); EXPECT(equalsObj(priorFactorPoint2)); EXPECT(equalsObj(priorFactorStereoPoint2)); EXPECT(equalsObj(priorFactorPoint3)); @@ -483,11 +437,8 @@ TEST (testSerializationSLAM, factors) { EXPECT(equalsObj(priorFactorCal3_S2)); EXPECT(equalsObj(priorFactorCal3DS2)); EXPECT(equalsObj(priorFactorCalibratedCamera)); - EXPECT(equalsObj(priorFactorSimpleCamera)); EXPECT(equalsObj(priorFactorStereoCamera)); - EXPECT(equalsObj(betweenFactorLieVector)); - EXPECT(equalsObj(betweenFactorLieMatrix)); EXPECT(equalsObj(betweenFactorPoint2)); EXPECT(equalsObj(betweenFactorPoint3)); EXPECT(equalsObj(betweenFactorRot2)); @@ -495,8 +446,6 @@ TEST (testSerializationSLAM, factors) { EXPECT(equalsObj(betweenFactorPose2)); EXPECT(equalsObj(betweenFactorPose3)); - EXPECT(equalsObj(nonlinearEqualityLieVector)); - EXPECT(equalsObj(nonlinearEqualityLieMatrix)); EXPECT(equalsObj(nonlinearEqualityPoint2)); EXPECT(equalsObj(nonlinearEqualityStereoPoint2)); EXPECT(equalsObj(nonlinearEqualityPoint3)); @@ -507,7 +456,6 @@ TEST (testSerializationSLAM, factors) { EXPECT(equalsObj(nonlinearEqualityCal3_S2)); EXPECT(equalsObj(nonlinearEqualityCal3DS2)); EXPECT(equalsObj(nonlinearEqualityCalibratedCamera)); - EXPECT(equalsObj(nonlinearEqualitySimpleCamera)); EXPECT(equalsObj(nonlinearEqualityStereoCamera)); EXPECT(equalsObj(rangeFactor2D)); @@ -537,8 +485,6 @@ TEST (testSerializationSLAM, factors) { EXPECT(equalsXML(values)); EXPECT(equalsXML(graph)); - EXPECT(equalsXML(priorFactorLieVector)); - EXPECT(equalsXML(priorFactorLieMatrix)); EXPECT(equalsXML(priorFactorPoint2)); EXPECT(equalsXML(priorFactorStereoPoint2)); EXPECT(equalsXML(priorFactorPoint3)); @@ -549,11 +495,8 @@ TEST (testSerializationSLAM, factors) { EXPECT(equalsXML(priorFactorCal3_S2)); EXPECT(equalsXML(priorFactorCal3DS2)); EXPECT(equalsXML(priorFactorCalibratedCamera)); - EXPECT(equalsXML(priorFactorSimpleCamera)); EXPECT(equalsXML(priorFactorStereoCamera)); - EXPECT(equalsXML(betweenFactorLieVector)); - EXPECT(equalsXML(betweenFactorLieMatrix)); EXPECT(equalsXML(betweenFactorPoint2)); EXPECT(equalsXML(betweenFactorPoint3)); EXPECT(equalsXML(betweenFactorRot2)); @@ -561,8 +504,6 @@ TEST (testSerializationSLAM, factors) { EXPECT(equalsXML(betweenFactorPose2)); EXPECT(equalsXML(betweenFactorPose3)); - EXPECT(equalsXML(nonlinearEqualityLieVector)); - EXPECT(equalsXML(nonlinearEqualityLieMatrix)); EXPECT(equalsXML(nonlinearEqualityPoint2)); EXPECT(equalsXML(nonlinearEqualityStereoPoint2)); EXPECT(equalsXML(nonlinearEqualityPoint3)); @@ -573,7 +514,6 @@ TEST (testSerializationSLAM, factors) { EXPECT(equalsXML(nonlinearEqualityCal3_S2)); EXPECT(equalsXML(nonlinearEqualityCal3DS2)); EXPECT(equalsXML(nonlinearEqualityCalibratedCamera)); - EXPECT(equalsXML(nonlinearEqualitySimpleCamera)); EXPECT(equalsXML(nonlinearEqualityStereoCamera)); EXPECT(equalsXML(rangeFactor2D)); @@ -603,8 +543,6 @@ TEST (testSerializationSLAM, factors) { EXPECT(equalsBinary(values)); EXPECT(equalsBinary(graph)); - EXPECT(equalsBinary(priorFactorLieVector)); - EXPECT(equalsBinary(priorFactorLieMatrix)); EXPECT(equalsBinary(priorFactorPoint2)); EXPECT(equalsBinary(priorFactorStereoPoint2)); EXPECT(equalsBinary(priorFactorPoint3)); @@ -615,11 +553,8 @@ TEST (testSerializationSLAM, factors) { EXPECT(equalsBinary(priorFactorCal3_S2)); EXPECT(equalsBinary(priorFactorCal3DS2)); EXPECT(equalsBinary(priorFactorCalibratedCamera)); - EXPECT(equalsBinary(priorFactorSimpleCamera)); EXPECT(equalsBinary(priorFactorStereoCamera)); - EXPECT(equalsBinary(betweenFactorLieVector)); - EXPECT(equalsBinary(betweenFactorLieMatrix)); EXPECT(equalsBinary(betweenFactorPoint2)); EXPECT(equalsBinary(betweenFactorPoint3)); EXPECT(equalsBinary(betweenFactorRot2)); @@ -627,8 +562,6 @@ TEST (testSerializationSLAM, factors) { EXPECT(equalsBinary(betweenFactorPose2)); EXPECT(equalsBinary(betweenFactorPose3)); - EXPECT(equalsBinary(nonlinearEqualityLieVector)); - EXPECT(equalsBinary(nonlinearEqualityLieMatrix)); EXPECT(equalsBinary(nonlinearEqualityPoint2)); EXPECT(equalsBinary(nonlinearEqualityStereoPoint2)); EXPECT(equalsBinary(nonlinearEqualityPoint3)); @@ -639,7 +572,6 @@ TEST (testSerializationSLAM, factors) { EXPECT(equalsBinary(nonlinearEqualityCal3_S2)); EXPECT(equalsBinary(nonlinearEqualityCal3DS2)); EXPECT(equalsBinary(nonlinearEqualityCalibratedCamera)); - EXPECT(equalsBinary(nonlinearEqualitySimpleCamera)); EXPECT(equalsBinary(nonlinearEqualityStereoCamera)); EXPECT(equalsBinary(rangeFactor2D)); @@ -663,7 +595,6 @@ TEST (testSerializationSLAM, factors) { EXPECT(equalsBinary(genericStereoFactor3D)); } -#endif /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); }