Added more serialization functionality to noiseModel, but SharedGaussians segfault on test, so tests for Nonlinear graphs are commented out
Alex Cunningham
parent
f7e30a5d52
commit
4865f1a64d
|
|
@ -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<const Gaussian*> (&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<vector<int>&> 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<std::vector<int>&> fi
|
|||
return mixed ? Constrained::MixedPrecisions(precisions) : Diagonal::Precisions(precisions);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
SharedDiagonal Constrained::QRColumnWise(ublas::matrix<double, ublas::column_major>& Ab, vector<int>& firstZeroRows) const {
|
||||
Matrix AbRowWise(Ab);
|
||||
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) {
|
||||
if (smart && fabs(variance-1.0)<1e-9) return Unit::Create(dim);
|
||||
return shared_ptr(new Isotropic(dim, sqrt(variance)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Isotropic::print(const string& name) const {
|
||||
cout << name << ": isotropic sigma " << " " << sigma_ << endl;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double Isotropic::Mahalanobis(const Vector& v) const {
|
||||
double dot = inner_prod(v, v);
|
||||
return dot * invsigma_ * invsigma_;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Isotropic::whiten(const Vector& v) const {
|
||||
return v * invsigma_;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Isotropic::unwhiten(const Vector& v) const {
|
||||
return v * sigma_;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix Isotropic::Whiten(const Matrix& H) const {
|
||||
return invsigma_ * H;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Isotropic::WhitenInPlace(Matrix& H) const {
|
||||
H *= invsigma_;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Isotropic::WhitenInPlace(MatrixColMajor& H) const {
|
||||
H *= invsigma_;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// faster version
|
||||
Vector Isotropic::sample() const {
|
||||
typedef boost::normal_distribution<double> 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;
|
||||
|
|
|
|||
|
|
@ -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<class ARCHIVE>
|
||||
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<Matrix>& sqrt_information = boost::none) :
|
||||
Gaussian(size_t dim = 1, const boost::optional<Matrix>& 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<class ARCHIVE>
|
||||
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<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
|
||||
|
||||
/**
|
||||
|
|
@ -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<class ARCHIVE>
|
||||
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<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 Matrix Whiten(const Matrix& H) const { return H; }
|
||||
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
|
||||
|
|
|
|||
|
|
@ -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<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
|
||||
|
|
|
|||
|
|
@ -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<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));
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -26,6 +26,7 @@
|
|||
using namespace boost::assign;
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/base/TestableAssertions.h>
|
||||
#include <gtsam/linear/NoiseModel.h>
|
||||
#include <gtsam/linear/SharedGaussian.h>
|
||||
#include <gtsam/linear/SharedDiagonal.h>
|
||||
|
|
@ -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<MatrixColMajor, ublas::upper>(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));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -148,7 +148,8 @@ namespace gtsam {
|
|||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
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<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
ar & boost::serialization::make_nvp("NonlinearFactor",
|
||||
boost::serialization::base_object<NonlinearFactor>(*this));
|
||||
boost::serialization::base_object<Base>(*this));
|
||||
ar & BOOST_SERIALIZATION_NVP(key_);
|
||||
}
|
||||
|
||||
|
|
@ -395,7 +396,7 @@ namespace gtsam {
|
|||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
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(key2_);
|
||||
}
|
||||
|
|
@ -571,7 +572,7 @@ namespace gtsam {
|
|||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
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(key2_);
|
||||
ar & BOOST_SERIALIZATION_NVP(key3_);
|
||||
|
|
|
|||
|
|
@ -53,6 +53,7 @@ namespace gtsam {
|
|||
inline const Rot2 measured() const {
|
||||
return z_;
|
||||
}
|
||||
|
||||
}; // BearingFactor
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -24,6 +24,8 @@
|
|||
#include <string>
|
||||
|
||||
// includes for standard serialization types
|
||||
#include <boost/serialization/optional.hpp>
|
||||
#include <boost/serialization/shared_ptr.hpp>
|
||||
#include <boost/serialization/vector.hpp>
|
||||
#include <boost/serialization/map.hpp>
|
||||
#include <boost/serialization/list.hpp>
|
||||
|
|
@ -120,6 +122,14 @@ bool equalsXML(const T& input = T()) {
|
|||
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
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -148,9 +158,12 @@ TEST (Serialization, text_geometry) {
|
|||
EXPECT(equalsObj<gtsam::Point2>(Point2(1.0, 2.0)));
|
||||
EXPECT(equalsObj<gtsam::Pose2>(Pose2(1.0, 2.0, 0.3)));
|
||||
EXPECT(equalsObj<gtsam::Rot2>(Rot2::fromDegrees(30.0)));
|
||||
EXPECT(equalsObj<gtsam::Point3>(Point3(1.0, 2.0, 3.0)));
|
||||
EXPECT(equalsObj<gtsam::Pose3>());
|
||||
EXPECT(equalsObj<gtsam::Rot3>(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<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::Pose2>(Pose2(1.0, 2.0, 0.3)));
|
||||
EXPECT(equalsXML<gtsam::Rot2>(Rot2::fromDegrees(30.0)));
|
||||
EXPECT(equalsXML<gtsam::Point3>(Point3(1.0, 2.0, 3.0)));
|
||||
EXPECT(equalsXML<gtsam::Pose3>());
|
||||
EXPECT(equalsXML<gtsam::Rot3>(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<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) {
|
||||
EXPECT(equalsObj<gtsam::planarSLAM::PoseKey>(gtsam::planarSLAM::PoseKey(2)));
|
||||
EXPECT(equalsObj<gtsam::planarSLAM::PointKey>(gtsam::planarSLAM::PointKey(2)));
|
||||
// EXPECT(equalsObj<gtsam::planarSLAM::Values>());
|
||||
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<SharedDiagonal>(diag3));
|
||||
EXPECT(equalsDereferencedXML<SharedDiagonal>(diag3));
|
||||
|
||||
// FAIL: Segfaults
|
||||
// EXPECT(equalsDereferenced<SharedGaussian>(model3));
|
||||
// EXPECT(equalsDereferencedXML<SharedGaussian>(model3));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST (Serialization, xml_planar) {
|
||||
EXPECT(equalsXML<gtsam::planarSLAM::PoseKey>(gtsam::planarSLAM::PoseKey(2)));
|
||||
EXPECT(equalsXML<gtsam::planarSLAM::PointKey>(gtsam::planarSLAM::PointKey(2)));
|
||||
// EXPECT(equalsXML<gtsam::planarSLAM::Values>());
|
||||
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>(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));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue