From 4865f1a64d6b587edb1ca664632d90b7290b7442 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Wed, 23 Feb 2011 05:19:07 +0000 Subject: [PATCH] Added more serialization functionality to noiseModel, but SharedGaussians segfault on test, so tests for Nonlinear graphs are commented out --- gtsam/linear/NoiseModel.cpp | 43 ++++++++++++++-- gtsam/linear/NoiseModel.h | 64 +++++++++++++++++++++-- gtsam/linear/SharedDiagonal.h | 10 +++- gtsam/linear/SharedGaussian.h | 8 +++ gtsam/linear/tests/testNoiseModel.cpp | 62 +++++++++++++--------- gtsam/nonlinear/NonlinearFactor.h | 9 ++-- gtsam/slam/BearingFactor.h | 1 + tests/testSerialization.cpp | 74 ++++++++++++++++++++++----- 8 files changed, 219 insertions(+), 52 deletions(-) diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 9db82a4d3..e2ef0cb32 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -82,10 +82,12 @@ Gaussian::shared_ptr Gaussian::Covariance(const Matrix& covariance, bool smart) full: return shared_ptr(new Gaussian(n, inverse_square_root(covariance))); } +/* ************************************************************************* */ void Gaussian::print(const string& name) const { gtsam::print(thisR(), "Gaussian"); } +/* ************************************************************************* */ bool Gaussian::equals(const Base& expected, double tol) const { const Gaussian* p = dynamic_cast (&expected); if (p == NULL) return false; @@ -94,33 +96,39 @@ bool Gaussian::equals(const Base& expected, double tol) const { return equal_with_abs_tol(R(), p->R(), sqrt(tol)); } +/* ************************************************************************* */ Vector Gaussian::whiten(const Vector& v) const { return thisR() * v; } - +/* ************************************************************************* */ Vector Gaussian::unwhiten(const Vector& v) const { return backSubstituteUpper(thisR(), v); } +/* ************************************************************************* */ double Gaussian::Mahalanobis(const Vector& v) const { // Note: for Diagonal, which does ediv_, will be correct for constraints Vector w = whiten(v); return inner_prod(w, w); } +/* ************************************************************************* */ Matrix Gaussian::Whiten(const Matrix& H) const { return thisR() * H; } +/* ************************************************************************* */ void Gaussian::WhitenInPlace(Matrix& H) const { H = thisR() * H; } +/* ************************************************************************* */ void Gaussian::WhitenInPlace(MatrixColMajor& H) const { H = ublas::prod(thisR(), H); } +/* ************************************************************************* */ // General QR, see also special version in Constrained SharedDiagonal Gaussian::QR(Matrix& Ab, boost::optional&> firstZeroRows) const { @@ -285,11 +293,14 @@ SharedDiagonal Gaussian::Cholesky(MatrixColMajor& Ab, size_t nFrontals) const { return Unit::Create(maxrank); } +/* ************************************************************************* */ +// Diagonal /* ************************************************************************* */ Diagonal::Diagonal(const Vector& sigmas) : Gaussian(sigmas.size()), sigmas_(sigmas), invsigmas_(reciprocal(sigmas)) { } +/* ************************************************************************* */ Diagonal::shared_ptr Diagonal::Variances(const Vector& variances, bool smart) { if (smart) { // check whether all the same entry @@ -301,6 +312,7 @@ Diagonal::shared_ptr Diagonal::Variances(const Vector& variances, bool smart) { full: return shared_ptr(new Diagonal(esqrt(variances))); } +/* ************************************************************************* */ Diagonal::shared_ptr Diagonal::Sigmas(const Vector& sigmas, bool smart) { if (smart) { // look for zeros to make a constraint @@ -311,31 +323,38 @@ Diagonal::shared_ptr Diagonal::Sigmas(const Vector& sigmas, bool smart) { return Diagonal::shared_ptr(new Diagonal(sigmas)); } +/* ************************************************************************* */ void Diagonal::print(const string& name) const { gtsam::print(sigmas_, name + ": diagonal sigmas"); } +/* ************************************************************************* */ Vector Diagonal::whiten(const Vector& v) const { return emul(v, invsigmas_); } +/* ************************************************************************* */ Vector Diagonal::unwhiten(const Vector& v) const { return emul(v, sigmas_); } +/* ************************************************************************* */ Matrix Diagonal::Whiten(const Matrix& H) const { return vector_scale(invsigmas_, H); } +/* ************************************************************************* */ void Diagonal::WhitenInPlace(Matrix& H) const { vector_scale_inplace(invsigmas_, H); } +/* ************************************************************************* */ void Diagonal::WhitenInPlace(MatrixColMajor& H) const { vector_scale_inplace(invsigmas_, H); } +/* ************************************************************************* */ Vector Diagonal::sample() const { Vector result(dim_); for (size_t i = 0; i < dim_; i++) { @@ -348,28 +367,34 @@ Vector Diagonal::sample() const { } /* ************************************************************************* */ - +// Constrained +/* ************************************************************************* */ void Constrained::print(const std::string& name) const { gtsam::print(sigmas_, name + ": constrained sigmas"); } +/* ************************************************************************* */ Vector Constrained::whiten(const Vector& v) const { // ediv_ does the right thing with the errors return ediv_(v, sigmas_); } +/* ************************************************************************* */ Matrix Constrained::Whiten(const Matrix& H) const { throw logic_error("noiseModel::Constrained cannot Whiten"); } +/* ************************************************************************* */ void Constrained::WhitenInPlace(Matrix& H) const { throw logic_error("noiseModel::Constrained cannot Whiten"); } +/* ************************************************************************* */ void Constrained::WhitenInPlace(MatrixColMajor& H) const { throw logic_error("noiseModel::Constrained cannot Whiten"); } +/* ************************************************************************* */ // Special version of QR for Constrained calls slower but smarter code // that deals with possibly zero sigmas // It is Gram-Schmidt orthogonalization rather than Householder @@ -445,6 +470,7 @@ SharedDiagonal Constrained::QR(Matrix& Ab, boost::optional&> fi return mixed ? Constrained::MixedPrecisions(precisions) : Diagonal::Precisions(precisions); } +/* ************************************************************************* */ SharedDiagonal Constrained::QRColumnWise(ublas::matrix& Ab, vector& firstZeroRows) const { Matrix AbRowWise(Ab); SharedDiagonal result = this->QR(AbRowWise, firstZeroRows); @@ -453,41 +479,50 @@ SharedDiagonal Constrained::QRColumnWise(ublas::matrix Normal; @@ -499,6 +534,8 @@ Vector Isotropic::sample() const { return result; } +/* ************************************************************************* */ +// Unit /* ************************************************************************* */ void Unit::print(const std::string& name) const { cout << name << ": unit (" << dim_ << ") " << endl; diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index aebbdddf7..c96491c07 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -44,7 +44,8 @@ namespace gtsam { public: - Base(size_t dim):dim_(dim) {} + /** primary constructor @param dim is the dimension of the model */ + Base(size_t dim = 1):dim_(dim) {} virtual ~Base() {} /** @@ -71,6 +72,14 @@ namespace gtsam { virtual void unwhitenInPlace(Vector& v) const { v = unwhiten(v); } + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(dim_); + } }; /** @@ -103,7 +112,7 @@ namespace gtsam { protected: /** protected constructor takes square root information matrix */ - Gaussian(size_t dim, const boost::optional& sqrt_information = boost::none) : + Gaussian(size_t dim = 1, const boost::optional& sqrt_information = boost::none) : Base(dim), sqrt_information_(sqrt_information) { } @@ -126,7 +135,7 @@ namespace gtsam { static shared_ptr Covariance(const Matrix& covariance, bool smart=false); virtual void print(const std::string& name) const; - virtual bool equals(const Base& expected, double tol) const; + virtual bool equals(const Base& expected, double tol=1e-9) const; virtual Vector whiten(const Vector& v) const; virtual Vector unwhiten(const Vector& v) const; @@ -184,6 +193,15 @@ namespace gtsam { */ virtual bool isConstrained() const {return false;} + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar & BOOST_SERIALIZATION_NVP(sqrt_information_); + } + }; // Gaussian @@ -199,7 +217,7 @@ namespace gtsam { Vector sigmas_, invsigmas_; /** protected constructor takes sigmas */ - Diagonal(const Vector& sigmas); + Diagonal(const Vector& sigmas = ones(1)); public: @@ -256,6 +274,16 @@ namespace gtsam { virtual Matrix R() const { return diag(invsigmas_); } + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Gaussian); + ar & BOOST_SERIALIZATION_NVP(sigmas_); + ar & BOOST_SERIALIZATION_NVP(invsigmas_); + } }; // Diagonal /** @@ -273,7 +301,7 @@ namespace gtsam { // Instead (possibly zero) sigmas are stored in Diagonal Base class /** protected constructor takes sigmas */ - Constrained(const Vector& sigmas) :Diagonal(sigmas) {} + Constrained(const Vector& sigmas = zero(1)) :Diagonal(sigmas) {} public: @@ -337,6 +365,14 @@ namespace gtsam { */ virtual bool isConstrained() const {return true;} + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Diagonal); + } + }; // Constrained /** @@ -393,6 +429,16 @@ namespace gtsam { */ virtual Vector sample() const; + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Diagonal); + ar & BOOST_SERIALIZATION_NVP(sigma_); + ar & BOOST_SERIALIZATION_NVP(invsigma_); + } + }; /** @@ -420,6 +466,14 @@ namespace gtsam { virtual Vector unwhiten(const Vector& v) const { return v; } virtual Matrix Whiten(const Matrix& H) const { return H; } virtual void WhitenInPlace(Matrix& H) const {} + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Isotropic); + } }; } // namespace noiseModel diff --git a/gtsam/linear/SharedDiagonal.h b/gtsam/linear/SharedDiagonal.h index c754913e0..57d5e4c2b 100644 --- a/gtsam/linear/SharedDiagonal.h +++ b/gtsam/linear/SharedDiagonal.h @@ -9,7 +9,7 @@ * -------------------------------------------------------------------------- */ -/* +/** * SharedDiagonal.h * @brief Class that wraps a shared noise model with diagonal covariance * @Author: Frank Dellaert @@ -43,6 +43,14 @@ namespace gtsam { // note, deliberately not in noiseModel namespace SharedDiagonal(const Vector& sigmas) : noiseModel::Diagonal::shared_ptr(noiseModel::Diagonal::Sigmas(sigmas)) { } + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("SharedDiagonal", + boost::serialization::base_object(*this)); + } }; // TODO: make these the ones really used in unit tests diff --git a/gtsam/linear/SharedGaussian.h b/gtsam/linear/SharedGaussian.h index cb7d53a83..d14a43068 100644 --- a/gtsam/linear/SharedGaussian.h +++ b/gtsam/linear/SharedGaussian.h @@ -65,6 +65,14 @@ namespace gtsam { // note, deliberately not in noiseModel namespace GTSAM_DANGEROUS_GAUSSIAN, s)) { } #endif + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("SharedGaussian", + boost::serialization::base_object(*this)); + } }; } diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 499b2dd56..b75652273 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -26,6 +26,7 @@ using namespace boost::assign; #include +#include #include #include #include @@ -71,11 +72,11 @@ TEST(NoiseModel, constructors) // test whiten BOOST_FOREACH(Gaussian::shared_ptr mi, m) - CHECK(assert_equal(whitened,mi->whiten(unwhitened))); + EXPECT(assert_equal(whitened,mi->whiten(unwhitened))); // test unwhiten BOOST_FOREACH(Gaussian::shared_ptr mi, m) - CHECK(assert_equal(unwhitened,mi->unwhiten(whitened))); + EXPECT(assert_equal(unwhitened,mi->unwhiten(whitened))); // test Mahalanobis distance double distance = 5*5+10*10+15*15; @@ -89,7 +90,7 @@ TEST(NoiseModel, constructors) 0.0, 0.0, s_1)); BOOST_FOREACH(Gaussian::shared_ptr mi, m) - CHECK(assert_equal(expectedR,mi->R())); + EXPECT(assert_equal(expectedR,mi->R())); // test Whiten operator Matrix H(Matrix_(3, 4, @@ -103,27 +104,38 @@ TEST(NoiseModel, constructors) s_1, 0.0, 0.0, s_1)); BOOST_FOREACH(Gaussian::shared_ptr mi, m) - CHECK(assert_equal(expected,mi->Whiten(H))); + EXPECT(assert_equal(expected,mi->Whiten(H))); // can only test inplace version once :-) m[0]->WhitenInPlace(H); - CHECK(assert_equal(expected,H)); + EXPECT(assert_equal(expected,H)); } /* ************************************************************************* */ TEST(NoiseModel, Unit) { Vector v = Vector_(3,5.0,10.0,15.0); Gaussian::shared_ptr u(Unit::Create(3)); - CHECK(assert_equal(v,u->whiten(v))); + EXPECT(assert_equal(v,u->whiten(v))); } /* ************************************************************************* */ TEST(NoiseModel, equals) { - Gaussian::shared_ptr g = Gaussian::SqrtInformation(R); - Diagonal::shared_ptr d = Diagonal::Sigmas(Vector_(3, sigma, sigma, sigma)); - Isotropic::shared_ptr i = Isotropic::Sigma(3, sigma); - CHECK(assert_equal(*g,*g)); + Gaussian::shared_ptr g1 = Gaussian::SqrtInformation(R), + g2 = Gaussian::SqrtInformation(eye(3,3)); + Diagonal::shared_ptr d1 = Diagonal::Sigmas(Vector_(3, sigma, sigma, sigma)), + d2 = Diagonal::Sigmas(Vector_(3, 0.1, 0.2, 0.3)); + Isotropic::shared_ptr i1 = Isotropic::Sigma(3, sigma), + i2 = Isotropic::Sigma(3, 0.7); + + EXPECT(assert_equal(*g1,*g1)); + EXPECT(assert_inequal(*g1, *g2)); + + EXPECT(assert_equal(*d1,*d1)); + EXPECT(assert_inequal(*d1,*d2)); + + EXPECT(assert_equal(*i1,*i1)); + EXPECT(assert_inequal(*i1,*i2)); } /* ************************************************************************* */ @@ -132,8 +144,8 @@ TEST(NoiseModel, ConstrainedMixed ) Vector feasible = Vector_(3, 1.0, 0.0, 1.0), infeasible = Vector_(3, 1.0, 1.0, 1.0); Constrained::shared_ptr d = Constrained::MixedSigmas(Vector_(3, sigma, 0.0, sigma)); - CHECK(assert_equal(Vector_(3, 0.5, inf, 0.5),d->whiten(infeasible))); - CHECK(assert_equal(Vector_(3, 0.5, 0.0, 0.5),d->whiten(feasible))); + EXPECT(assert_equal(Vector_(3, 0.5, inf, 0.5),d->whiten(infeasible))); + EXPECT(assert_equal(Vector_(3, 0.5, 0.0, 0.5),d->whiten(feasible))); DOUBLES_EQUAL(inf,d->Mahalanobis(infeasible),1e-9); DOUBLES_EQUAL(0.5,d->Mahalanobis(feasible),1e-9); } @@ -145,8 +157,8 @@ TEST(NoiseModel, ConstrainedAll ) infeasible = Vector_(3, 1.0, 1.0, 1.0); Constrained::shared_ptr i = Constrained::All(3); - CHECK(assert_equal(Vector_(3, inf, inf, inf),i->whiten(infeasible))); - CHECK(assert_equal(Vector_(3, 0.0, 0.0, 0.0),i->whiten(feasible))); + EXPECT(assert_equal(Vector_(3, inf, inf, inf),i->whiten(infeasible))); + EXPECT(assert_equal(Vector_(3, 0.0, 0.0, 0.0),i->whiten(feasible))); DOUBLES_EQUAL(inf,i->Mahalanobis(infeasible),1e-9); DOUBLES_EQUAL(0.0,i->Mahalanobis(feasible),1e-9); } @@ -183,20 +195,20 @@ TEST( NoiseModel, QR ) // Call Gaussian version SharedDiagonal actual1 = exampleQR::diagonal->QR(Ab1); SharedDiagonal expected = noiseModel::Unit::Create(4); - CHECK(assert_equal(*expected,*actual1)); - CHECK(linear_dependent(exampleQR::Rd,Ab1,1e-4)); // Ab was modified in place !!! + EXPECT(assert_equal(*expected,*actual1)); + EXPECT(linear_dependent(exampleQR::Rd,Ab1,1e-4)); // Ab was modified in place !!! // Call Constrained version SharedDiagonal constrained = noiseModel::Constrained::MixedSigmas(exampleQR::sigmas); SharedDiagonal actual2 = constrained->QR(Ab2); SharedDiagonal expectedModel2 = noiseModel::Diagonal::Sigmas(expectedSigmas); - CHECK(assert_equal(*expectedModel2,*actual2)); + EXPECT(assert_equal(*expectedModel2,*actual2,1e-6)); Matrix expectedRd2 = Matrix_(4, 6+1, 1., 0., -0.2, 0., -0.8, 0., 0.2, 0., 1., 0.,-0.2, 0., -0.8,-0.14, 0., 0., 1., 0., -1., 0., 0.0, 0., 0., 0., 1., 0., -1., 0.2); - CHECK(linear_dependent(expectedRd2,Ab2,1e-6)); // Ab was modified in place !!! + EXPECT(linear_dependent(expectedRd2,Ab2,1e-6)); // Ab was modified in place !!! } /* ************************************************************************* */ @@ -208,12 +220,12 @@ TEST( NoiseModel, QR ) // firstZeroRows += 0,1,2,3,4,5; // FD: no idea as not documented :-( // SharedDiagonal actual = exampleQR::diagonal->QRColumnWise(Ab,firstZeroRows); // SharedDiagonal expected = noiseModel::Unit::Create(4); -// CHECK(assert_equal(*expected,*actual)); +// EXPECT(assert_equal(*expected,*actual)); // Matrix AbResized = ublas::triangular_adaptor(Ab); // print(exampleQR::Rd, "Rd: "); // print(Ab, "Ab: "); // print(AbResized, "AbResized: "); -// CHECK(linear_dependent(exampleQR::Rd,AbResized,1e-4)); // Ab was modified in place !!! +// EXPECT(linear_dependent(exampleQR::Rd,AbResized,1e-4)); // Ab was modified in place !!! //} /* ************************************************************************* */ @@ -240,8 +252,8 @@ TEST(NoiseModel, QRNan ) Matrix expectedAb = Matrix_(2, 5, 1., 2., 1., 2., 3., 0., 1., 0., 0., 2.0/3); SharedDiagonal actual = constrained->QR(Ab); - CHECK(assert_equal(*expected,*actual)); - CHECK(assert_equal(expectedAb,Ab)); + EXPECT(assert_equal(*expected,*actual)); + EXPECT(assert_equal(expectedAb,Ab)); } /* ************************************************************************* */ @@ -250,7 +262,7 @@ TEST(NoiseModel, SmartCovariance ) bool smart = true; SharedGaussian expected = Unit::Create(3); SharedGaussian actual = Gaussian::Covariance(eye(3), smart); - CHECK(assert_equal(*expected,*actual)); + EXPECT(assert_equal(*expected,*actual)); } /* ************************************************************************* */ @@ -259,7 +271,7 @@ TEST(NoiseModel, ScalarOrVector ) bool smart = true; SharedGaussian expected = Unit::Create(3); SharedGaussian actual = Gaussian::Covariance(eye(3), smart); - CHECK(assert_equal(*expected,*actual)); + EXPECT(assert_equal(*expected,*actual)); } /* ************************************************************************* */ @@ -270,7 +282,7 @@ TEST(NoiseModel, WhitenInPlace) Matrix A = eye(3); model->WhitenInPlace(A); Matrix expected = eye(3) * 10; - CHECK(assert_equal(expected, A)); + EXPECT(assert_equal(expected, A)); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 93d57e3a3..f5bb7e1c8 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -148,7 +148,8 @@ namespace gtsam { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - // TODO NoiseModel + ar & BOOST_SERIALIZATION_NVP(noiseModel_); + ar & BOOST_SERIALIZATION_NVP(keys_); } }; // NonlinearFactor @@ -262,7 +263,7 @@ namespace gtsam { template void serialize(ARCHIVE & ar, const unsigned int version) { ar & boost::serialization::make_nvp("NonlinearFactor", - boost::serialization::base_object(*this)); + boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(key_); } @@ -395,7 +396,7 @@ namespace gtsam { template void serialize(ARCHIVE & ar, const unsigned int version) { ar & boost::serialization::make_nvp("NonlinearFactor", - boost::serialization::base_object(*this)); + boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(key1_); ar & BOOST_SERIALIZATION_NVP(key2_); } @@ -571,7 +572,7 @@ namespace gtsam { template void serialize(ARCHIVE & ar, const unsigned int version) { ar & boost::serialization::make_nvp("NonlinearFactor", - boost::serialization::base_object(*this)); + boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(key1_); ar & BOOST_SERIALIZATION_NVP(key2_); ar & BOOST_SERIALIZATION_NVP(key3_); diff --git a/gtsam/slam/BearingFactor.h b/gtsam/slam/BearingFactor.h index cc3c2428d..84d730cea 100644 --- a/gtsam/slam/BearingFactor.h +++ b/gtsam/slam/BearingFactor.h @@ -53,6 +53,7 @@ namespace gtsam { inline const Rot2 measured() const { return z_; } + }; // BearingFactor } // namespace gtsam diff --git a/tests/testSerialization.cpp b/tests/testSerialization.cpp index 13e4d3cf6..348200052 100644 --- a/tests/testSerialization.cpp +++ b/tests/testSerialization.cpp @@ -24,6 +24,8 @@ #include // includes for standard serialization types +#include +#include #include #include #include @@ -120,6 +122,14 @@ bool equalsXML(const T& input = T()) { return input.equals(output); } +// This version is for pointers +template +bool equalsDereferencedXML(const T& input = T()) { + T output; + roundtripXML(input,output); + return input->equals(*output); +} + /* ************************************************************************* */ // Actual Tests /* ************************************************************************* */ @@ -148,9 +158,12 @@ TEST (Serialization, text_geometry) { EXPECT(equalsObj(Point2(1.0, 2.0))); EXPECT(equalsObj(Pose2(1.0, 2.0, 0.3))); EXPECT(equalsObj(Rot2::fromDegrees(30.0))); - EXPECT(equalsObj(Point3(1.0, 2.0, 3.0))); - EXPECT(equalsObj()); - EXPECT(equalsObj(Rot3::RzRyRx(1.0, 3.0, 2.0))); + + Point3 pt3(1.0, 2.0, 3.0); + Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0); + EXPECT(equalsObj(pt3)); + EXPECT(equalsObj(rt3)); + EXPECT(equalsObj(Pose3(rt3, pt3))); } /* ************************************************************************* */ @@ -158,9 +171,13 @@ TEST (Serialization, xml_geometry) { EXPECT(equalsXML(Point2(1.0, 2.0))); EXPECT(equalsXML(Pose2(1.0, 2.0, 0.3))); EXPECT(equalsXML(Rot2::fromDegrees(30.0))); - EXPECT(equalsXML(Point3(1.0, 2.0, 3.0))); - EXPECT(equalsXML()); - EXPECT(equalsXML(Rot3::RzRyRx(1.0, 3.0, 2.0))); + + Point3 pt3(1.0, 2.0, 3.0); + Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0); + EXPECT(equalsXML(pt3)); + EXPECT(equalsXML(rt3)); + EXPECT(equalsXML(Pose3(rt3, pt3))); + } /* ************************************************************************* */ @@ -178,17 +195,46 @@ TEST (Serialization, xml_linear) { } /* ************************************************************************* */ -TEST (Serialization, text_planar) { - EXPECT(equalsObj(gtsam::planarSLAM::PoseKey(2))); - EXPECT(equalsObj(gtsam::planarSLAM::PointKey(2))); -// EXPECT(equalsObj()); +TEST (Serialization, noiseModels) { + SharedDiagonal diag3 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.2, 0.3)); + SharedGaussian model3 = noiseModel::Isotropic::Sigma(3, 0.3); + + EXPECT(equalsDereferenced(diag3)); + EXPECT(equalsDereferencedXML(diag3)); + + // FAIL: Segfaults +// EXPECT(equalsDereferenced(model3)); +// EXPECT(equalsDereferencedXML(model3)); } /* ************************************************************************* */ -TEST (Serialization, xml_planar) { - EXPECT(equalsXML(gtsam::planarSLAM::PoseKey(2))); - EXPECT(equalsXML(gtsam::planarSLAM::PointKey(2))); -// EXPECT(equalsXML()); +TEST (Serialization, planar_system) { + using namespace planarSLAM; + + Values values; + values.insert(PointKey(3), Point2(1.0, 2.0)); + values.insert(PoseKey(4), Pose2(1.0, 2.0, 0.3)); + + 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); + + // text + EXPECT(equalsObj(PoseKey(2))); + EXPECT(equalsObj(PointKey(3))); + EXPECT(equalsObj(values)); +// EXPECT(equalsObj(graph)); + + // xml + EXPECT(equalsXML(PoseKey(2))); + EXPECT(equalsXML(PointKey(3))); + EXPECT(equalsXML(values)); +// EXPECT(equalsXML(graph)); } /* ************************************************************************* */