From c2daf40c5df71258abb6f498af2ac37be044035f Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Mon, 23 Jul 2012 22:50:21 +0000 Subject: [PATCH] Removed SLAM namespaces from testSerializationSLAM. Instead of testing each SLAM namespace individually, a single test was created that instanciates all variable-factor combinations. However, there is currently an issue with LieVector that should be resolved. --- tests/testSerializationSLAM.cpp | 525 +++++++++++++++++++++++++------- 1 file changed, 422 insertions(+), 103 deletions(-) diff --git a/tests/testSerializationSLAM.cpp b/tests/testSerializationSLAM.cpp index d9094111e..27f74c222 100644 --- a/tests/testSerializationSLAM.cpp +++ b/tests/testSerializationSLAM.cpp @@ -18,11 +18,31 @@ #include -#include -#include +#include +#include +#include +#include +#include +#include +#include #include #include #include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + #include #include @@ -30,17 +50,70 @@ using namespace std; 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; +typedef PriorFactor PriorFactorRot2; +typedef PriorFactor PriorFactorRot3; +typedef PriorFactor PriorFactorPose2; +typedef PriorFactor PriorFactorPose3; +typedef PriorFactor PriorFactorCal3_S2; +typedef PriorFactor PriorFactorCal3DS2; +typedef PriorFactor PriorFactorCalibratedCamera; +typedef PriorFactor PriorFactorSimpleCamera; + +typedef BetweenFactor BetweenFactorLieVector; +typedef BetweenFactor BetweenFactorLieMatrix; +typedef BetweenFactor BetweenFactorPoint2; +typedef BetweenFactor BetweenFactorPoint3; +typedef BetweenFactor BetweenFactorRot2; +typedef BetweenFactor BetweenFactorRot3; +typedef BetweenFactor BetweenFactorPose2; +typedef BetweenFactor BetweenFactorPose3; + +typedef NonlinearEquality NonlinearEqualityLieVector; +typedef NonlinearEquality NonlinearEqualityLieMatrix; +typedef NonlinearEquality NonlinearEqualityPoint2; +typedef NonlinearEquality NonlinearEqualityStereoPoint2; +typedef NonlinearEquality NonlinearEqualityPoint3; +typedef NonlinearEquality NonlinearEqualityRot2; +typedef NonlinearEquality NonlinearEqualityRot3; +typedef NonlinearEquality NonlinearEqualityPose2; +typedef NonlinearEquality NonlinearEqualityPose3; +typedef NonlinearEquality NonlinearEqualityCal3_S2; +typedef NonlinearEquality NonlinearEqualityCal3DS2; +typedef NonlinearEquality NonlinearEqualityCalibratedCamera; +typedef NonlinearEquality NonlinearEqualitySimpleCamera; + +typedef RangeFactor RangeFactorPosePoint2; +typedef RangeFactor RangeFactorPosePoint3; +typedef RangeFactor RangeFactorPose2; +typedef RangeFactor RangeFactorPose3; +typedef RangeFactor RangeFactorCalibratedCameraPoint; +typedef RangeFactor RangeFactorSimpleCameraPoint; +typedef RangeFactor RangeFactorCalibratedCamera; +typedef RangeFactor RangeFactorSimpleCamera; + +typedef BearingFactor BearingFactor2D; +typedef BearingFactor BearingFactor3D; + +typedef BearingRangeFactor BearingRangeFactor2D; +typedef BearingRangeFactor BearingRangeFactor3D; + +typedef GenericProjectionFactor GenericProjectionFactorCal3_S2; +typedef GenericProjectionFactor GenericProjectionFactorCal3DS2; + + // Convenience for named keys using symbol_shorthand::X; using symbol_shorthand::L; -/* Create GUIDs for factors */ -/* ************************************************************************* */ -BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor"); -BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor"); +/* Create GUIDs for Noisemodels */ /* ************************************************************************* */ -// Export Noisemodels BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian"); @@ -50,6 +123,82 @@ BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropi BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); +/* Create GUIDs for geometry */ +/* ************************************************************************* */ +BOOST_CLASS_EXPORT(gtsam::LieVector); +BOOST_CLASS_EXPORT(gtsam::LieMatrix); +BOOST_CLASS_EXPORT(gtsam::Point2); +BOOST_CLASS_EXPORT(gtsam::StereoPoint2); +BOOST_CLASS_EXPORT(gtsam::Point3); +BOOST_CLASS_EXPORT(gtsam::Rot2); +BOOST_CLASS_EXPORT(gtsam::Rot3); +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::CalibratedCamera); +BOOST_CLASS_EXPORT(gtsam::SimpleCamera); + + +/* Create GUIDs for factors */ +/* ************************************************************************* */ +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"); +BOOST_CLASS_EXPORT_GUID(PriorFactorRot2, "gtsam::PriorFactorRot2"); +BOOST_CLASS_EXPORT_GUID(PriorFactorRot3, "gtsam::PriorFactorRot3"); +BOOST_CLASS_EXPORT_GUID(PriorFactorPose2, "gtsam::PriorFactorPose2"); +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(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"); +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"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityRot2, "gtsam::NonlinearEqualityRot2"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityRot3, "gtsam::NonlinearEqualityRot3"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPose2, "gtsam::NonlinearEqualityPose2"); +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(RangeFactorPosePoint2, "gtsam::RangeFactorPosePoint2"); +BOOST_CLASS_EXPORT_GUID(RangeFactorPosePoint3, "gtsam::RangeFactorPosePoint3"); +BOOST_CLASS_EXPORT_GUID(RangeFactorPose2, "gtsam::RangeFactorPose2"); +BOOST_CLASS_EXPORT_GUID(RangeFactorPose3, "gtsam::RangeFactorPose3"); +BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCameraPoint, "gtsam::RangeFactorCalibratedCameraPoint"); +BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCameraPoint, "gtsam::RangeFactorSimpleCameraPoint"); +BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCamera, "gtsam::RangeFactorCalibratedCamera"); +BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCamera, "gtsam::RangeFactorSimpleCamera"); + +BOOST_CLASS_EXPORT_GUID(BearingFactor2D, "gtsam::BearingFactor2D"); + +BOOST_CLASS_EXPORT_GUID(BearingRangeFactor2D, "gtsam::BearingRangeFactor2D"); + +BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3_S2, "gtsam::GenericProjectionFactorCal3_S2"); +BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3DS2, "gtsam::GenericProjectionFactorCal3DS2"); + + /* ************************************************************************* */ TEST (Serialization, smallExample_linear) { using namespace example; @@ -85,12 +234,11 @@ TEST (Serialization, gaussianISAM) { BOOST_CLASS_EXPORT_GUID(simulated2D::Prior, "gtsam::simulated2D::Prior" ) BOOST_CLASS_EXPORT_GUID(simulated2D::Odometry, "gtsam::simulated2D::Odometry" ) BOOST_CLASS_EXPORT_GUID(simulated2D::Measurement, "gtsam::simulated2D::Measurement") -BOOST_CLASS_EXPORT(gtsam::Point2) /* ************************************************************************* */ -TEST (Serialization, smallExample) { +TEST (Serialization, smallExample_nonlinear) { using namespace example; - Graph nfg = createNonlinearFactorGraph(); + NonlinearFactorGraph nfg = createNonlinearFactorGraph(); Values c1 = createValues(); EXPECT(equalsObj(nfg)); EXPECT(equalsXML(nfg)); @@ -100,113 +248,284 @@ TEST (Serialization, smallExample) { } /* ************************************************************************* */ -/* Create GUIDs for factors */ -BOOST_CLASS_EXPORT_GUID(planarSLAM::Prior, "gtsam::planarSLAM::Prior"); -BOOST_CLASS_EXPORT_GUID(planarSLAM::Bearing, "gtsam::planarSLAM::Bearing"); -BOOST_CLASS_EXPORT_GUID(planarSLAM::Range, "gtsam::planarSLAM::Range"); -BOOST_CLASS_EXPORT_GUID(planarSLAM::BearingRange,"gtsam::planarSLAM::BearingRange"); -BOOST_CLASS_EXPORT_GUID(planarSLAM::Odometry, "gtsam::planarSLAM::Odometry"); -BOOST_CLASS_EXPORT_GUID(planarSLAM::Constraint, "gtsam::planarSLAM::Constraint"); +TEST (Serialization, factors) { -BOOST_CLASS_EXPORT(gtsam::Pose2) + LieVector lieVector(4, 1.0, 2.0, 3.0, 4.0); + LieMatrix lieMatrix(2, 3, 1.0, 2.0, 3.0, 4.0, 5.0 ,6.0); + Point2 point2(1.0, 2.0); + StereoPoint2 stereoPoint2(1.0, 2.0, 3.0); + Point3 point3(1.0, 2.0, 3.0); + Rot2 rot2(1.0); + Rot3 rot3(Rot3::RzRyRx(1.0, 2.0, 3.0)); + Pose2 pose2(rot2, point2); + 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); + CalibratedCamera calibratedCamera(pose3); + SimpleCamera simpleCamera(pose3, cal3_s2); + + + 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); + + Values values; +// values.insert(a01, lieVector); +// values.insert(a02, lieMatrix); + values.insert(a03, point2); + values.insert(a04, stereoPoint2); + values.insert(a05, point3); + values.insert(a06, rot2); + values.insert(a07, rot3); + values.insert(a08, pose2); + values.insert(a09, pose3); + values.insert(a10, cal3_s2); + values.insert(a11, cal3ds2); + values.insert(a12, calibratedCamera); + values.insert(a13, simpleCamera); -/* ************************************************************************* */ -TEST (Serialization, planar_system) { - using namespace planarSLAM; - planarSLAM::Values values; - Symbol i2('x',2), i3('x',3), i4('x',4), i9('x',9), j3('l',3), j5('l',5), j9('l',9); - values.insert(j3, Point2(1.0, 2.0)); - values.insert(i4, Pose2(1.0, 2.0, 0.3)); SharedNoiseModel model1 = noiseModel::Isotropic::Sigma(1, 0.3); SharedNoiseModel model2 = noiseModel::Isotropic::Sigma(2, 0.3); SharedNoiseModel model3 = noiseModel::Isotropic::Sigma(3, 0.3); + SharedNoiseModel model4 = noiseModel::Isotropic::Sigma(4, 0.3); + SharedNoiseModel model5 = noiseModel::Isotropic::Sigma(5, 0.3); + SharedNoiseModel model6 = noiseModel::Isotropic::Sigma(6, 0.3); + SharedNoiseModel model9 = noiseModel::Isotropic::Sigma(9, 0.3); + SharedNoiseModel model11 = noiseModel::Isotropic::Sigma(11, 0.3); - Prior prior(i3, Pose2(0.1,-0.3, 0.2), model1); - Bearing bearing(i3, j5, Rot2::fromDegrees(0.5), model1); - Range range(i2, j9, 7.0, model1); - BearingRange bearingRange(i2, j3, Rot2::fromDegrees(0.6), 2.0, model2); - Odometry odometry(i2, i3, Pose2(1.0, 2.0, 0.3), model3); - Constraint constraint(i9, Pose2(2.0,-1.0, 0.2)); - Graph graph; - graph.add(prior); - graph.add(bearing); - graph.add(range); - graph.add(bearingRange); - graph.add(odometry); - graph.add(constraint); + + 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); + PriorFactorRot2 priorFactorRot2(a06, rot2, model1); + PriorFactorRot3 priorFactorRot3(a07, rot3, model3); + PriorFactorPose2 priorFactorPose2(a08, pose2, model3); + PriorFactorPose3 priorFactorPose3(a09, pose3, model6); + PriorFactorCal3_S2 priorFactorCal3_S2(a10, cal3_s2, model5); + PriorFactorCal3DS2 priorFactorCal3DS2(a11, cal3ds2, model9); + PriorFactorCalibratedCamera priorFactorCalibratedCamera(a12, calibratedCamera, model6); + PriorFactorSimpleCamera priorFactorSimpleCamera(a13, simpleCamera, 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); + BetweenFactorRot3 betweenFactorRot3(a07, b07, rot3, model3); + 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); + NonlinearEqualityRot2 nonlinearEqualityRot2(a06, rot2); + NonlinearEqualityRot3 nonlinearEqualityRot3(a07, rot3); + NonlinearEqualityPose2 nonlinearEqualityPose2(a08, pose2); + NonlinearEqualityPose3 nonlinearEqualityPose3(a09, pose3); + NonlinearEqualityCal3_S2 nonlinearEqualityCal3_S2(a10, cal3_s2); + NonlinearEqualityCal3DS2 nonlinearEqualityCal3DS2(a11, cal3ds2); + NonlinearEqualityCalibratedCamera nonlinearEqualityCalibratedCamera(a12, calibratedCamera); + NonlinearEqualitySimpleCamera nonlinearEqualitySimpleCamera(a13, simpleCamera); + + RangeFactorPosePoint2 rangeFactorPosePoint2(a08, a03, 2.0, model1); + RangeFactorPosePoint3 rangeFactorPosePoint3(a09, a05, 2.0, model1); + RangeFactorPose2 rangeFactorPose2(a08, b08, 2.0, model1); + RangeFactorPose3 rangeFactorPose3(a09, b09, 2.0, model1); + RangeFactorCalibratedCameraPoint rangeFactorCalibratedCameraPoint(a12, a05, 2.0, model1); + RangeFactorSimpleCameraPoint rangeFactorSimpleCameraPoint(a13, a05, 2.0, model1); + RangeFactorCalibratedCamera rangeFactorCalibratedCamera(a12, b12, 2.0, model1); + RangeFactorSimpleCamera rangeFactorSimpleCamera(a13, b13, 2.0, model1); + + BearingFactor2D bearingFactor2D(a08, a03, rot2, model1); + + BearingRangeFactor2D bearingRangeFactor2D(a08, a03, rot2, 2.0, model2); + + GenericProjectionFactorCal3_S2 genericProjectionFactorCal3_S2(point2, model2, a09, a05, boost::make_shared(cal3_s2)); + GenericProjectionFactorCal3DS2 genericProjectionFactorCal3DS2(point2, model2, a09, a05, boost::make_shared(cal3ds2)); + + + + + NonlinearFactorGraph graph; +// graph.add(priorFactorLieVector); +// graph.add(priorFactorLieMatrix); + graph.add(priorFactorPoint2); + graph.add(priorFactorStereoPoint2); + graph.add(priorFactorPoint3); + graph.add(priorFactorRot2); + graph.add(priorFactorRot3); + graph.add(priorFactorPose2); + graph.add(priorFactorPose3); + graph.add(priorFactorCal3_S2); + graph.add(priorFactorCal3_S2); + graph.add(priorFactorCalibratedCamera); + graph.add(priorFactorSimpleCamera); + +// graph.add(betweenFactorLieVector); +// graph.add(betweenFactorLieMatrix); + graph.add(betweenFactorPoint2); + graph.add(betweenFactorPoint3); + graph.add(betweenFactorRot2); + graph.add(betweenFactorRot3); + graph.add(betweenFactorPose2); + graph.add(betweenFactorPose3); + +// graph.add(nonlinearEqualityLieVector); +// graph.add(nonlinearEqualityLieMatrix); + graph.add(nonlinearEqualityPoint2); + graph.add(nonlinearEqualityStereoPoint2); + graph.add(nonlinearEqualityPoint3); + graph.add(nonlinearEqualityRot2); + graph.add(nonlinearEqualityRot3); + graph.add(nonlinearEqualityPose2); + graph.add(nonlinearEqualityPose3); + graph.add(nonlinearEqualityCal3_S2); + graph.add(nonlinearEqualityCal3DS2); + graph.add(nonlinearEqualityCalibratedCamera); + graph.add(nonlinearEqualitySimpleCamera); + + graph.add(rangeFactorPosePoint2); + graph.add(rangeFactorPosePoint3); + graph.add(rangeFactorPose2); + graph.add(rangeFactorPose3); + graph.add(rangeFactorCalibratedCameraPoint); + graph.add(rangeFactorSimpleCameraPoint); + graph.add(rangeFactorCalibratedCamera); + graph.add(rangeFactorSimpleCamera); + + graph.add(bearingFactor2D); + + graph.add(bearingRangeFactor2D); + + graph.add(genericProjectionFactorCal3_S2); + graph.add(genericProjectionFactorCal3DS2); + // text - EXPECT(equalsObj(i2)); - EXPECT(equalsObj(j3)); - EXPECT(equalsObj(values)); - EXPECT(equalsObj(prior)); - EXPECT(equalsObj(bearing)); - EXPECT(equalsObj(bearingRange)); - EXPECT(equalsObj(range)); - EXPECT(equalsObj(odometry)); - EXPECT(equalsObj(constraint)); - EXPECT(equalsObj(graph)); + EXPECT(equalsObj(a01)); + EXPECT(equalsObj(b02)); + EXPECT(equalsObj(values)); + EXPECT(equalsObj(graph)); + +// EXPECT(equalsObj(priorFactorLieVector)); +// EXPECT(equalsObj(priorFactorLieMatrix)); + EXPECT(equalsObj(priorFactorPoint2)); + EXPECT(equalsObj(priorFactorStereoPoint2)); + EXPECT(equalsObj(priorFactorPoint3)); + EXPECT(equalsObj(priorFactorRot2)); + EXPECT(equalsObj(priorFactorRot3)); + EXPECT(equalsObj(priorFactorPose2)); + EXPECT(equalsObj(priorFactorPose3)); + EXPECT(equalsObj(priorFactorCal3_S2)); + EXPECT(equalsObj(priorFactorCal3DS2)); + EXPECT(equalsObj(priorFactorCalibratedCamera)); + EXPECT(equalsObj(priorFactorSimpleCamera)); + +// EXPECT(equalsObj(betweenFactorLieVector)); +// EXPECT(equalsObj(betweenFactorLieMatrix)); + EXPECT(equalsObj(betweenFactorPoint2)); + EXPECT(equalsObj(betweenFactorPoint3)); + EXPECT(equalsObj(betweenFactorRot2)); + EXPECT(equalsObj(betweenFactorRot3)); + EXPECT(equalsObj(betweenFactorPose2)); + EXPECT(equalsObj(betweenFactorPose3)); + +// EXPECT(equalsObj(nonlinearEqualityLieVector)); +// EXPECT(equalsObj(nonlinearEqualityLieMatrix)); + EXPECT(equalsObj(nonlinearEqualityPoint2)); + EXPECT(equalsObj(nonlinearEqualityStereoPoint2)); + EXPECT(equalsObj(nonlinearEqualityPoint3)); + EXPECT(equalsObj(nonlinearEqualityRot2)); + EXPECT(equalsObj(nonlinearEqualityRot3)); + EXPECT(equalsObj(nonlinearEqualityPose2)); + EXPECT(equalsObj(nonlinearEqualityPose3)); + EXPECT(equalsObj(nonlinearEqualityCal3_S2)); + EXPECT(equalsObj(nonlinearEqualityCal3DS2)); + EXPECT(equalsObj(nonlinearEqualityCalibratedCamera)); + EXPECT(equalsObj(nonlinearEqualitySimpleCamera)); + + EXPECT(equalsObj(rangeFactorPosePoint2)); + EXPECT(equalsObj(rangeFactorPosePoint3)); + EXPECT(equalsObj(rangeFactorPose2)); + EXPECT(equalsObj(rangeFactorPose3)); + EXPECT(equalsObj(rangeFactorCalibratedCameraPoint)); + EXPECT(equalsObj(rangeFactorSimpleCameraPoint)); + EXPECT(equalsObj(rangeFactorCalibratedCamera)); + EXPECT(equalsObj(rangeFactorSimpleCamera)); + + EXPECT(equalsObj(bearingFactor2D)); + + EXPECT(equalsObj(bearingRangeFactor2D)); + + EXPECT(equalsObj(genericProjectionFactorCal3_S2)); + EXPECT(equalsObj(genericProjectionFactorCal3DS2)); + // xml - EXPECT(equalsXML(i2)); - EXPECT(equalsXML(j3)); - EXPECT(equalsXML(values)); - EXPECT(equalsXML(prior)); - EXPECT(equalsXML(bearing)); - EXPECT(equalsXML(bearingRange)); - EXPECT(equalsXML(range)); - EXPECT(equalsXML(odometry)); - EXPECT(equalsXML(constraint)); - EXPECT(equalsXML(graph)); + EXPECT(equalsXML(a01)); + EXPECT(equalsXML(b02)); + EXPECT(equalsXML(values)); + EXPECT(equalsXML(graph)); + +// EXPECT(equalsXML(priorFactorLieVector)); +// EXPECT(equalsXML(priorFactorLieMatrix)); + EXPECT(equalsXML(priorFactorPoint2)); + EXPECT(equalsXML(priorFactorStereoPoint2)); + EXPECT(equalsXML(priorFactorPoint3)); + EXPECT(equalsXML(priorFactorRot2)); + EXPECT(equalsXML(priorFactorRot3)); + EXPECT(equalsXML(priorFactorPose2)); + EXPECT(equalsXML(priorFactorPose3)); + EXPECT(equalsXML(priorFactorCal3_S2)); + EXPECT(equalsXML(priorFactorCal3DS2)); + EXPECT(equalsXML(priorFactorCalibratedCamera)); + EXPECT(equalsXML(priorFactorSimpleCamera)); + +// EXPECT(equalsXML(betweenFactorLieVector)); +// EXPECT(equalsXML(betweenFactorLieMatrix)); + EXPECT(equalsXML(betweenFactorPoint2)); + EXPECT(equalsXML(betweenFactorPoint3)); + EXPECT(equalsXML(betweenFactorRot2)); + EXPECT(equalsXML(betweenFactorRot3)); + EXPECT(equalsXML(betweenFactorPose2)); + EXPECT(equalsXML(betweenFactorPose3)); + +// EXPECT(equalsXML(nonlinearEqualityLieVector)); +// EXPECT(equalsXML(nonlinearEqualityLieMatrix)); + EXPECT(equalsXML(nonlinearEqualityPoint2)); + EXPECT(equalsXML(nonlinearEqualityStereoPoint2)); + EXPECT(equalsXML(nonlinearEqualityPoint3)); + EXPECT(equalsXML(nonlinearEqualityRot2)); + EXPECT(equalsXML(nonlinearEqualityRot3)); + EXPECT(equalsXML(nonlinearEqualityPose2)); + EXPECT(equalsXML(nonlinearEqualityPose3)); + EXPECT(equalsXML(nonlinearEqualityCal3_S2)); + EXPECT(equalsXML(nonlinearEqualityCal3DS2)); + EXPECT(equalsXML(nonlinearEqualityCalibratedCamera)); + EXPECT(equalsXML(nonlinearEqualitySimpleCamera)); + + EXPECT(equalsXML(rangeFactorPosePoint2)); + EXPECT(equalsXML(rangeFactorPosePoint3)); + EXPECT(equalsXML(rangeFactorPose2)); + EXPECT(equalsXML(rangeFactorPose3)); + EXPECT(equalsXML(rangeFactorCalibratedCameraPoint)); + EXPECT(equalsXML(rangeFactorSimpleCameraPoint)); + EXPECT(equalsXML(rangeFactorCalibratedCamera)); + EXPECT(equalsXML(rangeFactorSimpleCamera)); + + EXPECT(equalsXML(bearingFactor2D)); + + EXPECT(equalsXML(bearingRangeFactor2D)); + + EXPECT(equalsXML(genericProjectionFactorCal3_S2)); + EXPECT(equalsXML(genericProjectionFactorCal3DS2)); } -/* ************************************************************************* */ -/* Create GUIDs for factors */ -BOOST_CLASS_EXPORT_GUID(visualSLAM::PoseConstraint, "gtsam::visualSLAM::PoseConstraint"); -BOOST_CLASS_EXPORT_GUID(visualSLAM::PointConstraint, "gtsam::visualSLAM::PointConstraint"); -BOOST_CLASS_EXPORT_GUID(visualSLAM::PosePrior, "gtsam::visualSLAM::PosePrior"); -BOOST_CLASS_EXPORT_GUID(visualSLAM::PointPrior, "gtsam::visualSLAM::PointPrior"); -BOOST_CLASS_EXPORT_GUID(visualSLAM::ProjectionFactor,"gtsam::visualSLAM::ProjectionFactor"); -BOOST_CLASS_EXPORT_GUID(visualSLAM::StereoFactor, "gtsam::visualSLAM::StereoFactor"); - -BOOST_CLASS_EXPORT(gtsam::Pose3) -BOOST_CLASS_EXPORT(gtsam::Point3) - -static Point3 pt3(1.0, 2.0, 3.0); -static Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0); -static Pose3 pose3(rt3, pt3); -static Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5); - -/* ************************************************************************* */ -TEST (Serialization, visual_system) { - using namespace visualSLAM; - visualSLAM::Values values; - Symbol x1('x',1), x2('x',2); - Symbol l1('l',1), l2('l',2); - Pose3 pose1 = pose3, pose2 = pose3.inverse(); - Point3 pt1(1.0, 2.0, 3.0), pt2(4.0, 5.0, 6.0); - values.insert(x1, pose1); - values.insert(l1, pt1); - SharedNoiseModel model2 = noiseModel::Isotropic::Sigma(2, 0.3); - SharedNoiseModel model3 = noiseModel::Isotropic::Sigma(3, 0.3); - SharedNoiseModel model6 = noiseModel::Isotropic::Sigma(6, 0.3); - boost::shared_ptr K(new Cal3_S2(cal1)); - - Graph graph; - graph.addMeasurement(Point2(1.0, 2.0), model2, x1, l1, K); - graph.addPointConstraint(l1, pt1); - graph.addPointPrior(l1, pt2, model3); - graph.addPoseConstraint(x1, pose1); - graph.addPosePrior(x1, pose2, model6); - - EXPECT(equalsObj(values)); - EXPECT(equalsObj(graph)); - - EXPECT(equalsXML(values)); - EXPECT(equalsXML(graph)); -} /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); }