diff --git a/.cproject b/.cproject index f23017ddb..38b008f93 100644 --- a/.cproject +++ b/.cproject @@ -322,14 +322,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -356,6 +348,7 @@ make + tests/testBayesTree.run true false @@ -363,6 +356,7 @@ make + testBinaryBayesNet.run true false @@ -410,6 +404,7 @@ make + testSymbolicBayesNet.run true false @@ -417,6 +412,7 @@ make + tests/testSymbolicFactor.run true false @@ -424,6 +420,7 @@ make + testSymbolicFactorGraph.run true false @@ -439,11 +436,20 @@ make + tests/testBayesTree true false true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -478,7 +484,6 @@ make - testGraph.run true false @@ -574,7 +579,6 @@ make - testInference.run true false @@ -582,7 +586,6 @@ make - testGaussianBayesNet.run true false @@ -590,7 +593,6 @@ make - testGaussianFactor.run true false @@ -598,7 +600,6 @@ make - testJunctionTree.run true false @@ -606,7 +607,6 @@ make - testSymbolicBayesNet.run true false @@ -614,7 +614,6 @@ make - testSymbolicFactorGraph.run true false @@ -684,6 +683,14 @@ true true + + make + -j2 + tests/testPlanarSLAM.run + true + true + true + make -j2 @@ -966,6 +973,7 @@ make + testErrors.run true false @@ -1325,7 +1333,6 @@ make - testSimulated2DOriented.run true false @@ -1365,7 +1372,6 @@ make - testSimulated2D.run true false @@ -1373,7 +1379,6 @@ make - testSimulated3D.run true false @@ -1461,7 +1466,6 @@ make - tests/testGaussianISAM2 true false @@ -1483,6 +1487,86 @@ true true + + make + -j2 + install + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + dist + true + true + true + + + make + -j2 + inference/tests/testEliminationTree + true + true + true + + + make + -j2 + slam/tests/testGaussianISAM2 + true + true + true + + + make + -j2 + inference/tests/testVariableIndex + true + true + true + + + make + -j2 + inference/tests/testJunctionTree + true + true + true + + + make + -j2 + linear/tests/testGaussianJunctionTree + true + true + true + make -j2 @@ -1579,94 +1663,6 @@ true true - - make - -j2 - install - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - dist - true - true - true - - - make - -j2 - inference/tests/testEliminationTree - true - true - true - - - make - -j2 - slam/tests/testGaussianISAM2 - true - true - true - - - make - -j2 - inference/tests/testVariableIndex - true - true - true - - - make - -j2 - inference/tests/testJunctionTree - true - true - true - - - make - -j2 - linear/tests/testGaussianJunctionTree - true - true - true - - - make - -j2 - check - true - true - true - make -j2 @@ -1699,6 +1695,14 @@ true true + + make + -j2 + check + true + true + true + @@ -2021,14 +2025,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -2055,6 +2051,7 @@ make + tests/testBayesTree.run true false @@ -2062,6 +2059,7 @@ make + testBinaryBayesNet.run true false @@ -2109,6 +2107,7 @@ make + testSymbolicBayesNet.run true false @@ -2116,6 +2115,7 @@ make + tests/testSymbolicFactor.run true false @@ -2123,6 +2123,7 @@ make + testSymbolicFactorGraph.run true false @@ -2138,11 +2139,20 @@ make + tests/testBayesTree true false true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -2177,7 +2187,6 @@ make - testGraph.run true false @@ -2273,7 +2282,6 @@ make - testInference.run true false @@ -2281,7 +2289,6 @@ make - testGaussianBayesNet.run true false @@ -2289,7 +2296,6 @@ make - testGaussianFactor.run true false @@ -2297,7 +2303,6 @@ make - testJunctionTree.run true false @@ -2305,7 +2310,6 @@ make - testSymbolicBayesNet.run true false @@ -2313,7 +2317,6 @@ make - testSymbolicFactorGraph.run true false @@ -2383,6 +2386,14 @@ true true + + make + -j2 + tests/testPlanarSLAM.run + true + true + true + make -j2 @@ -2665,6 +2676,7 @@ make + testErrors.run true false @@ -3024,7 +3036,6 @@ make - testSimulated2DOriented.run true false @@ -3064,7 +3075,6 @@ make - testSimulated2D.run true false @@ -3072,7 +3082,6 @@ make - testSimulated3D.run true false @@ -3160,7 +3169,6 @@ make - tests/testGaussianISAM2 true false @@ -3182,6 +3190,86 @@ true true + + make + -j2 + install + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + dist + true + true + true + + + make + -j2 + inference/tests/testEliminationTree + true + true + true + + + make + -j2 + slam/tests/testGaussianISAM2 + true + true + true + + + make + -j2 + inference/tests/testVariableIndex + true + true + true + + + make + -j2 + inference/tests/testJunctionTree + true + true + true + + + make + -j2 + linear/tests/testGaussianJunctionTree + true + true + true + make -j2 @@ -3278,94 +3366,6 @@ true true - - make - -j2 - install - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - dist - true - true - true - - - make - -j2 - inference/tests/testEliminationTree - true - true - true - - - make - -j2 - slam/tests/testGaussianISAM2 - true - true - true - - - make - -j2 - inference/tests/testVariableIndex - true - true - true - - - make - -j2 - inference/tests/testJunctionTree - true - true - true - - - make - -j2 - linear/tests/testGaussianJunctionTree - true - true - true - - - make - -j2 - check - true - true - true - make -j2 @@ -3398,6 +3398,14 @@ true true + + make + -j2 + check + true + true + true + diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index c96491c07..904d0be1c 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -30,6 +30,12 @@ namespace gtsam { namespace noiseModel { + class Gaussian; + class Diagonal; + class Constrained; + class Isotropic; + class Unit; + /** * noiseModel::Base is the abstract base class for all noise models. * @@ -389,6 +395,9 @@ namespace gtsam { public: + /* dummy constructor to allow for serialization */ + Isotropic() : Diagonal(repeat(1, 1.0)),sigma_(1.0),invsigma_(1.0) {} + typedef boost::shared_ptr shared_ptr; /** @@ -447,7 +456,7 @@ namespace gtsam { class Unit : public Isotropic { protected: - Unit(size_t dim): Isotropic(dim,1.0) {} + Unit(size_t dim=1): Isotropic(dim,1.0) {} public: diff --git a/gtsam/linear/SharedGaussian.h b/gtsam/linear/SharedGaussian.h index d14a43068..2271749b0 100644 --- a/gtsam/linear/SharedGaussian.h +++ b/gtsam/linear/SharedGaussian.h @@ -70,6 +70,12 @@ namespace gtsam { // note, deliberately not in noiseModel namespace friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { + // apparently requires that subclasses be registered? + ar.template register_type(); + ar.template register_type(); + ar.template register_type(); + ar.template register_type(); + ar.template register_type(); ar & boost::serialization::make_nvp("SharedGaussian", boost::serialization::base_object(*this)); } diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index 102cf6614..5d1f5e381 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -67,6 +67,9 @@ namespace gtsam { typedef NonlinearFactor1 Base; + /** default constructor - only for serialization */ + NonlinearEquality() {} + /** * Constructor - forces exact evaluation */ @@ -134,6 +137,19 @@ namespace gtsam { return JacobianFactor::shared_ptr(new JacobianFactor(ordering[this->key_], A, b, model)); } + private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NonlinearFactor1", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(feasible_); + ar & BOOST_SERIALIZATION_NVP(allow_error_); + ar & BOOST_SERIALIZATION_NVP(error_gain_); + } + }; // NonlinearEquality } // namespace gtsam diff --git a/gtsam/slam/BearingFactor.h b/gtsam/slam/BearingFactor.h index 84d730cea..5d4c758af 100644 --- a/gtsam/slam/BearingFactor.h +++ b/gtsam/slam/BearingFactor.h @@ -32,11 +32,15 @@ namespace gtsam { Rot2 z_; /** measurement */ + typedef BearingFactor This; typedef NonlinearFactor2 Base; public: - BearingFactor(); /* Default constructor */ + /** default constructor for serialization/testing only */ + BearingFactor() {} + + /** primary constructor */ BearingFactor(const POSEKEY& i, const POINTKEY& j, const Rot2& z, const SharedGaussian& model) : Base(model, i, j), z_(z) { @@ -54,6 +58,23 @@ namespace gtsam { return z_; } + /** equals */ + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + const This *e = dynamic_cast (&expected); + return e != NULL && Base::equals(*e, tol) && this->z_.equals(e->z_, tol); + } + + private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NonlinearFactor2", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(z_); + } + }; // BearingFactor } // namespace gtsam diff --git a/gtsam/slam/BearingRangeFactor.h b/gtsam/slam/BearingRangeFactor.h index 38353c0ca..ef13b7374 100644 --- a/gtsam/slam/BearingRangeFactor.h +++ b/gtsam/slam/BearingRangeFactor.h @@ -35,11 +35,12 @@ namespace gtsam { Rot2 bearing_; double range_; + typedef BearingRangeFactor This; typedef NonlinearFactor2 Base; public: - BearingRangeFactor(); /* Default constructor */ + BearingRangeFactor() {} /* Default constructor */ BearingRangeFactor(const POSEKEY& i, const POINTKEY& j, const Rot2& bearing, const double range, const SharedGaussian& model) : Base(model, i, j), bearing_(bearing), range_(range) { @@ -69,6 +70,26 @@ namespace gtsam { inline const std::pair measured() const { return std::make_pair(bearing_, range_); } + + /** equals */ + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + const This *e = dynamic_cast (&expected); + return e != NULL && Base::equals(*e, tol) && + fabs(this->range_ - e->range_) < tol && + this->bearing_.equals(e->bearing_, tol); + } + + private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NonlinearFactor2", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(bearing_); + ar & BOOST_SERIALIZATION_NVP(range_); + } }; // BearingRangeFactor } // namespace gtsam diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index e1c20b96e..91d7b0a55 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -38,6 +38,7 @@ namespace gtsam { private: + typedef BetweenFactor This; typedef NonlinearFactor2 Base; T measured_; /** The measurement */ @@ -47,6 +48,9 @@ namespace gtsam { // shorthand for a smart pointer to a factor typedef typename boost::shared_ptr shared_ptr; + /** default constructor - only use for serialization */ + BetweenFactor() {} + /** Constructor */ BetweenFactor(const KEY1& key1, const KEY2& key2, const T& measured, const SharedGaussian& model) : @@ -62,9 +66,8 @@ namespace gtsam { } /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol) const { - const BetweenFactor *e = - dynamic_cast*> (&expected); + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + const This *e = dynamic_cast (&expected); return e != NULL && Base::equals(*e, tol) && this->measured_.equals(e->measured_, tol); } @@ -87,6 +90,17 @@ namespace gtsam { inline std::size_t size() const { return 2; } + + private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NonlinearFactor2", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(measured_); + } }; } /// namespace gtsam diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h index 850377ec7..7174d91bb 100644 --- a/gtsam/slam/PriorFactor.h +++ b/gtsam/slam/PriorFactor.h @@ -48,6 +48,9 @@ namespace gtsam { // shorthand for a smart pointer to a factor typedef typename boost::shared_ptr shared_ptr; + /** default constructor - only use for serialization */ + PriorFactor() {} + /** Constructor */ PriorFactor(const KEY& key, const T& prior, const SharedGaussian& model) : @@ -63,7 +66,7 @@ namespace gtsam { } /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol) const { + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const PriorFactor *e = dynamic_cast*> (&expected); return e != NULL && Base::equals(*e, tol) && this->prior_.equals(e->prior_, tol); @@ -77,6 +80,17 @@ namespace gtsam { // manifold equivalent of h(x)-z -> log(z,h(x)) return prior_.logmap(p); } + + private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NonlinearFactor1", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(prior_); + } }; } /// namespace gtsam diff --git a/gtsam/slam/RangeFactor.h b/gtsam/slam/RangeFactor.h index a8afa7a04..6480f39f4 100644 --- a/gtsam/slam/RangeFactor.h +++ b/gtsam/slam/RangeFactor.h @@ -25,19 +25,20 @@ namespace gtsam { /** * Binary factor for a range measurement */ - template - class RangeFactor: public NonlinearFactor2 { + template + class RangeFactor: public NonlinearFactor2 { private: double z_; /** measurement */ - typedef NonlinearFactor2 Base; + typedef RangeFactor This; + typedef NonlinearFactor2 Base; public: - RangeFactor(); /* Default constructor */ + RangeFactor() {} /* Default constructor */ - RangeFactor(const PoseKey& i, const PointKey& j, double z, + RangeFactor(const POSEKEY& i, const POINTKEY& j, double z, const SharedGaussian& model) : Base(model, i, j), z_(z) { } @@ -53,6 +54,23 @@ namespace gtsam { inline double measured() const { return z_; } + + /** equals specialized to this factor */ + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + const This *e = dynamic_cast (&expected); + return e != NULL && Base::equals(*e, tol) && fabs(this->z_ - e->z_) < tol; + } + + private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NonlinearFactor2", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(z_); + } }; // RangeFactor } // namespace gtsam diff --git a/gtsam/slam/tests/testPlanarSLAM.cpp b/gtsam/slam/tests/testPlanarSLAM.cpp index 18b3abd85..a7f45f6d9 100644 --- a/gtsam/slam/tests/testPlanarSLAM.cpp +++ b/gtsam/slam/tests/testPlanarSLAM.cpp @@ -17,6 +17,7 @@ #include #include +#include #include #include @@ -32,6 +33,15 @@ SharedGaussian sigma2(noiseModel::Isotropic::Sigma(2,0.1)), I3(noiseModel::Unit::Create(3)); +/* ************************************************************************* */ +TEST( planarSLAM, PriorFactor_equals ) +{ + planarSLAM::Prior factor1(2, x1, I3), factor2(2, x2, I3); + EXPECT(assert_equal(factor1, factor1, 1e-5)); + EXPECT(assert_equal(factor2, factor2, 1e-5)); + EXPECT(assert_inequal(factor1, factor2, 1e-5)); +} + /* ************************************************************************* */ TEST( planarSLAM, BearingFactor ) { @@ -46,7 +56,18 @@ TEST( planarSLAM, BearingFactor ) // Check error Vector actual = factor.unwhitenedError(c); - CHECK(assert_equal(Vector_(1,-0.1),actual)); + EXPECT(assert_equal(Vector_(1,-0.1),actual)); +} + +/* ************************************************************************* */ +TEST( planarSLAM, BearingFactor_equals ) +{ + planarSLAM::Bearing + factor1(2, 3, Rot2::fromAngle(0.1), sigma), + factor2(2, 3, Rot2::fromAngle(2.3), sigma); + EXPECT(assert_equal(factor1, factor1, 1e-5)); + EXPECT(assert_equal(factor2, factor2, 1e-5)); + EXPECT(assert_inequal(factor1, factor2, 1e-5)); } /* ************************************************************************* */ @@ -63,7 +84,16 @@ TEST( planarSLAM, RangeFactor ) // Check error Vector actual = factor.unwhitenedError(c); - CHECK(assert_equal(Vector_(1,0.22),actual)); + EXPECT(assert_equal(Vector_(1,0.22),actual)); +} + +/* ************************************************************************* */ +TEST( planarSLAM, RangeFactor_equals ) +{ + planarSLAM::Range factor1(2, 3, 1.2, sigma), factor2(2, 3, 7.2, sigma); + EXPECT(assert_equal(factor1, factor1, 1e-5)); + EXPECT(assert_equal(factor2, factor2, 1e-5)); + EXPECT(assert_inequal(factor1, factor2, 1e-5)); } /* ************************************************************************* */ @@ -81,7 +111,27 @@ TEST( planarSLAM, BearingRangeFactor ) // Check error Vector actual = factor.unwhitenedError(c); - CHECK(assert_equal(Vector_(2,-0.1, 0.22),actual)); + EXPECT(assert_equal(Vector_(2,-0.1, 0.22),actual)); +} + +/* ************************************************************************* */ +TEST( planarSLAM, BearingRangeFactor_equals ) +{ + planarSLAM::BearingRange + factor1(2, 3, Rot2::fromAngle(0.1), 7.3, sigma2), + factor2(2, 3, Rot2::fromAngle(3), 2.0, sigma2); + EXPECT(assert_equal(factor1, factor1, 1e-5)); + EXPECT(assert_equal(factor2, factor2, 1e-5)); + EXPECT(assert_inequal(factor1, factor2, 1e-5)); +} + +/* ************************************************************************* */ +TEST( planarSLAM, PoseConstraint_equals ) +{ + planarSLAM::Constraint factor1(2, x2), factor2(2, x3); + EXPECT(assert_equal(factor1, factor1, 1e-5)); + EXPECT(assert_equal(factor2, factor2, 1e-5)); + EXPECT(assert_inequal(factor1, factor2, 1e-5)); } /* ************************************************************************* */ @@ -111,7 +161,7 @@ TEST( planarSLAM, constructor ) G.addRange(2, 3, z2, sigma); Vector expected = Vector_(8, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.1, 0.22); - CHECK(assert_equal(expected,G.unwhitenedError(c))); + EXPECT(assert_equal(expected,G.unwhitenedError(c))); } /* ************************************************************************* */ diff --git a/tests/testSerialization.cpp b/tests/testSerialization.cpp index 348200052..5c037831f 100644 --- a/tests/testSerialization.cpp +++ b/tests/testSerialization.cpp @@ -147,6 +147,7 @@ bool equalsDereferencedXML(const T& input = T()) { //#include #include +#include #include @@ -177,7 +178,6 @@ TEST (Serialization, xml_geometry) { EXPECT(equalsXML(pt3)); EXPECT(equalsXML(rt3)); EXPECT(equalsXML(Pose3(rt3, pt3))); - } /* ************************************************************************* */ @@ -195,16 +195,44 @@ TEST (Serialization, xml_linear) { } /* ************************************************************************* */ -TEST (Serialization, noiseModels) { +TEST (Serialization, Shared_noiseModels) { SharedDiagonal diag3 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.2, 0.3)); - SharedGaussian model3 = noiseModel::Isotropic::Sigma(3, 0.3); + SharedGaussian iso3 = noiseModel::Isotropic::Sigma(3, 0.3); + SharedGaussian gaussian3 = noiseModel::Gaussian::SqrtInformation(eye(3,3)); EXPECT(equalsDereferenced(diag3)); EXPECT(equalsDereferencedXML(diag3)); - // FAIL: Segfaults -// EXPECT(equalsDereferenced(model3)); -// EXPECT(equalsDereferencedXML(model3)); + EXPECT(equalsDereferenced(iso3)); + EXPECT(equalsDereferencedXML(iso3)); + + EXPECT(equalsDereferenced(gaussian3)); + EXPECT(equalsDereferencedXML(gaussian3)); +} + +/* ************************************************************************* */ +TEST (Serialization, noiseModels) { + noiseModel::Diagonal::shared_ptr diag3 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.2, 0.3)); + noiseModel::Gaussian::shared_ptr gaussian3 = noiseModel::Gaussian::SqrtInformation(2.0 * eye(3,3)); + noiseModel::Isotropic::shared_ptr iso3 = noiseModel::Isotropic::Sigma(3, 0.2); + noiseModel::Constrained::shared_ptr constrained3 = noiseModel::Constrained::All(3); + noiseModel::Unit::shared_ptr unit3 = noiseModel::Unit::Create(3); + + EXPECT( equalsDereferenced(diag3)); + EXPECT(equalsDereferencedXML(diag3)); + + EXPECT( equalsDereferenced(gaussian3)); + EXPECT(equalsDereferencedXML(gaussian3)); + + EXPECT( equalsDereferenced(iso3)); + EXPECT(equalsDereferencedXML(iso3)); + + // FAIL: stream error +// EXPECT( equalsDereferenced(constrained3)); +// EXPECT(equalsDereferencedXML(constrained3)); + + EXPECT( equalsDereferenced(unit3)); + EXPECT(equalsDereferencedXML(unit3)); } /* ************************************************************************* */ @@ -218,22 +246,43 @@ TEST (Serialization, planar_system) { SharedGaussian model1 = noiseModel::Isotropic::Sigma(1, 0.3); SharedGaussian model2 = noiseModel::Isotropic::Sigma(2, 0.3); SharedGaussian model3 = noiseModel::Isotropic::Sigma(3, 0.3); + Graph graph; - graph.addBearing(PoseKey(3), PointKey(5), Rot2::fromDegrees(0.5), model1); - graph.addRange(PoseKey(2), PointKey(9), 7.0, model1); - graph.addBearingRange(PoseKey(2), PointKey(3), Rot2::fromDegrees(0.6), 2.0, model2); - graph.addOdometry(PoseKey(2), PoseKey(3), Pose2(1.0, 2.0, 0.3), model3); + Prior prior(PoseKey(3), Pose2(0.1,-0.3, 0.2), model1); + graph.add(prior); + Bearing bearing(PoseKey(3), PointKey(5), Rot2::fromDegrees(0.5), model1); + graph.add(bearing); + Range range(PoseKey(2), PointKey(9), 7.0, model1); + graph.add(range); + BearingRange bearingRange(PoseKey(2), PointKey(3), Rot2::fromDegrees(0.6), 2.0, model2); + graph.add(bearingRange); + Odometry odometry(PoseKey(2), PoseKey(3), Pose2(1.0, 2.0, 0.3), model3); + graph.add(odometry); + Constraint constraint(PoseKey(9), Pose2(2.0,-1.0, 0.2)); + graph.add(constraint); // text EXPECT(equalsObj(PoseKey(2))); EXPECT(equalsObj(PointKey(3))); EXPECT(equalsObj(values)); -// EXPECT(equalsObj(graph)); + EXPECT(equalsObj(prior)); + EXPECT(equalsObj(bearing)); + EXPECT(equalsObj(bearingRange)); + EXPECT(equalsObj(range)); + EXPECT(equalsObj(odometry)); +// EXPECT(equalsObj(constraint)); // FAIL: stream error +// EXPECT(equalsObj(graph)); // FAIL: segfaults if there are factors // xml EXPECT(equalsXML(PoseKey(2))); EXPECT(equalsXML(PointKey(3))); EXPECT(equalsXML(values)); + EXPECT(equalsXML(prior)); + EXPECT(equalsXML(bearing)); + EXPECT(equalsXML(bearingRange)); + EXPECT(equalsXML(range)); + EXPECT(equalsXML(odometry)); +// EXPECT(equalsXML(constraint)); // FAIL: stream error // EXPECT(equalsXML(graph)); }