12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Added more serialization functionality to noiseModel, but SharedGaussians segfault on test, so tests for Nonlinear graphs are commented out

release/4.3a0
Alex Cunningham 2011-02-23 05:19:07 +00:00
parent f7e30a5d52
commit 4865f1a64d
8 changed files with 219 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
gtsam/linear/NoiseModel.cpp
View File

@ -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))); full: return shared_ptr(new Gaussian(n, inverse_square_root(covariance)));
} }
/* ************************************************************************* */
void Gaussian::print(const string& name) const { void Gaussian::print(const string& name) const {
gtsam::print(thisR(), "Gaussian"); gtsam::print(thisR(), "Gaussian");
} }
/* ************************************************************************* */
bool Gaussian::equals(const Base& expected, double tol) const { bool Gaussian::equals(const Base& expected, double tol) const {
const Gaussian* p = dynamic_cast<const Gaussian*> (&expected); const Gaussian* p = dynamic_cast<const Gaussian*> (&expected);
if (p == NULL) return false; 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)); return equal_with_abs_tol(R(), p->R(), sqrt(tol));
} }
/* ************************************************************************* */
Vector Gaussian::whiten(const Vector& v) const { Vector Gaussian::whiten(const Vector& v) const {
return thisR() * v; return thisR() * v;
} }
/* ************************************************************************* */
Vector Gaussian::unwhiten(const Vector& v) const { Vector Gaussian::unwhiten(const Vector& v) const {
return backSubstituteUpper(thisR(), v); return backSubstituteUpper(thisR(), v);
} }
/* ************************************************************************* */
double Gaussian::Mahalanobis(const Vector& v) const { double Gaussian::Mahalanobis(const Vector& v) const {
// Note: for Diagonal, which does ediv_, will be correct for constraints // Note: for Diagonal, which does ediv_, will be correct for constraints
Vector w = whiten(v); Vector w = whiten(v);
return inner_prod(w, w); return inner_prod(w, w);
} }
/* ************************************************************************* */
Matrix Gaussian::Whiten(const Matrix& H) const { Matrix Gaussian::Whiten(const Matrix& H) const {
return thisR() * H; return thisR() * H;
} }
/* ************************************************************************* */
void Gaussian::WhitenInPlace(Matrix& H) const { void Gaussian::WhitenInPlace(Matrix& H) const {
H = thisR() * H; H = thisR() * H;
} }
/* ************************************************************************* */
void Gaussian::WhitenInPlace(MatrixColMajor& H) const { void Gaussian::WhitenInPlace(MatrixColMajor& H) const {
H = ublas::prod(thisR(), H); H = ublas::prod(thisR(), H);
} }
/* ************************************************************************* */
// General QR, see also special version in Constrained // General QR, see also special version in Constrained
SharedDiagonal Gaussian::QR(Matrix& Ab, boost::optional<vector<int>&> firstZeroRows) const { SharedDiagonal Gaussian::QR(Matrix& Ab, boost::optional<vector<int>&> firstZeroRows) const {
@ -285,11 +293,14 @@ SharedDiagonal Gaussian::Cholesky(MatrixColMajor& Ab, size_t nFrontals) const {
return Unit::Create(maxrank); return Unit::Create(maxrank);
} }
/* ************************************************************************* */
// Diagonal
/* ************************************************************************* */ /* ************************************************************************* */
Diagonal::Diagonal(const Vector& sigmas) : Diagonal::Diagonal(const Vector& sigmas) :
Gaussian(sigmas.size()), sigmas_(sigmas), invsigmas_(reciprocal(sigmas)) { Gaussian(sigmas.size()), sigmas_(sigmas), invsigmas_(reciprocal(sigmas)) {
} }
/* ************************************************************************* */
Diagonal::shared_ptr Diagonal::Variances(const Vector& variances, bool smart) { Diagonal::shared_ptr Diagonal::Variances(const Vector& variances, bool smart) {
if (smart) { if (smart) {
// check whether all the same entry // 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))); full: return shared_ptr(new Diagonal(esqrt(variances)));
} }
/* ************************************************************************* */
Diagonal::shared_ptr Diagonal::Sigmas(const Vector& sigmas, bool smart) { Diagonal::shared_ptr Diagonal::Sigmas(const Vector& sigmas, bool smart) {
if (smart) { if (smart) {
// look for zeros to make a constraint // 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)); return Diagonal::shared_ptr(new Diagonal(sigmas));
} }
/* ************************************************************************* */
void Diagonal::print(const string& name) const { void Diagonal::print(const string& name) const {
gtsam::print(sigmas_, name + ": diagonal sigmas"); gtsam::print(sigmas_, name + ": diagonal sigmas");
} }
/* ************************************************************************* */
Vector Diagonal::whiten(const Vector& v) const { Vector Diagonal::whiten(const Vector& v) const {
return emul(v, invsigmas_); return emul(v, invsigmas_);
} }
/* ************************************************************************* */
Vector Diagonal::unwhiten(const Vector& v) const { Vector Diagonal::unwhiten(const Vector& v) const {
return emul(v, sigmas_); return emul(v, sigmas_);
} }
/* ************************************************************************* */
Matrix Diagonal::Whiten(const Matrix& H) const { Matrix Diagonal::Whiten(const Matrix& H) const {
return vector_scale(invsigmas_, H); return vector_scale(invsigmas_, H);
} }
/* ************************************************************************* */
void Diagonal::WhitenInPlace(Matrix& H) const { void Diagonal::WhitenInPlace(Matrix& H) const {
vector_scale_inplace(invsigmas_, H); vector_scale_inplace(invsigmas_, H);
} }
/* ************************************************************************* */
void Diagonal::WhitenInPlace(MatrixColMajor& H) const { void Diagonal::WhitenInPlace(MatrixColMajor& H) const {
vector_scale_inplace(invsigmas_, H); vector_scale_inplace(invsigmas_, H);
} }
/* ************************************************************************* */
Vector Diagonal::sample() const { Vector Diagonal::sample() const {
Vector result(dim_); Vector result(dim_);
for (size_t i = 0; i < dim_; i++) { 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 { void Constrained::print(const std::string& name) const {
gtsam::print(sigmas_, name + ": constrained sigmas"); gtsam::print(sigmas_, name + ": constrained sigmas");
} }
/* ************************************************************************* */
Vector Constrained::whiten(const Vector& v) const { Vector Constrained::whiten(const Vector& v) const {
// ediv_ does the right thing with the errors // ediv_ does the right thing with the errors
return ediv_(v, sigmas_); return ediv_(v, sigmas_);
} }
/* ************************************************************************* */
Matrix Constrained::Whiten(const Matrix& H) const { Matrix Constrained::Whiten(const Matrix& H) const {
throw logic_error("noiseModel::Constrained cannot Whiten"); throw logic_error("noiseModel::Constrained cannot Whiten");
} }
/* ************************************************************************* */
void Constrained::WhitenInPlace(Matrix& H) const { void Constrained::WhitenInPlace(Matrix& H) const {
throw logic_error("noiseModel::Constrained cannot Whiten"); throw logic_error("noiseModel::Constrained cannot Whiten");
} }
/* ************************************************************************* */
void Constrained::WhitenInPlace(MatrixColMajor& H) const { void Constrained::WhitenInPlace(MatrixColMajor& H) const {
throw logic_error("noiseModel::Constrained cannot Whiten"); throw logic_error("noiseModel::Constrained cannot Whiten");
} }
/* ************************************************************************* */
// Special version of QR for Constrained calls slower but smarter code // Special version of QR for Constrained calls slower but smarter code
// that deals with possibly zero sigmas // that deals with possibly zero sigmas
// It is Gram-Schmidt orthogonalization rather than Householder // It is Gram-Schmidt orthogonalization rather than Householder
@ -445,6 +470,7 @@ SharedDiagonal Constrained::QR(Matrix& Ab, boost::optional<std::vector<int>&> fi
return mixed ? Constrained::MixedPrecisions(precisions) : Diagonal::Precisions(precisions); return mixed ? Constrained::MixedPrecisions(precisions) : Diagonal::Precisions(precisions);
} }
/* ************************************************************************* */
SharedDiagonal Constrained::QRColumnWise(ublas::matrix<double, ublas::column_major>& Ab, vector<int>& firstZeroRows) const { SharedDiagonal Constrained::QRColumnWise(ublas::matrix<double, ublas::column_major>& Ab, vector<int>& firstZeroRows) const {
Matrix AbRowWise(Ab); Matrix AbRowWise(Ab);
SharedDiagonal result = this->QR(AbRowWise, firstZeroRows); SharedDiagonal result = this->QR(AbRowWise, firstZeroRows);
@ -453,41 +479,50 @@ SharedDiagonal Constrained::QRColumnWise(ublas::matrix<double, ublas::column_maj
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Isotropic
/* ************************************************************************* */
Isotropic::shared_ptr Isotropic::Variance(size_t dim, double variance, bool smart) { Isotropic::shared_ptr Isotropic::Variance(size_t dim, double variance, bool smart) {
if (smart && fabs(variance-1.0)<1e-9) return Unit::Create(dim); if (smart && fabs(variance-1.0)<1e-9) return Unit::Create(dim);
return shared_ptr(new Isotropic(dim, sqrt(variance))); return shared_ptr(new Isotropic(dim, sqrt(variance)));
} }
/* ************************************************************************* */
void Isotropic::print(const string& name) const { void Isotropic::print(const string& name) const {
cout << name << ": isotropic sigma " << " " << sigma_ << endl; cout << name << ": isotropic sigma " << " " << sigma_ << endl;
} }
/* ************************************************************************* */
double Isotropic::Mahalanobis(const Vector& v) const { double Isotropic::Mahalanobis(const Vector& v) const {
double dot = inner_prod(v, v); double dot = inner_prod(v, v);
return dot * invsigma_ * invsigma_; return dot * invsigma_ * invsigma_;
} }
/* ************************************************************************* */
Vector Isotropic::whiten(const Vector& v) const { Vector Isotropic::whiten(const Vector& v) const {
return v * invsigma_; return v * invsigma_;
} }
/* ************************************************************************* */
Vector Isotropic::unwhiten(const Vector& v) const { Vector Isotropic::unwhiten(const Vector& v) const {
return v * sigma_; return v * sigma_;
} }
/* ************************************************************************* */
Matrix Isotropic::Whiten(const Matrix& H) const { Matrix Isotropic::Whiten(const Matrix& H) const {
return invsigma_ * H; return invsigma_ * H;
} }
/* ************************************************************************* */
void Isotropic::WhitenInPlace(Matrix& H) const { void Isotropic::WhitenInPlace(Matrix& H) const {
H *= invsigma_; H *= invsigma_;
} }
/* ************************************************************************* */
void Isotropic::WhitenInPlace(MatrixColMajor& H) const { void Isotropic::WhitenInPlace(MatrixColMajor& H) const {
H *= invsigma_; H *= invsigma_;
} }
/* ************************************************************************* */
// faster version // faster version
Vector Isotropic::sample() const { Vector Isotropic::sample() const {
typedef boost::normal_distribution<double> Normal; typedef boost::normal_distribution<double> Normal;
@ -499,6 +534,8 @@ Vector Isotropic::sample() const {
return result; return result;
} }
/* ************************************************************************* */
// Unit
/* ************************************************************************* */ /* ************************************************************************* */
void Unit::print(const std::string& name) const { void Unit::print(const std::string& name) const {
cout << name << ": unit (" << dim_ << ") " << endl; cout << name << ": unit (" << dim_ << ") " << endl;

64
gtsam/linear/NoiseModel.h
View File

@ -44,7 +44,8 @@ namespace gtsam {
public: 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() {} virtual ~Base() {}
/** /**
@ -71,6 +72,14 @@ namespace gtsam {
virtual void unwhitenInPlace(Vector& v) const { virtual void unwhitenInPlace(Vector& v) const {
v = unwhiten(v); v = unwhiten(v);
} }
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(dim_);
}
}; };
/** /**
@ -103,7 +112,7 @@ namespace gtsam {
protected: protected:
/** protected constructor takes square root information matrix */ /** protected constructor takes square root information matrix */
Gaussian(size_t dim, const boost::optional<Matrix>& sqrt_information = boost::none) : Gaussian(size_t dim = 1, const boost::optional<Matrix>& sqrt_information = boost::none) :
Base(dim), sqrt_information_(sqrt_information) { Base(dim), sqrt_information_(sqrt_information) {
} }
@ -126,7 +135,7 @@ namespace gtsam {
static shared_ptr Covariance(const Matrix& covariance, bool smart=false); static shared_ptr Covariance(const Matrix& covariance, bool smart=false);
virtual void print(const std::string& name) const; 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 whiten(const Vector& v) const;
virtual Vector unwhiten(const Vector& v) const; virtual Vector unwhiten(const Vector& v) const;
@ -184,6 +193,15 @@ namespace gtsam {
*/ */
virtual bool isConstrained() const {return false;} virtual bool isConstrained() const {return false;}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(sqrt_information_);
}
}; // Gaussian }; // Gaussian
@ -199,7 +217,7 @@ namespace gtsam {
Vector sigmas_, invsigmas_; Vector sigmas_, invsigmas_;
/** protected constructor takes sigmas */ /** protected constructor takes sigmas */
Diagonal(const Vector& sigmas); Diagonal(const Vector& sigmas = ones(1));
public: public:
@ -256,6 +274,16 @@ namespace gtsam {
virtual Matrix R() const { virtual Matrix R() const {
return diag(invsigmas_); return diag(invsigmas_);
} }
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
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 }; // Diagonal
/** /**
@ -273,7 +301,7 @@ namespace gtsam {
// Instead (possibly zero) sigmas are stored in Diagonal Base class // Instead (possibly zero) sigmas are stored in Diagonal Base class
/** protected constructor takes sigmas */ /** protected constructor takes sigmas */
Constrained(const Vector& sigmas) :Diagonal(sigmas) {} Constrained(const Vector& sigmas = zero(1)) :Diagonal(sigmas) {}
public: public:
@ -337,6 +365,14 @@ namespace gtsam {
*/ */
virtual bool isConstrained() const {return true;} virtual bool isConstrained() const {return true;}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Diagonal);
}
}; // Constrained }; // Constrained
/** /**
@ -393,6 +429,16 @@ namespace gtsam {
*/ */
virtual Vector sample() const; virtual Vector sample() const;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
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 Vector unwhiten(const Vector& v) const { return v; }
virtual Matrix Whiten(const Matrix& H) const { return H; } virtual Matrix Whiten(const Matrix& H) const { return H; }
virtual void WhitenInPlace(Matrix& H) const {} virtual void WhitenInPlace(Matrix& H) const {}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Isotropic);
}
}; };
} // namespace noiseModel } // namespace noiseModel

10
gtsam/linear/SharedDiagonal.h
View File

@ -9,7 +9,7 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/* /**
* SharedDiagonal.h * SharedDiagonal.h
* @brief Class that wraps a shared noise model with diagonal covariance * @brief Class that wraps a shared noise model with diagonal covariance
* @Author: Frank Dellaert * @Author: Frank Dellaert
@ -43,6 +43,14 @@ namespace gtsam { // note, deliberately not in noiseModel namespace
SharedDiagonal(const Vector& sigmas) : SharedDiagonal(const Vector& sigmas) :
noiseModel::Diagonal::shared_ptr(noiseModel::Diagonal::Sigmas(sigmas)) { noiseModel::Diagonal::shared_ptr(noiseModel::Diagonal::Sigmas(sigmas)) {
} }
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("SharedDiagonal",
boost::serialization::base_object<noiseModel::Diagonal::shared_ptr >(*this));
}
}; };
// TODO: make these the ones really used in unit tests // TODO: make these the ones really used in unit tests

8
gtsam/linear/SharedGaussian.h
View File

@ -65,6 +65,14 @@ namespace gtsam { // note, deliberately not in noiseModel namespace
GTSAM_DANGEROUS_GAUSSIAN, s)) { GTSAM_DANGEROUS_GAUSSIAN, s)) {
} }
#endif #endif
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("SharedGaussian",
boost::serialization::base_object<noiseModel::Gaussian::shared_ptr >(*this));
}
}; };
} }

62
gtsam/linear/tests/testNoiseModel.cpp
View File

@ -26,6 +26,7 @@
using namespace boost::assign; using namespace boost::assign;
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/linear/NoiseModel.h> #include <gtsam/linear/NoiseModel.h>
#include <gtsam/linear/SharedGaussian.h> #include <gtsam/linear/SharedGaussian.h>
#include <gtsam/linear/SharedDiagonal.h> #include <gtsam/linear/SharedDiagonal.h>
@ -71,11 +72,11 @@ TEST(NoiseModel, constructors)
// test whiten // test whiten
BOOST_FOREACH(Gaussian::shared_ptr mi, m) BOOST_FOREACH(Gaussian::shared_ptr mi, m)
CHECK(assert_equal(whitened,mi->whiten(unwhitened))); EXPECT(assert_equal(whitened,mi->whiten(unwhitened)));
// test unwhiten // test unwhiten
BOOST_FOREACH(Gaussian::shared_ptr mi, m) BOOST_FOREACH(Gaussian::shared_ptr mi, m)
CHECK(assert_equal(unwhitened,mi->unwhiten(whitened))); EXPECT(assert_equal(unwhitened,mi->unwhiten(whitened)));
// test Mahalanobis distance // test Mahalanobis distance
double distance = 5*5+10*10+15*15; double distance = 5*5+10*10+15*15;
@ -89,7 +90,7 @@ TEST(NoiseModel, constructors)
0.0, 0.0, s_1)); 0.0, 0.0, s_1));
BOOST_FOREACH(Gaussian::shared_ptr mi, m) BOOST_FOREACH(Gaussian::shared_ptr mi, m)
CHECK(assert_equal(expectedR,mi->R())); EXPECT(assert_equal(expectedR,mi->R()));
// test Whiten operator // test Whiten operator
Matrix H(Matrix_(3, 4, Matrix H(Matrix_(3, 4,
@ -103,27 +104,38 @@ TEST(NoiseModel, constructors)
s_1, 0.0, 0.0, s_1)); s_1, 0.0, 0.0, s_1));
BOOST_FOREACH(Gaussian::shared_ptr mi, m) 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 :-) // can only test inplace version once :-)
m[0]->WhitenInPlace(H); m[0]->WhitenInPlace(H);
CHECK(assert_equal(expected,H)); EXPECT(assert_equal(expected,H));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(NoiseModel, Unit) { TEST(NoiseModel, Unit) {
Vector v = Vector_(3,5.0,10.0,15.0); Vector v = Vector_(3,5.0,10.0,15.0);
Gaussian::shared_ptr u(Unit::Create(3)); Gaussian::shared_ptr u(Unit::Create(3));
CHECK(assert_equal(v,u->whiten(v))); EXPECT(assert_equal(v,u->whiten(v)));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(NoiseModel, equals) TEST(NoiseModel, equals)
{ {
Gaussian::shared_ptr g = Gaussian::SqrtInformation(R); Gaussian::shared_ptr g1 = Gaussian::SqrtInformation(R),
Diagonal::shared_ptr d = Diagonal::Sigmas(Vector_(3, sigma, sigma, sigma)); g2 = Gaussian::SqrtInformation(eye(3,3));
Isotropic::shared_ptr i = Isotropic::Sigma(3, sigma); Diagonal::shared_ptr d1 = Diagonal::Sigmas(Vector_(3, sigma, sigma, sigma)),
CHECK(assert_equal(*g,*g)); 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), Vector feasible = Vector_(3, 1.0, 0.0, 1.0),
infeasible = Vector_(3, 1.0, 1.0, 1.0); infeasible = Vector_(3, 1.0, 1.0, 1.0);
Constrained::shared_ptr d = Constrained::MixedSigmas(Vector_(3, sigma, 0.0, sigma)); 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))); EXPECT(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, 0.0, 0.5),d->whiten(feasible)));
DOUBLES_EQUAL(inf,d->Mahalanobis(infeasible),1e-9); DOUBLES_EQUAL(inf,d->Mahalanobis(infeasible),1e-9);
DOUBLES_EQUAL(0.5,d->Mahalanobis(feasible),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); infeasible = Vector_(3, 1.0, 1.0, 1.0);
Constrained::shared_ptr i = Constrained::All(3); Constrained::shared_ptr i = Constrained::All(3);
CHECK(assert_equal(Vector_(3, inf, inf, inf),i->whiten(infeasible))); EXPECT(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, 0.0, 0.0, 0.0),i->whiten(feasible)));
DOUBLES_EQUAL(inf,i->Mahalanobis(infeasible),1e-9); DOUBLES_EQUAL(inf,i->Mahalanobis(infeasible),1e-9);
DOUBLES_EQUAL(0.0,i->Mahalanobis(feasible),1e-9); DOUBLES_EQUAL(0.0,i->Mahalanobis(feasible),1e-9);
} }
@ -183,20 +195,20 @@ TEST( NoiseModel, QR )
// Call Gaussian version // Call Gaussian version
SharedDiagonal actual1 = exampleQR::diagonal->QR(Ab1); SharedDiagonal actual1 = exampleQR::diagonal->QR(Ab1);
SharedDiagonal expected = noiseModel::Unit::Create(4); SharedDiagonal expected = noiseModel::Unit::Create(4);
CHECK(assert_equal(*expected,*actual1)); EXPECT(assert_equal(*expected,*actual1));
CHECK(linear_dependent(exampleQR::Rd,Ab1,1e-4)); // Ab was modified in place !!! EXPECT(linear_dependent(exampleQR::Rd,Ab1,1e-4)); // Ab was modified in place !!!
// Call Constrained version // Call Constrained version
SharedDiagonal constrained = noiseModel::Constrained::MixedSigmas(exampleQR::sigmas); SharedDiagonal constrained = noiseModel::Constrained::MixedSigmas(exampleQR::sigmas);
SharedDiagonal actual2 = constrained->QR(Ab2); SharedDiagonal actual2 = constrained->QR(Ab2);
SharedDiagonal expectedModel2 = noiseModel::Diagonal::Sigmas(expectedSigmas); SharedDiagonal expectedModel2 = noiseModel::Diagonal::Sigmas(expectedSigmas);
CHECK(assert_equal(*expectedModel2,*actual2)); EXPECT(assert_equal(*expectedModel2,*actual2,1e-6));
Matrix expectedRd2 = Matrix_(4, 6+1, Matrix expectedRd2 = Matrix_(4, 6+1,
1., 0., -0.2, 0., -0.8, 0., 0.2, 1., 0., -0.2, 0., -0.8, 0., 0.2,
0., 1., 0.,-0.2, 0., -0.8,-0.14, 0., 1., 0.,-0.2, 0., -0.8,-0.14,
0., 0., 1., 0., -1., 0., 0.0, 0., 0., 1., 0., -1., 0., 0.0,
0., 0., 0., 1., 0., -1., 0.2); 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 :-( // firstZeroRows += 0,1,2,3,4,5; // FD: no idea as not documented :-(
// SharedDiagonal actual = exampleQR::diagonal->QRColumnWise(Ab,firstZeroRows); // SharedDiagonal actual = exampleQR::diagonal->QRColumnWise(Ab,firstZeroRows);
// SharedDiagonal expected = noiseModel::Unit::Create(4); // SharedDiagonal expected = noiseModel::Unit::Create(4);
// CHECK(assert_equal(*expected,*actual)); // EXPECT(assert_equal(*expected,*actual));
// Matrix AbResized = ublas::triangular_adaptor<MatrixColMajor, ublas::upper>(Ab); // Matrix AbResized = ublas::triangular_adaptor<MatrixColMajor, ublas::upper>(Ab);
// print(exampleQR::Rd, "Rd: "); // print(exampleQR::Rd, "Rd: ");
// print(Ab, "Ab: "); // print(Ab, "Ab: ");
// print(AbResized, "AbResized: "); // 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); Matrix expectedAb = Matrix_(2, 5, 1., 2., 1., 2., 3., 0., 1., 0., 0., 2.0/3);
SharedDiagonal actual = constrained->QR(Ab); SharedDiagonal actual = constrained->QR(Ab);
CHECK(assert_equal(*expected,*actual)); EXPECT(assert_equal(*expected,*actual));
CHECK(assert_equal(expectedAb,Ab)); EXPECT(assert_equal(expectedAb,Ab));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -250,7 +262,7 @@ TEST(NoiseModel, SmartCovariance )
bool smart = true; bool smart = true;
SharedGaussian expected = Unit::Create(3); SharedGaussian expected = Unit::Create(3);
SharedGaussian actual = Gaussian::Covariance(eye(3), smart); 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; bool smart = true;
SharedGaussian expected = Unit::Create(3); SharedGaussian expected = Unit::Create(3);
SharedGaussian actual = Gaussian::Covariance(eye(3), smart); 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); Matrix A = eye(3);
model->WhitenInPlace(A); model->WhitenInPlace(A);
Matrix expected = eye(3) * 10; Matrix expected = eye(3) * 10;
CHECK(assert_equal(expected, A)); EXPECT(assert_equal(expected, A));
} }
/* ************************************************************************* */ /* ************************************************************************* */

9
gtsam/nonlinear/NonlinearFactor.h
View File

@ -148,7 +148,8 @@ namespace gtsam {
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) { void serialize(ARCHIVE & ar, const unsigned int version) {
// TODO NoiseModel ar & BOOST_SERIALIZATION_NVP(noiseModel_);
ar & BOOST_SERIALIZATION_NVP(keys_);
} }
}; // NonlinearFactor }; // NonlinearFactor
@ -262,7 +263,7 @@ namespace gtsam {
template<class ARCHIVE> template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) { void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NonlinearFactor", ar & boost::serialization::make_nvp("NonlinearFactor",
boost::serialization::base_object<NonlinearFactor>(*this)); boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(key_); ar & BOOST_SERIALIZATION_NVP(key_);
} }
@ -395,7 +396,7 @@ namespace gtsam {
template<class ARCHIVE> template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) { void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NonlinearFactor", ar & boost::serialization::make_nvp("NonlinearFactor",
boost::serialization::base_object<NonlinearFactor>(*this)); boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(key1_); ar & BOOST_SERIALIZATION_NVP(key1_);
ar & BOOST_SERIALIZATION_NVP(key2_); ar & BOOST_SERIALIZATION_NVP(key2_);
} }
@ -571,7 +572,7 @@ namespace gtsam {
template<class ARCHIVE> template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) { void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NonlinearFactor", ar & boost::serialization::make_nvp("NonlinearFactor",
boost::serialization::base_object<NonlinearFactor>(*this)); boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(key1_); ar & BOOST_SERIALIZATION_NVP(key1_);
ar & BOOST_SERIALIZATION_NVP(key2_); ar & BOOST_SERIALIZATION_NVP(key2_);
ar & BOOST_SERIALIZATION_NVP(key3_); ar & BOOST_SERIALIZATION_NVP(key3_);

1
gtsam/slam/BearingFactor.h
View File

@ -53,6 +53,7 @@ namespace gtsam {
inline const Rot2 measured() const { inline const Rot2 measured() const {
return z_; return z_;
} }
}; // BearingFactor }; // BearingFactor
} // namespace gtsam } // namespace gtsam

74
tests/testSerialization.cpp
View File

@ -24,6 +24,8 @@
#include <string> #include <string>
// includes for standard serialization types // includes for standard serialization types
#include <boost/serialization/optional.hpp>
#include <boost/serialization/shared_ptr.hpp>
#include <boost/serialization/vector.hpp> #include <boost/serialization/vector.hpp>
#include <boost/serialization/map.hpp> #include <boost/serialization/map.hpp>
#include <boost/serialization/list.hpp> #include <boost/serialization/list.hpp>
@ -120,6 +122,14 @@ bool equalsXML(const T& input = T()) {
return input.equals(output); return input.equals(output);
} }
// This version is for pointers
template<class T>
bool equalsDereferencedXML(const T& input = T()) {
T output;
roundtripXML<T>(input,output);
return input->equals(*output);
}
/* ************************************************************************* */ /* ************************************************************************* */
// Actual Tests // Actual Tests
/* ************************************************************************* */ /* ************************************************************************* */
@ -148,9 +158,12 @@ TEST (Serialization, text_geometry) {
EXPECT(equalsObj<gtsam::Point2>(Point2(1.0, 2.0))); EXPECT(equalsObj<gtsam::Point2>(Point2(1.0, 2.0)));
EXPECT(equalsObj<gtsam::Pose2>(Pose2(1.0, 2.0, 0.3))); EXPECT(equalsObj<gtsam::Pose2>(Pose2(1.0, 2.0, 0.3)));
EXPECT(equalsObj<gtsam::Rot2>(Rot2::fromDegrees(30.0))); EXPECT(equalsObj<gtsam::Rot2>(Rot2::fromDegrees(30.0)));
EXPECT(equalsObj<gtsam::Point3>(Point3(1.0, 2.0, 3.0)));
EXPECT(equalsObj<gtsam::Pose3>()); Point3 pt3(1.0, 2.0, 3.0);
EXPECT(equalsObj<gtsam::Rot3>(Rot3::RzRyRx(1.0, 3.0, 2.0))); Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0);
EXPECT(equalsObj<gtsam::Point3>(pt3));
EXPECT(equalsObj<gtsam::Rot3>(rt3));
EXPECT(equalsObj<gtsam::Pose3>(Pose3(rt3, pt3)));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -158,9 +171,13 @@ TEST (Serialization, xml_geometry) {
EXPECT(equalsXML<gtsam::Point2>(Point2(1.0, 2.0))); EXPECT(equalsXML<gtsam::Point2>(Point2(1.0, 2.0)));
EXPECT(equalsXML<gtsam::Pose2>(Pose2(1.0, 2.0, 0.3))); EXPECT(equalsXML<gtsam::Pose2>(Pose2(1.0, 2.0, 0.3)));
EXPECT(equalsXML<gtsam::Rot2>(Rot2::fromDegrees(30.0))); EXPECT(equalsXML<gtsam::Rot2>(Rot2::fromDegrees(30.0)));
EXPECT(equalsXML<gtsam::Point3>(Point3(1.0, 2.0, 3.0)));
EXPECT(equalsXML<gtsam::Pose3>()); Point3 pt3(1.0, 2.0, 3.0);
EXPECT(equalsXML<gtsam::Rot3>(Rot3::RzRyRx(1.0, 3.0, 2.0))); Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0);
EXPECT(equalsXML<gtsam::Point3>(pt3));
EXPECT(equalsXML<gtsam::Rot3>(rt3));
EXPECT(equalsXML<gtsam::Pose3>(Pose3(rt3, pt3)));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -178,17 +195,46 @@ TEST (Serialization, xml_linear) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST (Serialization, text_planar) { TEST (Serialization, noiseModels) {
EXPECT(equalsObj<gtsam::planarSLAM::PoseKey>(gtsam::planarSLAM::PoseKey(2))); SharedDiagonal diag3 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.2, 0.3));
EXPECT(equalsObj<gtsam::planarSLAM::PointKey>(gtsam::planarSLAM::PointKey(2))); SharedGaussian model3 = noiseModel::Isotropic::Sigma(3, 0.3);
// EXPECT(equalsObj<gtsam::planarSLAM::Values>());
EXPECT(equalsDereferenced<SharedDiagonal>(diag3));
EXPECT(equalsDereferencedXML<SharedDiagonal>(diag3));
// FAIL: Segfaults
// EXPECT(equalsDereferenced<SharedGaussian>(model3));
// EXPECT(equalsDereferencedXML<SharedGaussian>(model3));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST (Serialization, xml_planar) { TEST (Serialization, planar_system) {
EXPECT(equalsXML<gtsam::planarSLAM::PoseKey>(gtsam::planarSLAM::PoseKey(2))); using namespace planarSLAM;
EXPECT(equalsXML<gtsam::planarSLAM::PointKey>(gtsam::planarSLAM::PointKey(2)));
// EXPECT(equalsXML<gtsam::planarSLAM::Values>()); 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>(PoseKey(2)));
EXPECT(equalsObj<PointKey>(PointKey(3)));
EXPECT(equalsObj<Values>(values));
// EXPECT(equalsObj<Graph>(graph));
// xml
EXPECT(equalsXML<PoseKey>(PoseKey(2)));
EXPECT(equalsXML<PointKey>(PointKey(3)));
EXPECT(equalsXML<Values>(values));
// EXPECT(equalsXML<Graph>(graph));
} }
/* ************************************************************************* */ /* ************************************************************************* */