NoiseModel inhertance structure reversed, all constructors protected, new static "factories"
Frank Dellaert
parent
20b4436da3
commit
ea14959835
|
|
@ -300,7 +300,8 @@ void householder_update(Matrix &A, int j, double beta, const Vector& vjm) {
|
|||
// update A, b
|
||||
// A' \define A_{S}-ar and b'\define b-ad
|
||||
// __attribute__ ((noinline)) // uncomment to prevent inlining when profiling
|
||||
void updateAb(Matrix& A, Vector& b, int j, const Vector& a, const Vector& r, double d) {
|
||||
static void updateAb(Matrix& A, Vector& b, int j, const Vector& a,
|
||||
const Vector& r, double d) {
|
||||
const size_t m = A.size1(), n = A.size2();
|
||||
for (int i = 0; i < m; i++) { // update all rows
|
||||
double ai = a(i);
|
||||
|
|
@ -308,7 +309,7 @@ void updateAb(Matrix& A, Vector& b, int j, const Vector& a, const Vector& r, dou
|
|||
double *Aij = A.data().begin() + i * n + j + 1;
|
||||
const double *rptr = r.data().begin() + j + 1;
|
||||
// A(i,j+1:end) -= ai*r(j+1:end)
|
||||
for (int j2 = j + 1; j2 < n; j2++,Aij++,rptr++)
|
||||
for (int j2 = j + 1; j2 < n; j2++, Aij++, rptr++)
|
||||
*Aij -= ai * (*rptr);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -13,6 +13,15 @@ typedef ublas::matrix_column<Matrix> column;
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector GaussianNoiseModel::whiten(const Vector& v) const {
|
||||
return sqrt_information_ * v;
|
||||
}
|
||||
|
||||
Vector GaussianNoiseModel::unwhiten(const Vector& v) const {
|
||||
return backSubstituteUpper(sqrt_information_, v);
|
||||
}
|
||||
|
||||
// functional
|
||||
Matrix GaussianNoiseModel::Whiten(const Matrix& H) const {
|
||||
size_t m = H.size1(), n = H.size2();
|
||||
|
|
@ -35,17 +44,11 @@ namespace gtsam {
|
|||
}
|
||||
}
|
||||
|
||||
Vector Isotropic::whiten(const Vector& v) const {
|
||||
return v * invsigma_;
|
||||
}
|
||||
|
||||
Vector Isotropic::unwhiten(const Vector& v) const {
|
||||
return v * sigma_;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// TODO: can we avoid calling reciprocal twice ?
|
||||
Diagonal::Diagonal(const Vector& sigmas) :
|
||||
GaussianNoiseModel(sigmas.size()), sigmas_(sigmas), invsigmas_(reciprocal(
|
||||
sigmas)) {
|
||||
GaussianNoiseModel(diag(reciprocal(sigmas))),
|
||||
invsigmas_(reciprocal(sigmas)), sigmas_(sigmas) {
|
||||
}
|
||||
|
||||
Vector Diagonal::whiten(const Vector& v) const {
|
||||
|
|
@ -56,28 +59,15 @@ namespace gtsam {
|
|||
return emul(v, sigmas_);
|
||||
}
|
||||
|
||||
static Vector sqrt(const Vector& v) {
|
||||
Vector s(v.size());
|
||||
transform(v.begin(), v.end(), s.begin(), ::sqrt);
|
||||
return s;
|
||||
/* ************************************************************************* */
|
||||
|
||||
Vector Isotropic::whiten(const Vector& v) const {
|
||||
return v * invsigma_;
|
||||
}
|
||||
|
||||
Variances::Variances(const Vector& variances) :
|
||||
Diagonal(sqrt(variances)) {
|
||||
}
|
||||
|
||||
FullCovariance::FullCovariance(const Matrix& cov) :
|
||||
GaussianNoiseModel(cov.size1()),
|
||||
sqrt_covariance_(square_root_positive(cov)), sqrt_inv_covariance_(
|
||||
inverse_square_root(cov)) {
|
||||
}
|
||||
|
||||
Vector FullCovariance::whiten(const Vector& v) const {
|
||||
return sqrt_inv_covariance_ * v;
|
||||
}
|
||||
|
||||
Vector FullCovariance::unwhiten(const Vector& v) const {
|
||||
return sqrt_covariance_ * v;
|
||||
Vector Isotropic::unwhiten(const Vector& v) const {
|
||||
return v * sigma_;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
} // gtsam
|
||||
|
|
|
|||
236
cpp/NoiseModel.h
236
cpp/NoiseModel.h
|
|
@ -30,6 +30,7 @@ namespace gtsam {
|
|||
public:
|
||||
|
||||
NoiseModel(size_t dim):dim_(dim) {}
|
||||
virtual ~NoiseModel() {}
|
||||
|
||||
/**
|
||||
* Whiten an error vector.
|
||||
|
|
@ -39,7 +40,7 @@ namespace gtsam {
|
|||
/**
|
||||
* Unwhiten an error vector.
|
||||
*/
|
||||
virtual Vector unwhiten(const Vector& v) const = 0;
|
||||
virtual Vector unwhiten(const Vector& v) const = 0;
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
@ -50,139 +51,180 @@ namespace gtsam {
|
|||
* x = unwhiten(x) = inv(R)*y
|
||||
* as indeed
|
||||
* |y|^2 = y'*y = x'*R'*R*x
|
||||
* Various simplified models are available that implement this efficiently.
|
||||
* Various derived classes are available that are more efficient.
|
||||
*/
|
||||
struct GaussianNoiseModel : public NoiseModel {
|
||||
struct GaussianNoiseModel: public NoiseModel {
|
||||
|
||||
GaussianNoiseModel(size_t dim):NoiseModel(dim) {}
|
||||
protected:
|
||||
|
||||
// TODO: store as boost upper-triangular or whatever is passed from above
|
||||
/* Matrix square root of information matrix (R) */
|
||||
Matrix sqrt_information_;
|
||||
|
||||
/** protected constructor takes square root information matrix */
|
||||
GaussianNoiseModel(const Matrix& sqrt_information) :
|
||||
NoiseModel(sqrt_information.size1()), sqrt_information_(sqrt_information) {
|
||||
}
|
||||
|
||||
public:
|
||||
|
||||
typedef boost::shared_ptr<GaussianNoiseModel> shared_ptr;
|
||||
|
||||
/**
|
||||
* Return R itself, but note that Whiten(H) is cheaper than R*H
|
||||
*/
|
||||
virtual Matrix R() const = 0;
|
||||
* A Gaussian noise model created by specifying a square root information matrix.
|
||||
*/
|
||||
static shared_ptr SqrtInformation(const Matrix& R) {
|
||||
return shared_ptr(new GaussianNoiseModel(R));
|
||||
}
|
||||
|
||||
/**
|
||||
* Multiply a derivative with R (derivative of whiten)
|
||||
* Equivalent to whitening each column of the input matrix.
|
||||
*/
|
||||
Matrix Whiten(const Matrix& H) const;
|
||||
/**
|
||||
* A Gaussian noise model created by specifying a covariance matrix.
|
||||
*/
|
||||
static shared_ptr Covariance(const Matrix& Sigma) {
|
||||
return shared_ptr(new GaussianNoiseModel(inverse_square_root(Sigma)));
|
||||
}
|
||||
|
||||
/**
|
||||
* In-place version
|
||||
*/
|
||||
void WhitenInPlace(Matrix& H) const;
|
||||
};
|
||||
/**
|
||||
* A Gaussian noise model created by specifying an information matrix.
|
||||
*/
|
||||
static shared_ptr Information(const Matrix& Q) {
|
||||
return shared_ptr(new GaussianNoiseModel(square_root_positive(Q)));
|
||||
}
|
||||
|
||||
virtual Vector whiten(const Vector& v) const;
|
||||
virtual Vector unwhiten(const Vector& v) const;
|
||||
|
||||
/**
|
||||
* Multiply a derivative with R (derivative of whiten)
|
||||
* Equivalent to whitening each column of the input matrix.
|
||||
*/
|
||||
virtual Matrix Whiten(const Matrix& H) const;
|
||||
|
||||
/**
|
||||
* In-place version
|
||||
*/
|
||||
virtual void WhitenInPlace(Matrix& H) const;
|
||||
|
||||
/**
|
||||
* Return R itself, but note that Whiten(H) is cheaper than R*H
|
||||
*/
|
||||
const Matrix& R() const {
|
||||
return sqrt_information_;
|
||||
}
|
||||
|
||||
}; // GaussianNoiseModel
|
||||
|
||||
/**
|
||||
* We identify the Gaussian noise model with R
|
||||
*/
|
||||
// FD: does not work, ambiguous overload :-(
|
||||
// inline Vector operator*(const GaussianNoiseModel& R, const Vector& v) {return R.whiten(v);}
|
||||
|
||||
/**
|
||||
* UnitCovariance: i.i.d. noise on all m dimensions.
|
||||
* 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 the static constructor functions Sigmas etc...
|
||||
*/
|
||||
class UnitCovariance : public GaussianNoiseModel {
|
||||
class Diagonal : public GaussianNoiseModel {
|
||||
protected:
|
||||
double sigma_;
|
||||
double invsigma_;
|
||||
|
||||
UnitCovariance(size_t dim): GaussianNoiseModel(dim) {}
|
||||
/** sigmas and reciprocal */
|
||||
Vector sigmas_, invsigmas_;
|
||||
|
||||
/** protected constructor takes sigmas */
|
||||
Diagonal(const Vector& sigmas);
|
||||
|
||||
public:
|
||||
Vector whiten(const Vector& v) const { return v; }
|
||||
Vector unwhiten(const Vector& v) const { return v; }
|
||||
Matrix R() const { return eye(dim_); }
|
||||
Matrix Whiten(const Matrix& H) const { return H; }
|
||||
void WhitenInPlace(Matrix& H) const {}
|
||||
|
||||
typedef boost::shared_ptr<Diagonal> shared_ptr;
|
||||
|
||||
/**
|
||||
* A diagonal noise model created by specifying a Vector of sigmas, i.e.
|
||||
* standard devations, the diagonal of the square root covariance matrix.
|
||||
*/
|
||||
static shared_ptr Sigmas(const Vector& sigmas) {
|
||||
return shared_ptr(new Diagonal(sigmas));
|
||||
}
|
||||
|
||||
/**
|
||||
* A diagonal noise model created by specifying a Vector of variances, i.e.
|
||||
* i.e. the diagonal of the covariance matrix.
|
||||
*/
|
||||
static shared_ptr Variances(const Vector& variances) {
|
||||
return shared_ptr(new Diagonal(esqrt(variances)));
|
||||
}
|
||||
|
||||
/**
|
||||
* A diagonal noise model created by specifying a Vector of precisions, i.e.
|
||||
* i.e. the diagonal of the information matrix, i.e., weights
|
||||
*/
|
||||
static shared_ptr Precisions(const Vector& precisions) {
|
||||
return Variances(reciprocal(precisions));
|
||||
}
|
||||
|
||||
virtual Vector whiten(const Vector& v) const;
|
||||
virtual Vector unwhiten(const Vector& v) const;
|
||||
};
|
||||
|
||||
/**
|
||||
* 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.
|
||||
* To construct, use one of the static methods
|
||||
*/
|
||||
class Isotropic : public GaussianNoiseModel {
|
||||
class Isotropic : public Diagonal {
|
||||
protected:
|
||||
double sigma_;
|
||||
double invsigma_;
|
||||
double sigma_, invsigma_;
|
||||
|
||||
/** protected constructor takes sigma */
|
||||
Isotropic(size_t dim, double sigma) :
|
||||
GaussianNoiseModel(dim), sigma_(sigma), invsigma_(1.0 / sigma) {}
|
||||
Diagonal(repeat(dim, sigma)),sigma_(sigma),invsigma_(1.0/sigma) {}
|
||||
|
||||
public:
|
||||
Vector whiten(const Vector& v) const;
|
||||
Vector unwhiten(const Vector& v) const;
|
||||
Matrix R() const { return diag(repeat(dim_,invsigma_)); }
|
||||
|
||||
typedef boost::shared_ptr<Isotropic> shared_ptr;
|
||||
|
||||
/**
|
||||
* An isotropic noise model created by specifying a standard devation sigma
|
||||
*/
|
||||
static shared_ptr Sigma(size_t dim, double sigma) {
|
||||
return shared_ptr(new Isotropic(dim, sigma));
|
||||
}
|
||||
|
||||
/**
|
||||
* An isotropic noise model created by specifying a variance = sigma^2.
|
||||
*/
|
||||
static shared_ptr Variance(size_t dim, double variance) {
|
||||
return shared_ptr(new Isotropic(dim, sqrt(variance)));
|
||||
}
|
||||
|
||||
/**
|
||||
* An isotropic noise model created by specifying a precision
|
||||
*/
|
||||
static shared_ptr Precision(size_t dim, double precision) {
|
||||
return Variance(dim, 1.0/precision);
|
||||
}
|
||||
|
||||
virtual Vector whiten(const Vector& v) const;
|
||||
virtual Vector unwhiten(const Vector& v) const;
|
||||
};
|
||||
|
||||
/**
|
||||
* An isotropic noise model using sigma, the standard deviation.
|
||||
* UnitCovariance: i.i.d. unit-variance noise on all m dimensions.
|
||||
*/
|
||||
class Sigma : public Isotropic {
|
||||
public:
|
||||
Sigma(size_t n, double sigma): Isotropic(n, sigma) {}
|
||||
};
|
||||
|
||||
/**
|
||||
* An isotropic noise model using the noise variance = sigma^2.
|
||||
*/
|
||||
class Variance : public Isotropic {
|
||||
public:
|
||||
Variance(size_t n, double variance): Isotropic(n, sqrt(variance)) {}
|
||||
};
|
||||
|
||||
/**
|
||||
* 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 GaussianNoiseModel {
|
||||
class UnitCovariance : public Isotropic {
|
||||
protected:
|
||||
Vector sigmas_;
|
||||
Vector invsigmas_;
|
||||
|
||||
Diagonal(const Vector& sigmas);
|
||||
UnitCovariance(size_t dim): Isotropic(dim,1.0) {}
|
||||
|
||||
public:
|
||||
Vector whiten(const Vector& v) const;
|
||||
Vector unwhiten(const Vector& v) const;
|
||||
Matrix R() const { return diag(invsigmas_); }
|
||||
};
|
||||
|
||||
/**
|
||||
* 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.
|
||||
*/
|
||||
class Sigmas : public Diagonal {
|
||||
public:
|
||||
Sigmas(const Vector& sigmas): Diagonal(sigmas) {}
|
||||
};
|
||||
typedef boost::shared_ptr<UnitCovariance> shared_ptr;
|
||||
|
||||
/**
|
||||
* 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 Vector& variances);
|
||||
};
|
||||
/**
|
||||
* An isotropic noise model created by specifying a standard devation sigma
|
||||
*/
|
||||
static shared_ptr Create(size_t dim);
|
||||
|
||||
/**
|
||||
* A full covariance noise model.
|
||||
*/
|
||||
class FullCovariance : public GaussianNoiseModel {
|
||||
protected:
|
||||
Matrix sqrt_covariance_;
|
||||
Matrix sqrt_inv_covariance_;
|
||||
|
||||
public:
|
||||
FullCovariance(const Matrix& covariance);
|
||||
FullCovariance(const FullCovariance& c);
|
||||
Vector whiten(const Vector& v) const;
|
||||
Vector unwhiten(const Vector& v) const;
|
||||
Matrix R() const { return sqrt_inv_covariance_; }
|
||||
virtual Vector whiten(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 void WhitenInPlace(Matrix& H) const {}
|
||||
};
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -38,7 +38,7 @@ namespace gtsam {
|
|||
|
||||
/** Constructor */
|
||||
PriorFactor(const Key& key, const T& prior, const Matrix& cov) :
|
||||
Base(1.0, key), prior_(prior), noiseModel_(new FullCovariance(cov)) {
|
||||
Base(1.0, key), prior_(prior), noiseModel_(GaussianNoiseModel::Covariance(cov)) {
|
||||
}
|
||||
|
||||
/** implement functions needed for Testable */
|
||||
|
|
|
|||
|
|
@ -204,6 +204,13 @@ namespace gtsam {
|
|||
return b;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector esqrt(const Vector& v) {
|
||||
Vector s(v.size());
|
||||
transform(v.begin(), v.end(), s.begin(), ::sqrt);
|
||||
return s;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double max(const Vector &a) {
|
||||
return *(std::max_element(a.begin(), a.end()));
|
||||
|
|
|
|||
|
|
@ -177,6 +177,13 @@ double sum(const Vector &a);
|
|||
*/
|
||||
Vector reciprocal(const Vector &a);
|
||||
|
||||
/**
|
||||
* elementwise sqrt of vector elements
|
||||
* @param a vector
|
||||
* @return [sqrt(a(i))]
|
||||
*/
|
||||
Vector esqrt(const Vector& v);
|
||||
|
||||
/**
|
||||
* return the max element of a vector
|
||||
* @param a vector
|
||||
|
|
|
|||
|
|
@ -9,7 +9,9 @@
|
|||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <boost/tuple/tuple.hpp>
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/numeric/ublas/matrix_proxy.hpp>
|
||||
#include "Matrix.h"
|
||||
#include "NoiseModel.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
|
@ -545,6 +547,87 @@ TEST( matrix, svd )
|
|||
EQUALITY(S,S1);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// update A, b
|
||||
// A' \define A_{S}-ar and b'\define b-ad
|
||||
// __attribute__ ((noinline)) // uncomment to prevent inlining when profiling
|
||||
static void updateAb(Matrix& A, Vector& b, int j, const Vector& a,
|
||||
const Vector& r, double d) {
|
||||
const size_t m = A.size1(), n = A.size2();
|
||||
for (int i = 0; i < m; i++) { // update all rows
|
||||
double ai = a(i);
|
||||
b(i) -= ai * d;
|
||||
double *Aij = A.data().begin() + i * n + j + 1;
|
||||
const double *rptr = r.data().begin() + j + 1;
|
||||
// A(i,j+1:end) -= ai*r(j+1:end)
|
||||
for (int j2 = j + 1; j2 < n; j2++, Aij++, rptr++)
|
||||
*Aij -= ai * (*rptr);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
list<boost::tuple<Vector, double, double> >
|
||||
weighted_eliminate2(Matrix& A, Vector& b, const GaussianNoiseModel& model) {
|
||||
size_t m = A.size1(), n = A.size2(); // get size(A)
|
||||
size_t maxRank = min(m,n);
|
||||
|
||||
// pre-whiten everything
|
||||
model.WhitenInPlace(A);
|
||||
b = model.whiten(b);
|
||||
|
||||
// create list
|
||||
list<boost::tuple<Vector, double, double> > results;
|
||||
|
||||
// We loop over all columns, because the columns that can be eliminated
|
||||
// are not necessarily contiguous. For each one, estimate the corresponding
|
||||
// scalar variable x as d-rS, with S the separator (remaining columns).
|
||||
// Then update A and b by substituting x with d-rS, zero-ing out x's column.
|
||||
for (int j=0; j<n; ++j) {
|
||||
// extract the first column of A
|
||||
Vector a(column(A, j)); // ublas::matrix_column is slower !
|
||||
|
||||
// Calculate weighted pseudo-inverse and corresponding precision
|
||||
double precision = dot(a,a);
|
||||
Vector pseudo = a/precision;
|
||||
|
||||
// if precision is zero, no information on this column
|
||||
if (precision < 1e-8) continue;
|
||||
|
||||
// create solution and copy into r
|
||||
Vector r(basis(n, j));
|
||||
for (int j2=j+1; j2<n; ++j2) // expensive !!
|
||||
r(j2) = inner_prod(pseudo, boost::numeric::ublas::matrix_column<Matrix>(A, j2));
|
||||
|
||||
// create the rhs
|
||||
double d = inner_prod(pseudo, b);
|
||||
|
||||
// construct solution (r, d, sigma)
|
||||
// TODO: avoid sqrt, store precision or at least variance
|
||||
results.push_back(boost::make_tuple(r, d, 1./sqrt(precision)));
|
||||
|
||||
// exit after rank exhausted
|
||||
if (results.size()>=maxRank) break;
|
||||
|
||||
// update A, b, expensive, suing outer product
|
||||
// A' \define A_{S}-a*r and b'\define b-d*a
|
||||
updateAb(A, b, j, a, r, d);
|
||||
}
|
||||
|
||||
return results;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void weighted_eliminate3(Matrix& A, Vector& b, const GaussianNoiseModel& model) {
|
||||
size_t m = A.size1(), n = A.size2(); // get size(A)
|
||||
size_t maxRank = min(m,n);
|
||||
|
||||
// pre-whiten everything
|
||||
model.WhitenInPlace(A);
|
||||
b = model.whiten(b);
|
||||
|
||||
householder_(A, maxRank);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( matrix, weighted_elimination )
|
||||
{
|
||||
|
|
@ -552,21 +635,17 @@ TEST( matrix, weighted_elimination )
|
|||
Matrix A = Matrix_(4, 6,
|
||||
-1., 0., 1., 0., 0., 0.,
|
||||
0., -1., 0., 1., 0., 0.,
|
||||
1., 0., 0., 0., -1., 0.,
|
||||
0., 1., 0., 0., 0., -1.);
|
||||
1., 0., 0., 0., -1., 0.,
|
||||
0., 1., 0., 0., 0., -1.);
|
||||
Vector b = Vector_(4, -0.2, 0.3, 0.2, -0.1);
|
||||
Vector sigmas = Vector_(4, 0.2, 0.2, 0.1, 0.1);
|
||||
|
||||
// perform elimination
|
||||
std::list<boost::tuple<Vector, double, double> > solution =
|
||||
weighted_eliminate(A, b, sigmas);
|
||||
|
||||
// expected values
|
||||
Matrix expectedR = Matrix_(4, 6,
|
||||
1., 0.,-0.2, 0.,-0.8, 0.,
|
||||
0., 1., 0.,-0.2, 0.,-0.8,
|
||||
0., 0., 1., 0., -1., 0.,
|
||||
0., 0., 0., 1., 0., -1.);
|
||||
1., 0., -0.2, 0., -0.8, 0.,
|
||||
0., 1., 0.,-0.2, 0., -0.8,
|
||||
0., 0., 1., 0., -1., 0.,
|
||||
0., 0., 0., 1., 0., -1.);
|
||||
Vector d = Vector_(4, 0.2, -0.14, 0.0, 0.2);
|
||||
Vector newSigmas = Vector_(4,
|
||||
0.0894427,
|
||||
|
|
@ -574,15 +653,43 @@ TEST( matrix, weighted_elimination )
|
|||
0.223607,
|
||||
0.223607);
|
||||
|
||||
// unpack and verify
|
||||
Vector r; double di, sigma;
|
||||
size_t i = 0;
|
||||
size_t i;
|
||||
|
||||
// perform elimination
|
||||
Matrix A1 = A; Vector b1 = b;
|
||||
std::list<boost::tuple<Vector, double, double> > solution =
|
||||
weighted_eliminate(A1, b1, sigmas);
|
||||
|
||||
// unpack and verify
|
||||
i=0;
|
||||
BOOST_FOREACH(boost::tie(r, di, sigma), solution) {
|
||||
CHECK(assert_equal(r, row(expectedR, i))); // verify r
|
||||
DOUBLES_EQUAL(d(i), di, 1e-8); // verify d
|
||||
DOUBLES_EQUAL(newSigmas(i), sigma, 1e-5); // verify sigma
|
||||
i += 1;
|
||||
}
|
||||
|
||||
// perform elimination with NoiseModel
|
||||
Matrix A2 = A; Vector b2 = b;
|
||||
GaussianNoiseModel::shared_ptr model = Diagonal::Sigmas(sigmas);
|
||||
std::list<boost::tuple<Vector, double, double> > solution2 =
|
||||
weighted_eliminate2(A2, b2, *model);
|
||||
|
||||
// unpack and verify
|
||||
i=0;
|
||||
BOOST_FOREACH(boost::tie(r, di, sigma), solution2) {
|
||||
CHECK(assert_equal(r, row(expectedR, i))); // verify r
|
||||
DOUBLES_EQUAL(d(i), di, 1e-8); // verify d
|
||||
DOUBLES_EQUAL(newSigmas(i), sigma, 1e-5); // verify sigma
|
||||
i += 1;
|
||||
}
|
||||
|
||||
// perform elimination with NoiseModel
|
||||
weighted_eliminate3(A, b, *model);
|
||||
GaussianNoiseModel::shared_ptr newModel = Diagonal::Sigmas(newSigmas);
|
||||
// print(A);
|
||||
// print(newModel->Whiten(expectedR));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -8,7 +8,7 @@
|
|||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include <boost/shared_ptr.hpp>
|
||||
#include <boost/foreach.hpp>
|
||||
#include <iostream>
|
||||
#include "NoiseModel.h"
|
||||
|
||||
|
|
@ -18,46 +18,48 @@ using namespace gtsam;
|
|||
/* ************************************************************************* */
|
||||
TEST(NoiseModel, constructors)
|
||||
{
|
||||
double sigma = 2, var = sigma*sigma;
|
||||
double sigma = 2, s_1=1.0/sigma, var = sigma*sigma, prc = 1.0/var;
|
||||
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(3,sigma);
|
||||
Variance m2(3,var);
|
||||
Sigmas m3(Vector_(3, sigma, sigma, sigma));
|
||||
Variances m4(Vector_(3, var, var, var));
|
||||
FullCovariance m5(Matrix_(3, 3,
|
||||
vector<GaussianNoiseModel::shared_ptr> m;
|
||||
m.push_back(GaussianNoiseModel::SqrtInformation(Matrix_(3, 3,
|
||||
s_1, 0.0, 0.0,
|
||||
0.0, s_1, 0.0,
|
||||
0.0, 0.0, s_1)));
|
||||
m.push_back(GaussianNoiseModel::Covariance(Matrix_(3, 3,
|
||||
var, 0.0, 0.0,
|
||||
0.0, var, 0.0,
|
||||
0.0, 0.0, var));
|
||||
0.0, 0.0, var)));
|
||||
m.push_back(GaussianNoiseModel::Information(Matrix_(3, 3,
|
||||
prc, 0.0, 0.0,
|
||||
0.0, prc, 0.0,
|
||||
0.0, 0.0, prc)));
|
||||
m.push_back(Diagonal::Sigmas(Vector_(3, sigma, sigma, sigma)));
|
||||
m.push_back(Diagonal::Variances(Vector_(3, var, var, var)));
|
||||
m.push_back(Diagonal::Precisions(Vector_(3, prc, prc, prc)));
|
||||
m.push_back(Isotropic::Sigma(3, sigma));
|
||||
m.push_back(Isotropic::Variance(3, var));
|
||||
m.push_back(Isotropic::Precision(3, prc));
|
||||
|
||||
// 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)));
|
||||
int i=0;
|
||||
BOOST_FOREACH(GaussianNoiseModel::shared_ptr mi, m)
|
||||
CHECK(assert_equal(whitened,mi->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)));
|
||||
BOOST_FOREACH(GaussianNoiseModel::shared_ptr mi, m)
|
||||
CHECK(assert_equal(unwhitened,mi->unwhiten(whitened)));
|
||||
|
||||
// test R matrix
|
||||
double s_1 = 1.0/sigma;
|
||||
Matrix expectedR(Matrix_(3, 3,
|
||||
s_1, 0.0, 0.0,
|
||||
0.0, s_1, 0.0,
|
||||
0.0, 0.0, s_1));
|
||||
|
||||
CHECK(assert_equal(expectedR,m1.R()));
|
||||
CHECK(assert_equal(expectedR,m2.R()));
|
||||
CHECK(assert_equal(expectedR,m3.R()));
|
||||
CHECK(assert_equal(expectedR,m4.R()));
|
||||
CHECK(assert_equal(expectedR,m5.R()));
|
||||
BOOST_FOREACH(GaussianNoiseModel::shared_ptr mi, m)
|
||||
CHECK(assert_equal(expectedR,mi->R()));
|
||||
|
||||
// test Whiten operator
|
||||
Matrix H(Matrix_(3, 4,
|
||||
|
|
@ -70,14 +72,11 @@ TEST(NoiseModel, constructors)
|
|||
0.0, s_1, 0.0, s_1,
|
||||
s_1, 0.0, 0.0, s_1));
|
||||
|
||||
CHECK(assert_equal(expected,m1.Whiten(H)));
|
||||
CHECK(assert_equal(expected,m2.Whiten(H)));
|
||||
CHECK(assert_equal(expected,m3.Whiten(H)));
|
||||
CHECK(assert_equal(expected,m4.Whiten(H)));
|
||||
CHECK(assert_equal(expected,m5.Whiten(H)));
|
||||
BOOST_FOREACH(GaussianNoiseModel::shared_ptr mi, m)
|
||||
CHECK(assert_equal(expected,mi->Whiten(H)));
|
||||
|
||||
// can only test inplace version once :-)
|
||||
m5.WhitenInPlace(H);
|
||||
m[0]->WhitenInPlace(H);
|
||||
CHECK(assert_equal(expected,H));
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -13,7 +13,7 @@ using namespace gtsam;
|
|||
|
||||
// Common measurement covariance
|
||||
static double sx=0.5, sy=0.5,st=0.1;
|
||||
static boost::shared_ptr<GaussianNoiseModel> model(new Sigmas(Vector_(3,sx,sy,st)));
|
||||
static GaussianNoiseModel::shared_ptr model = Diagonal::Sigmas(Vector_(3,sx,sy,st));
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Very simple test establishing Ax-b \approx z-h(x)
|
||||
|
|
|
|||
|
|
@ -1,4 +1,4 @@
|
|||
./configure --prefix=$HOME --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/
|
||||
|
||||
# for maximum performance on Intel Core2 platform:
|
||||
#./configure --prefix=$HOME --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/ CXXFLAGS=" -g -O3 -march=nocona -mtune=generic -DNDEBUG" --disable-static
|
||||
#./configure --prefix=$HOME --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/ CXXFLAGS=" -g -O3 -ftree-vectorizer-verbose=2 -march=nocona -mtune=generic -DNDEBUG" --disable-static
|
||||
Loading…
Reference in New Issue