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

KISS refactor of NoiseModel

release/4.3a0
Frank Dellaert 2010-01-17 00:37:34 +00:00
parent 4518069218
commit 3a83e0678d
5 changed files with 166 additions and 252 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
.cproject
View File

@ -743,6 +743,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testNoiseModel.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testNoiseModel.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>

57
cpp/NoiseModel.cpp
View File

@ -2,7 +2,62 @@
* NoiseModel.cpp
*
* Created on: Jan 13, 2010
* Author: richard
* Author: Richard Roberts
* Author: Frank Dellaert
*/
//#include <boost/numeric/ublas/traits.hpp>
#include "NoiseModel.h"
namespace gtsam {
Vector Isotropic::whiten(const Vector& v) const {
return v * invsigma_;
}
Vector Isotropic::unwhiten(const Vector& v) const {
return v * sigma_;
}
Diagonal::Diagonal(const Vector& sigmas) :
sigmas_(sigmas), invsigmas_(1.0 / sigmas) {
}
Diagonal::Diagonal(const Diagonal& d) :
sigmas_(d.sigmas_), invsigmas_(d.invsigmas_) {
}
Vector Diagonal::whiten(const Vector& v) const {
return emul(v, invsigmas_);
}
Vector Diagonal::unwhiten(const Vector& v) const {
return emul(v, sigmas_);
}
Variances::Variances(const Vector& variances) {
sigmas_.resize(variances.size());
std::transform(variances.begin(), variances.end(), sigmas_.begin(), sqrt);
invsigmas_ = reciprocal(sigmas_);
}
FullCovariance::FullCovariance(const Matrix& cov) :
sqrt_covariance_(square_root_positive(cov)), sqrt_inv_covariance_(
inverse_square_root(cov)) {
}
FullCovariance::FullCovariance(const FullCovariance& cov) :
sqrt_covariance_(cov.sqrt_covariance_), sqrt_inv_covariance_(
cov.sqrt_inv_covariance_) {
}
Vector FullCovariance::whiten(const Vector& v) const {
return sqrt_inv_covariance_ * v;
}
Vector FullCovariance::unwhiten(const Vector& v) const {
return sqrt_covariance_ * v;
}
} // gtsam

247
cpp/NoiseModel.h
View File

@ -2,31 +2,24 @@
* NoiseModel.h
*
* Created on: Jan 13, 2010
* Author: richard
* Author: Richard Roberts
* Author: Frank Dellaert
*/
#pragma once
#include <boost/shared_ptr.hpp>
#include <boost/numeric/ublas/traits.hpp>
//#include <iostream>
//using namespace std;
#include "Vector.h"
#include "Matrix.h"
namespace gtsam {
// Forward declaration
class NoiseModel;
/*****************************************************************************
* NoiseModelBase is the abstract base class for all noise models. NoiseModels
/**
* NoiseModel is the abstract base class for all noise models. NoiseModels
* must implement a 'whiten' function to normalize an error vector, and an
* 'unwhiten' function to unnormalize an error vector.
*/
class NoiseModelBase {
public:
struct NoiseModel {
/**
* Whiten an error vector.
*/
@ -36,82 +29,14 @@ namespace gtsam {
* Unwhiten an error vector.
*/
virtual Vector unwhiten(const Vector& v) const = 0;
friend class NoiseModel;
private:
/**
* Used internally to duplicate the object while retaining the type.
*/
virtual boost::shared_ptr<NoiseModelBase> clone() const = 0;
};
/*****************************************************************************
* NoiseModel is a container for NoiseModelBase, which internally stores
* a shared_ptr to a NoiseModelBase as to support fast and compact storage and
* copies. Copying this class simply assigns the internal shared_ptr.
*/
class NoiseModel {
private:
const boost::shared_ptr<const NoiseModelBase> base_;
/**
* Fast constructor, simply assigns shared_ptr.
*/
NoiseModel(boost::shared_ptr<NoiseModelBase> noiseModel): base_(noiseModel) {
/*std::cout << "Assigning pointer" << std::endl;*/
}
public:
/**
* Fast copy constructor, simply assigns shared_ptr.
*/
NoiseModel(const NoiseModel& noiseModel): base_(noiseModel.base_) { /*std::cout << "Assigning pointer" << std::endl;*/ }
/**
* Constructor that creates a fast-copyable NoiseModel class by cloning
* a non-pointer NoiseModelBase. The type is retained and can be retrieved
* using a dynamic_cast.
*/
template<class T>
NoiseModel(const T& noiseModel): base_(noiseModel.clone()) {}
/**
* Cast to boost::shared_ptr<NoiseModelBase> to retrieve a pointer to the
* NoiseModelBase type. Can be used with dynamic_pointer_cast to retrieve
* the type at runtime.
* E.g.: dynamic_pointer_cast<const Isotropic>(noiseModel).
*/
operator const boost::shared_ptr<const NoiseModelBase> () const {
return base_; }
/**
* Call the NoiseModelBase virtual whiten function
*/
Vector whiten(const Vector& v) const { return base_->whiten(v); }
/**
* Call the NoiseModelBase virtual unwhiten function
*/
Vector unwhiten(const Vector& v) const { return base_->unwhiten(v); }
template<class T> friend boost::shared_ptr<const T> dynamic_pointer_cast(const NoiseModel& p);
};
template<class T>
boost::shared_ptr<const T> dynamic_pointer_cast(const NoiseModel& p) {
return boost::dynamic_pointer_cast<const T>(p.base_); }
/*****************************************************************************
* An isotropic noise model assigns the same sigma to each vector element.
* An isotropic noise model corresponds to a scaled diagonal covariance
* This class has no public constructors. Instead, use either either the
* Sigma or Variance class.
*/
class Isotropic : public NoiseModelBase {
class Isotropic : public NoiseModel {
protected:
double sigma_;
double invsigma_;
@ -121,125 +46,48 @@ namespace gtsam {
sigma_(isotropic.sigma_), invsigma_(isotropic.invsigma_) {}
public:
/**
* Whiten error vector by dividing by sigma
*/
virtual Vector whiten(const Vector& v) const { return v * invsigma_; }
/**
* Unwhiten error vector by multiplying by sigma
*/
virtual Vector unwhiten(const Vector& v) const { return v * sigma_; }
/**
* Clone is used to duplicate object while retaining type
*/
boost::shared_ptr<NoiseModelBase> clone() const {
/*cout << "Cloning Isotropic" << endl;*/
return boost::shared_ptr<NoiseModelBase>(new Isotropic(*this)); }
Vector whiten(const Vector& v) const;
Vector unwhiten(const Vector& v) const;
};
/*****************************************************************************
* A diagonal noise model implements a diagonal covariance matrix, with the
* elements of the diagonal specified in a Vector. This class has no public
* constructors, instead, use either the Sigmas or Variances class.
*/
class Diagonal : public NoiseModelBase {
protected:
Vector sigmas_;
Vector invsigmas_;
Diagonal() {}
Diagonal(const Vector& sigmas): sigmas_(sigmas), invsigmas_(1.0 / sigmas) {}
Diagonal(const Diagonal& d): sigmas_(d.sigmas_), invsigmas_(d.invsigmas_) {}
public:
/**
* Whiten error vector by dividing by sigmas
*/
virtual Vector whiten(const Vector& v) const { return emul(v, invsigmas_); }
/**
* Unwhiten error vector by multiplying by sigmas
*/
virtual Vector unwhiten(const Vector& v) const { return emul(v, sigmas_); }
/**
* Clone is used to duplicate object while retaining type
*/
boost::shared_ptr<NoiseModelBase> clone() const {
/*cout << "Cloning Isotropic" << endl;*/
return boost::shared_ptr<NoiseModelBase>(new Diagonal(*this)); }
};
/*****************************************************************************
* A full covariance noise model.
*/
class FullCovariance : public NoiseModelBase {
protected:
Matrix sqrt_covariance_;
Matrix sqrt_inv_covariance_;
public:
FullCovariance(const Matrix& covariance):
sqrt_covariance_(square_root_positive(covariance)),
sqrt_inv_covariance_(inverse_square_root(covariance)) {}
FullCovariance(const FullCovariance& c):
sqrt_covariance_(c.sqrt_covariance_), sqrt_inv_covariance_(c.sqrt_inv_covariance_) {}
/**
* Whiten error vector by dividing by sigmas
*/
virtual Vector whiten(const Vector& v) const { return sqrt_inv_covariance_ * v; }
/**
* Unwhiten error vector by multiplying by sigmas
*/
virtual Vector unwhiten(const Vector& v) const { return sqrt_covariance_ * v; }
/**
* Clone is used to duplicate object while retaining type
*/
boost::shared_ptr<NoiseModelBase> clone() const {
/*cout << "Cloning Isotropic" << endl;*/
return boost::shared_ptr<NoiseModelBase>(new FullCovariance(*this)); }
};
/*****************************************************************************
* An isotropic noise model using sigma, the noise standard
* deviation.
* An isotropic noise model using sigma, the standard deviation.
*/
class Sigma : public Isotropic {
public:
Sigma(const Sigma& isotropic): Isotropic(isotropic) { /*cout << "Constructing Sigma from Sigma" << endl;*/ }
Sigma(double sigma): Isotropic(sigma) { /*cout << "Constructing Sigma from double" << endl;*/ }
boost::shared_ptr<NoiseModelBase> clone() const {
return boost::shared_ptr<NoiseModelBase>(new Sigma(*this)); }
Sigma(const Sigma& isotropic): Isotropic(isotropic) {}
Sigma(double sigma): Isotropic(sigma) {}
};
/*****************************************************************************
/**
* An isotropic noise model using the noise variance = sigma^2.
*/
class Variance : public Isotropic {
public:
Variance(const Variance& v): Isotropic(v) {}
Variance(double variance): Isotropic(sqrt(variance)) {}
boost::shared_ptr<NoiseModelBase> clone() const {
return boost::shared_ptr<NoiseModelBase>(new Variance(*this)); }
};
/**
* A diagonal noise model implements a diagonal covariance matrix, with the
* elements of the diagonal specified in a Vector. This class has no public
* constructors, instead, use either the Sigmas or Variances class.
*/
class Diagonal : public NoiseModel {
protected:
Vector sigmas_;
Vector invsigmas_;
/*****************************************************************************
Diagonal() {}
Diagonal(const Vector& sigmas);
Diagonal(const Diagonal& d);
public:
Vector whiten(const Vector& v) const;
Vector unwhiten(const Vector& v) const;
};
/**
* A diagonal noise model created by specifying a Vector of sigmas, i.e.
* standard devations, i.e. the diagonal of the square root covariance
* matrix.
@ -247,29 +95,34 @@ namespace gtsam {
class Sigmas : public Diagonal {
public:
Sigmas(const Sigmas& s): Diagonal(s) {}
Sigmas(const Vector& sigmas): Diagonal(sigmas) {}
boost::shared_ptr<NoiseModelBase> clone() const {
return boost::shared_ptr<NoiseModelBase>(new Sigmas(*this)); }
};
/*****************************************************************************
/**
* A diagonal noise model created by specifying a Vector of variances, i.e.
* i.e. the diagonal of the covariance matrix.
*/
class Variances : public Diagonal {
public:
Variances(const Variances& s): Diagonal(s) {}
Variances(const Vector& variances);
};
Variances(const Vector& variances) {
std::transform(variances.begin(), variances.end(), sigmas_.begin(), sqrt);
invsigmas_ = 1.0 / sigmas_;
}
/**
* A full covariance noise model.
*/
class FullCovariance : public NoiseModel {
protected:
Matrix sqrt_covariance_;
Matrix sqrt_inv_covariance_;
boost::shared_ptr<NoiseModelBase> clone() const {
return boost::shared_ptr<NoiseModelBase>(new Variances(*this)); }
public:
FullCovariance(const Matrix& covariance);
FullCovariance(const FullCovariance& c);
Vector whiten(const Vector& v) const;
Vector unwhiten(const Vector& v) const;
};
}

34
cpp/testMatrix.cpp
View File

@ -606,24 +606,24 @@ TEST( matrix, inverse_square_root )
}
/* ************************************************************************* */
//TEST( matrix, square_root_positive )
//{
// Matrix cov = Matrix_(3,3,
// 4.25, 1.5, 0.0,
// 1.5, 2.0, 0.0,
// 0.0, 0.0, 1.0);
//
// Matrix expected = Matrix_(3,3,
// -2.0,-1.0, 0.0,
// -0.5, 1.0, 0.0,
// 0.0, 0.0, 1.0);
//
// Matrix actual = square_root_positive(cov);
//
// CHECK(assert_equal(expected, actual));
// CHECK(assert_equal(cov, prod(trans(actual),actual)));
//}
TEST( matrix, square_root_positive )
{
Matrix cov = Matrix_(3,3,
4.0, 0.0, 0.0,
0.0, 4.0, 0.0,
0.0, 0.0, 100.0
);
Matrix expected = Matrix_(3,3,
2.0, 0.0, 0.0,
0.0, 2.0, 0.0,
0.0, 0.0, 10.0
);
Matrix actual = square_root_positive(cov);
CHECK(assert_equal(expected, actual));
CHECK(assert_equal(cov, prod(trans(actual),actual)));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }

64
cpp/testNoiseModel.cpp
View File

@ -2,7 +2,8 @@
* testNoiseModel.cpp
*
* Created on: Jan 13, 2010
* Author: richard
* Author: Richard Roberts
* Author: Frank Dellaert
*/
#include <CppUnitLite/TestHarness.h>
@ -12,41 +13,38 @@
#include "NoiseModel.h"
using namespace std;
using namespace boost;
using namespace gtsam;
class TakesNoiseModel {
public:
NoiseModel noiseModel_;
public:
//template<class N>
TakesNoiseModel(const NoiseModel& noiseModel): noiseModel_(noiseModel) {}
};
TEST(NoiseModel, sharedptr)
/* ************************************************************************* */
TEST(NoiseModel, constructors)
{
// TakesNoiseModel tnm1(Sigma(1.0));
// cout << endl;
// TakesNoiseModel tnm2(tnm1.noiseModel_);
//
// if(dynamic_pointer_cast<Sigma>(tnm1.noiseModel_))
// cout << "tnm1 has a Sigma!" << endl;
// if(dynamic_pointer_cast<Variance>(tnm1.noiseModel_))
// cout << "tnm1 has a Variance!" << endl;
// if(dynamic_pointer_cast<Isotropic>(tnm1.noiseModel_))
// cout << "tnm1 has an Isotropic!" << endl;
// if(dynamic_pointer_cast<NoiseModelBase>(tnm1.noiseModel_))
// cout << "tnm1 has a NoiseModelBase!" << endl;
//
// if(dynamic_pointer_cast<Sigma>(tnm2.noiseModel_))
// cout << "tnm2 has a Sigma!" << endl;
// if(dynamic_pointer_cast<Variance>(tnm2.noiseModel_))
// cout << "tnm2 has a Variance!" << endl;
// if(dynamic_pointer_cast<Isotropic>(tnm2.noiseModel_))
// cout << "tnm2 has an Isotropic!" << endl;
// if(dynamic_pointer_cast<NoiseModelBase>(tnm2.noiseModel_))
// cout << "tnm2 has a NoiseModelBase!" << endl;
double sigma = 2, var = sigma*sigma;
Vector whitened = Vector_(3,5.0,10.0,15.0);
Vector unwhitened = Vector_(3,10.0,20.0,30.0);
// Construct noise models
Sigma m1(sigma);
Variance m2(var);
Sigmas m3(Vector_(3, sigma, sigma, sigma));
Variances m4(Vector_(3, var, var, var));
FullCovariance m5(Matrix_(3, 3,
var, 0.0, 0.0,
0.0, var, 0.0,
0.0, 0.0, var));
// test whiten
CHECK(assert_equal(whitened,m1.whiten(unwhitened)));
CHECK(assert_equal(whitened,m2.whiten(unwhitened)));
CHECK(assert_equal(whitened,m3.whiten(unwhitened)));
CHECK(assert_equal(whitened,m4.whiten(unwhitened)));
CHECK(assert_equal(whitened,m5.whiten(unwhitened)));
// test unwhiten
CHECK(assert_equal(unwhitened,m1.unwhiten(whitened)));
CHECK(assert_equal(unwhitened,m2.unwhiten(whitened)));
CHECK(assert_equal(unwhitened,m3.unwhiten(whitened)));
CHECK(assert_equal(unwhitened,m4.unwhiten(whitened)));
CHECK(assert_equal(unwhitened,m5.unwhiten(whitened)));
}
/* ************************************************************************* */