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

Merged NoiseQR back into trunk

release/4.3a0
Alex Cunningham 2010-01-20 18:32:48 +00:00
parent 9c9007920a
commit 5f588031bc
25 changed files with 368 additions and 238 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.cproject
View File

@ -19,7 +19,7 @@
<folderInfo id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.2031210194" name="/" resourcePath="">
<toolChain id="cdt.managedbuild.toolchain.gnu.macosx.base.677243255" name="cdt.managedbuild.toolchain.gnu.macosx.base" superClass="cdt.managedbuild.toolchain.gnu.macosx.base">
<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.MachO" id="cdt.managedbuild.target.gnu.platform.macosx.base.752782918" name="Debug Platform" osList="macosx" superClass="cdt.managedbuild.target.gnu.platform.macosx.base"/>
<builder id="cdt.managedbuild.target.gnu.builder.macosx.base.319933862" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="2" superClass="cdt.managedbuild.target.gnu.builder.macosx.base"/>
<builder arguments="" command="make" id="cdt.managedbuild.target.gnu.builder.macosx.base.319933862" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="2" superClass="cdt.managedbuild.target.gnu.builder.macosx.base"/>
<tool id="cdt.managedbuild.tool.macosx.c.linker.macosx.base.457360678" name="MacOS X C Linker" superClass="cdt.managedbuild.tool.macosx.c.linker.macosx.base"/>
<tool id="cdt.managedbuild.tool.macosx.cpp.linker.macosx.base.1011140787" name="MacOS X C++ Linker" superClass="cdt.managedbuild.tool.macosx.cpp.linker.macosx.base">
<inputType id="cdt.managedbuild.tool.macosx.cpp.linker.input.1032375444" superClass="cdt.managedbuild.tool.macosx.cpp.linker.input">

2
.project
View File

@ -59,7 +59,7 @@
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.useDefaultBuildCmd</key>
<value>true</value>
<value>false</value>
</dictionary>
</arguments>
</buildCommand>

2
cpp/GaussianConditional.cpp
View File

@ -101,7 +101,7 @@ Vector GaussianConditional::solve(const VectorConfig& x) const {
const Matrix& Aj = it->second;
rhs -= Aj * x[j];
}
Vector result = backSubstituteUpper(R_, rhs, true);
Vector result = backSubstituteUpper(R_, rhs, false);
return result;
}

85
cpp/GaussianFactor.cpp
View File

@ -298,6 +298,86 @@ void GaussianFactor::append_factor(GaussianFactor::shared_ptr f, size_t m, size_
* and last rows to make a new joint linear factor on separator
*/
/* ************************************************************************* */
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
pair<GaussianConditional::shared_ptr, GaussianFactor::shared_ptr>
GaussianFactor::eliminate(const Symbol& key) const
{
// if this factor does not involve key, we exit with empty CG and LF
const_iterator it = As_.find(key);
if (it==As_.end()) {
// Conditional Gaussian is just a parent-less node with P(x)=1
GaussianFactor::shared_ptr lf(new GaussianFactor);
GaussianConditional::shared_ptr cg(new GaussianConditional(key));
return make_pair(cg,lf);
}
// create an internal ordering that eliminates key first
Ordering ordering;
ordering += key;
BOOST_FOREACH(const Symbol& k, keys())
if (k != key) ordering += k;
// extract [A b] from the combined linear factor (ensure that x is leading)
Matrix Ab = matrix_augmented(ordering,false);
// Use in-place QR on dense Ab appropriate to NoiseModel
sharedDiagonal noiseModel = model_->QR(Ab);
// get dimensions of the eliminated variable
// TODO: this is another map find that should be avoided !
size_t n1 = getDim(key), n = Ab.size2() - 1;
// if m<n1, this factor cannot be eliminated
size_t maxRank = noiseModel->dim();
if (maxRank<n1) {
cout << "Perhaps your factor graph is singular." << endl;
cout << "Here are the keys involved in the factor now being eliminated:" << endl;
ordering.print("Keys");
cout << "The first key, '" << (string)ordering.front() << "', corresponds to the variable being eliminated" << endl;
throw(domain_error("GaussianFactor::eliminate: fewer constraints than unknowns"));
}
// Get alias to augmented RHS d
ublas::matrix_column<Matrix> d(Ab,n);
// create base conditional Gaussian
GaussianConditional::shared_ptr conditional(new GaussianConditional(key,
sub(d, 0, n1), // form d vector
sub(Ab, 0, n1, 0, n1), // form R matrix
sub(noiseModel->sigmas(),0,n1))); // get standard deviations
// extract the block matrices for parents in both CG and LF
GaussianFactor::shared_ptr factor(new GaussianFactor);
size_t j = n1;
BOOST_FOREACH(Symbol& cur_key, ordering)
if (cur_key!=key) {
size_t dim = getDim(cur_key); // TODO avoid find !
conditional->add(cur_key, sub(Ab, 0, n1, j, j+dim));
factor->insert(cur_key, sub(Ab, n1, maxRank, j, j+dim));
j+=dim;
}
// Set sigmas
factor->model_ = noiseModel::Diagonal::Sigmas(sub(noiseModel->sigmas(),n1,maxRank));
// extract ds vector for the new b
factor->set_b(sub(d, n1, maxRank));
return make_pair(conditional, factor);
}
/* ************************************************************************* */
/* Note, in place !!!!
* Do incomplete QR factorization for the first n columns
* We will do QR on all matrices and on RHS
* Then take first n rows and make a GaussianConditional,
* and last rows to make a new joint linear factor on separator
*/
/* ************************************************************************* *
pair<GaussianConditional::shared_ptr, GaussianFactor::shared_ptr>
GaussianFactor::eliminate(const Symbol& key) const
{
@ -327,9 +407,6 @@ GaussianFactor::eliminate(const Symbol& key) const
std::list<boost::tuple<Vector, double, double> > solution =
weighted_eliminate(A, b, model_->sigmas());
// TODO, fix using NoiseModel, the read out Ab
// model->QR(Ab)
// get dimensions of the eliminated variable
// TODO: this is another map find that should be avoided !
size_t n1 = getDim(key);
@ -395,7 +472,7 @@ GaussianFactor::shared_ptr GaussianFactor::alphaFactor(const Symbol& key, const
Vector b = - unweighted_error(x);
// construct factor
shared_ptr factor(new GaussianFactor(key,Matrix_(A),b,model_->sigmas()));
shared_ptr factor(new GaussianFactor(key,Matrix_(A),b,model_));
return factor;
}

40
cpp/GaussianFactor.h
View File

@ -55,32 +55,16 @@ public:
/** Construct unary factor */
GaussianFactor(const Symbol& key1, const Matrix& A1,
const Vector& b, double sigma) :
b_(b), model_(noiseModel::Isotropic::Sigma(b.size(),sigma)) {
As_.insert(make_pair(key1, A1));
}
/** Construct unary factor with vector of sigmas */
GaussianFactor(const Symbol& key1, const Matrix& A1,
const Vector& b, const Vector& sigmas) :
b_(b), model_(noiseModel::Diagonal::Sigmas(sigmas)) {
const Vector& b, const sharedDiagonal& model) :
b_(b), model_(model) {
As_.insert(make_pair(key1, A1));
}
/** Construct binary factor */
GaussianFactor(const Symbol& key1, const Matrix& A1,
const Symbol& key2, const Matrix& A2,
const Vector& b, double sigma) :
b_(b), model_(noiseModel::Isotropic::Sigma(b.size(),sigma)) {
As_.insert(make_pair(key1, A1));
As_.insert(make_pair(key2, A2));
}
/** Construct binary factor with vector of sigmas */
GaussianFactor(const Symbol& key1, const Matrix& A1,
const Symbol& key2, const Matrix& A2,
const Vector& b, const Vector& sigmas) :
b_(b), model_(noiseModel::Diagonal::Sigmas(sigmas)) {
const Vector& b, const sharedDiagonal& model) :
b_(b), model_(model) {
As_.insert(make_pair(key1, A1));
As_.insert(make_pair(key2, A2));
}
@ -89,8 +73,8 @@ public:
GaussianFactor(const Symbol& key1, const Matrix& A1,
const Symbol& key2, const Matrix& A2,
const Symbol& key3, const Matrix& A3,
const Vector& b, double sigma) :
b_(b), model_(noiseModel::Isotropic::Sigma(b.size(),sigma)) {
const Vector& b, const sharedDiagonal& model) :
b_(b), model_(model) {
As_.insert(make_pair(key1, A1));
As_.insert(make_pair(key2, A2));
As_.insert(make_pair(key3, A3));
@ -98,20 +82,12 @@ public:
/** Construct an n-ary factor */
GaussianFactor(const std::vector<std::pair<Symbol, Matrix> > &terms,
const Vector &b, double sigma) :
b_(b), model_(noiseModel::Isotropic::Sigma(b.size(),sigma)) {
const Vector &b, const sharedDiagonal& model) :
b_(b), model_(model) {
for(unsigned int i=0; i<terms.size(); i++)
As_.insert(terms[i]);
}
/** Construct an n-ary factor with multiple sigmas*/
GaussianFactor(const std::vector<std::pair<Symbol, Matrix> > &terms,
const Vector &b, const Vector& sigmas) :
b_(b), model_(noiseModel::Diagonal::Sigmas(sigmas)) {
for (unsigned int i = 0; i < terms.size(); i++)
As_.insert(terms[i]);
}
/** Construct from Conditional Gaussian */
GaussianFactor(const boost::shared_ptr<GaussianConditional>& cg);

3
cpp/GaussianFactorGraph.cpp
View File

@ -181,7 +181,8 @@ GaussianFactorGraph GaussianFactorGraph::add_priors(double sigma) const {
FOREACH_PAIR(key,dim,vs) {
Matrix A = eye(dim);
Vector b = zero(dim);
sharedFactor prior(new GaussianFactor(key,A,b, sigma));
sharedDiagonal model = noiseModel::Isotropic::Sigma(dim,sigma);
sharedFactor prior(new GaussianFactor(key,A,b, model));
result.push_back(prior);
}
return result;

16
cpp/GaussianFactorGraph.h
View File

@ -48,29 +48,29 @@ namespace gtsam {
/** Add a unary factor */
inline void add(const Symbol& key1, const Matrix& A1,
const Vector& b, double sigma) {
push_back(sharedFactor(new GaussianFactor(key1,A1,b,sigma)));
const Vector& b, const sharedDiagonal& model) {
push_back(sharedFactor(new GaussianFactor(key1,A1,b,model)));
}
/** Add a binary factor */
inline void add(const Symbol& key1, const Matrix& A1,
const Symbol& key2, const Matrix& A2,
const Vector& b, double sigma) {
push_back(sharedFactor(new GaussianFactor(key1,A1,key2,A2,b,sigma)));
const Vector& b, const sharedDiagonal& model) {
push_back(sharedFactor(new GaussianFactor(key1,A1,key2,A2,b,model)));
}
/** Add a ternary factor */
inline void add(const Symbol& key1, const Matrix& A1,
const Symbol& key2, const Matrix& A2,
const Symbol& key3, const Matrix& A3,
const Vector& b, double sigma) {
push_back(sharedFactor(new GaussianFactor(key1,A1,key2,A2,key3,A3,b,sigma)));
const Vector& b, const sharedDiagonal& model) {
push_back(sharedFactor(new GaussianFactor(key1,A1,key2,A2,key3,A3,b,model)));
}
/** Add an n-ary factor */
inline void add(const std::vector<std::pair<Symbol, Matrix> > &terms,
const Vector &b, double sigma) {
push_back(sharedFactor(new GaussianFactor(terms,b,sigma)));
const Vector &b, const sharedDiagonal& model) {
push_back(sharedFactor(new GaussianFactor(terms,b,model)));
}
/** return A*x-b */

3
cpp/Matrix.cpp
View File

@ -339,9 +339,12 @@ weighted_eliminate(Matrix& A, Vector& b, const Vector& sigmas) {
for (size_t j=0; j<n; ++j) {
// extract the first column of A
Vector a(column_(A, j)); // ublas::matrix_column is slower !
//print(a,"a");
// Calculate weighted pseudo-inverse and corresponding precision
double precision = weightedPseudoinverse(a, weights, pseudo);
// cout << precision << endl;
// print(pseudo,"pseudo");
// if precision is zero, no information on this column
if (precision < 1e-8) continue;

42
cpp/NoiseModel.cpp
View File

@ -24,6 +24,24 @@ using namespace std;
namespace gtsam {
namespace noiseModel {
/* ************************************************************************* */
// update A, b
// A' \define A_{S}-ar and b'\define b-ad
// Linear algebra: takes away projection on latest orthogonal
// Graph: make a new factor on the separator S
// __attribute__ ((noinline)) // uncomment to prevent inlining when profiling
static void updateAb(Matrix& Ab, int j, const Vector& a, const Vector& rd) {
size_t m = Ab.size1(), n = Ab.size2()-1;
for (int i = 0; i < m; i++) { // update all rows
double ai = a(i);
double *Aij = Ab.data().begin() + i * (n+1) + j + 1;
const double *rptr = rd.data().begin() + j + 1;
// Ab(i,j+1:end) -= ai*rd(j+1:end)
for (int j2 = j + 1; j2 < n+1; j2++, Aij++, rptr++)
*Aij -= ai * (*rptr);
}
}
/* ************************************************************************* */
Gaussian::shared_ptr Gaussian::Covariance(const Matrix& covariance, bool smart) {
size_t m = covariance.size1(), n = covariance.size2();
@ -85,8 +103,6 @@ namespace gtsam {
WhitenInPlace(Ab);
// Perform in-place Householder
// TODO: think about 1 on diagonal.
// Currently, GaussianConditional assumes unit upper
householder_(Ab, maxRank);
return Unit::Create(maxRank);
@ -149,28 +165,10 @@ namespace gtsam {
throw logic_error("noiseModel::Constrained cannot Whiten");
}
/* ************************************************************************* */
// update A, b
// A' \define A_{S}-ar and b'\define b-ad
// Linear algebra: takes away projection on latest orthogonal
// Graph: make a new factor on the separator S
// __attribute__ ((noinline)) // uncomment to prevent inlining when profiling
static void updateAb(Matrix& Ab, int j, const Vector& a, const Vector& rd) {
size_t m = Ab.size1(), n = Ab.size2()-1;
for (int i = 0; i < m; i++) { // update all rows
double ai = a(i);
double *Aij = Ab.data().begin() + i * (n+1) + j + 1;
const double *rptr = rd.data().begin() + j + 1;
// Ab(i,j+1:end) -= ai*rd(j+1:end)
for (int j2 = j + 1; j2 < n+1; j2++, Aij++, rptr++)
*Aij -= ai * (*rptr);
}
}
// 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
sharedDiagonal Constrained::QR(Matrix& Ab) const {
sharedDiagonal Diagonal::QR(Matrix& Ab) const {
// get size(A) and maxRank
size_t m = Ab.size1(), n = Ab.size2()-1;
size_t maxRank = min(m,n);
@ -233,7 +231,7 @@ namespace gtsam {
i+=1;
}
return Diagonal::Precisions(precisions);
return Constrained::MixedPrecisions(precisions);
}
/* ************************************************************************* */

60
cpp/NoiseModel.h
View File

@ -13,10 +13,11 @@
#include "Vector.h"
#include "Matrix.h"
namespace gtsam { namespace noiseModel {
namespace gtsam {
class Diagonal;
typedef boost::shared_ptr<Diagonal> sharedDiagonal;
class sharedDiagonal; // forward declare, defined at end
namespace noiseModel {
/**
* noiseModel::Base is the abstract base class for all noise models.
@ -35,6 +36,11 @@ namespace gtsam { namespace noiseModel {
Base(size_t dim):dim_(dim) {}
virtual ~Base() {}
/**
* Dimensionality
*/
inline size_t dim() const { return dim_;}
/**
* Whiten an error vector.
*/
@ -201,6 +207,11 @@ namespace gtsam { namespace noiseModel {
virtual Matrix Whiten(const Matrix& H) const;
virtual void WhitenInPlace(Matrix& H) const;
/**
* Apply QR factorization to the system [A b], taking into account constraints
*/
virtual sharedDiagonal QR(Matrix& Ab) const;
/**
* Return standard deviations (sqrt of diagonal)
*/
@ -235,15 +246,31 @@ namespace gtsam { namespace noiseModel {
* A diagonal noise model created by specifying a Vector of
* standard devations, some of which might be zero
*/
static shared_ptr Mixed(const Vector& sigmas) {
static shared_ptr MixedSigmas(const Vector& sigmas) {
return shared_ptr(new Constrained(sigmas));
}
/**
* A diagonal noise model created by specifying a Vector of
* standard devations, some of which might be zero
*/
static shared_ptr MixedVariances(const Vector& variances) {
return shared_ptr(new Constrained(esqrt(variances)));
}
/**
* A diagonal noise model created by specifying a Vector of
* precisions, some of which might be inf
*/
static shared_ptr MixedPrecisions(const Vector& precisions) {
return MixedVariances(reciprocal(precisions));
}
/**
* Fully constrained. TODO: subclass ?
*/
static shared_ptr All(size_t dim) {
return Mixed(repeat(dim,0));
return MixedSigmas(repeat(dim,0));
}
virtual void print(const std::string& name) const;
@ -255,11 +282,6 @@ namespace gtsam { namespace noiseModel {
virtual Matrix Whiten(const Matrix& H) const;
virtual void WhitenInPlace(Matrix& H) const;
/**
* Apply QR factorization to the system [A b], taking into account constraints
*/
virtual sharedDiagonal QR(Matrix& Ab) const;
}; // Constrained
/**
@ -342,6 +364,8 @@ namespace gtsam { namespace noiseModel {
using namespace noiseModel;
// TODO: very ugly, just keep shared pointers and use Sigma/Sigmas everywhere
// A useful convenience class to refer to a shared Gaussian model
// Define GTSAM_MAGIC_GAUSSIAN to desired dimension to have access to slightly
// dangerous and non-shared (inefficient, wasteful) noise models. Only in tests!
@ -359,5 +383,21 @@ namespace gtsam { namespace noiseModel {
#endif
};
// A useful convenience class to refer to a shared Diagonal model
// There are (somewhat dangerous) constructors from Vector and pair<size_t,double>
// that call Sigmas and Sigma, respectively.
struct sharedDiagonal : public Diagonal::shared_ptr {
sharedDiagonal() {}
sharedDiagonal(const Diagonal::shared_ptr& p):Diagonal::shared_ptr(p) {}
sharedDiagonal(const Constrained::shared_ptr& p):Diagonal::shared_ptr(p) {}
sharedDiagonal(const Isotropic::shared_ptr& p):Diagonal::shared_ptr(p) {}
sharedDiagonal(const Unit::shared_ptr& p):Diagonal::shared_ptr(p) {}
sharedDiagonal(const Vector& sigmas):Diagonal::shared_ptr(Diagonal::Sigmas(sigmas)) {}
};
// TODO: make these the ones really used in unit tests
inline sharedDiagonal sharedSigmas(const Vector& sigmas) { return Diagonal::Sigmas(sigmas);}
inline sharedDiagonal sharedSigma(int dim, double sigma) { return Isotropic::Sigma(dim,sigma);}
} // namespace gtsam

12
cpp/NonlinearConstraint-inl.h
View File

@ -122,11 +122,13 @@ NonlinearConstraint1<Config, Key, X>::linearize(const Config& config, const Vect
// construct probabilistic factor
Matrix A1 = vector_scale(lambda, grad);
sharedDiagonal probModel = sharedSigma(this->p_,1.0);
GaussianFactor::shared_ptr factor(new
GaussianFactor(key_, A1, this->lagrange_key_, eye(this->p_), zero(this->p_), 1.0));
GaussianFactor(key_, A1, this->lagrange_key_, eye(this->p_), zero(this->p_), probModel));
// construct the constraint
GaussianFactor::shared_ptr constraint(new GaussianFactor(key_, grad, -1*g, 0.0));
sharedDiagonal constraintModel = noiseModel::Constrained::All(this->p_);
GaussianFactor::shared_ptr constraint(new GaussianFactor(key_, grad, -1*g, constraintModel));
return std::make_pair(factor, constraint);
}
@ -220,12 +222,14 @@ std::pair<GaussianFactor::shared_ptr, GaussianFactor::shared_ptr> NonlinearConst
// construct probabilistic factor
Matrix A1 = vector_scale(lambda, grad1);
Matrix A2 = vector_scale(lambda, grad2);
sharedDiagonal probModel = sharedSigma(this->p_,1.0);
GaussianFactor::shared_ptr factor(new GaussianFactor(key1_, A1, key2_, A2,
this->lagrange_key_, eye(this->p_), zero(this->p_), 1.0));
this->lagrange_key_, eye(this->p_), zero(this->p_), probModel));
// construct the constraint
sharedDiagonal constraintModel = noiseModel::Constrained::All(this->p_);
GaussianFactor::shared_ptr constraint(new GaussianFactor(key1_, grad1,
key2_, grad2, -1.0 * g, 0.0));
key2_, grad2, -1.0 * g, constraintModel));
return std::make_pair(factor, constraint);
}

3
cpp/NonlinearEquality.h
View File

@ -83,7 +83,8 @@ namespace gtsam {
Matrix A;
Vector b = - evaluateError(xj, A);
// TODO pass unwhitened + noise model to Gaussian factor
return GaussianFactor::shared_ptr(new GaussianFactor(this->key_, A, b, 0.0));
sharedDiagonal model = noiseModel::Constrained::All(b.size());
return GaussianFactor::shared_ptr(new GaussianFactor(this->key_, A, b, model));
}
}; // NonlinearEquality

5
cpp/NonlinearFactor.h
View File

@ -183,7 +183,8 @@ namespace gtsam {
// TODO pass unwhitened + noise model to Gaussian factor
this->noiseModel_->WhitenInPlace(A);
this->noiseModel_->whitenInPlace(b);
return GaussianFactor::shared_ptr(new GaussianFactor(key_, A, b, 1.0));
return GaussianFactor::shared_ptr(new GaussianFactor(key_, A, b,
noiseModel::Unit::Create(b.size())));
}
/*
@ -282,7 +283,7 @@ namespace gtsam {
this->noiseModel_->WhitenInPlace(A2);
this->noiseModel_->whitenInPlace(b);
return GaussianFactor::shared_ptr(new GaussianFactor(key1_, A1, key2_,
A2, b, 1.0));
A2, b, noiseModel::Unit::Create(b.size())));
}
/** methods to retrieve both keys */

33
cpp/smallExample.cpp
View File

@ -30,6 +30,10 @@ namespace example {
typedef boost::shared_ptr<NonlinearFactor<Config> > shared;
static sharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1);
static sharedDiagonal sigma0_2 = noiseModel::Isotropic::Sigma(2,0.2);
static sharedDiagonal constraintModel = noiseModel::Constrained::All(2);
/* ************************************************************************* */
boost::shared_ptr<const Graph> sharedNonlinearFactorGraph() {
// Create
@ -126,24 +130,20 @@ namespace example {
GaussianFactorGraph fg;
// linearized prior on x1: c["x1"]+x1=0 i.e. x1=-c["x1"]
double sigma1 = 0.1;
Vector b1 = -Vector_(2,0.1, 0.1);
fg.add("x1", I, b1, sigma1);
fg.add("x1", I, b1, sigma0_1);
// odometry between x1 and x2: x2-x1=[0.2;-0.1]
double sigma2 = 0.1;
Vector b2 = Vector_(2, 0.2, -0.1);
fg.add("x1", -I, "x2", I, b2, sigma2);
fg.add("x1", -I, "x2", I, b2, sigma0_1);
// measurement between x1 and l1: l1-x1=[0.0;0.2]
double sigma3 = 0.2;
Vector b3 = Vector_(2, 0.0, 0.2);
fg.add("x1", -I, "l1", I, b3, sigma3);
fg.add("x1", -I, "l1", I, b3, sigma0_2);
// measurement between x2 and l1: l1-x2=[-0.2;0.3]
double sigma4 = 0.2;
Vector b4 = Vector_(2, -0.2, 0.3);
fg.add("x2", -I, "l1", I, b4, sigma4);
fg.add("x2", -I, "l1", I, b4, sigma0_2);
return fg;
}
@ -273,12 +273,11 @@ namespace example {
GaussianFactorGraph createSimpleConstraintGraph() {
// create unary factor
// prior on "x", mean = [1,-1], sigma=0.1
double sigma = 0.1;
Matrix Ax = eye(2);
Vector b1(2);
b1(0) = 1.0;
b1(1) = -1.0;
GaussianFactor::shared_ptr f1(new GaussianFactor("x", Ax, b1, sigma));
GaussianFactor::shared_ptr f1(new GaussianFactor("x", Ax, b1, sigma0_1));
// create binary constraint factor
// between "x" and "y", that is going to be the only factor on "y"
@ -288,7 +287,7 @@ namespace example {
Matrix Ay1 = eye(2) * -1;
Vector b2 = Vector_(2, 0.0, 0.0);
GaussianFactor::shared_ptr f2(new GaussianFactor("x", Ax1, "y", Ay1, b2,
0.0));
constraintModel));
// construct the graph
GaussianFactorGraph fg;
@ -311,12 +310,11 @@ namespace example {
GaussianFactorGraph createSingleConstraintGraph() {
// create unary factor
// prior on "x", mean = [1,-1], sigma=0.1
double sigma = 0.1;
Matrix Ax = eye(2);
Vector b1(2);
b1(0) = 1.0;
b1(1) = -1.0;
GaussianFactor::shared_ptr f1(new GaussianFactor("x", Ax, b1, sigma));
GaussianFactor::shared_ptr f1(new GaussianFactor("x", Ax, b1, sigma0_1));
// create binary constraint factor
// between "x" and "y", that is going to be the only factor on "y"
@ -330,7 +328,7 @@ namespace example {
Matrix Ay1 = eye(2) * 10;
Vector b2 = Vector_(2, 1.0, 2.0);
GaussianFactor::shared_ptr f2(new GaussianFactor("x", Ax1, "y", Ay1, b2,
0.0));
constraintModel));
// construct the graph
GaussianFactorGraph fg;
@ -351,10 +349,9 @@ namespace example {
/* ************************************************************************* */
GaussianFactorGraph createMultiConstraintGraph() {
// unary factor 1
double sigma = 0.1;
Matrix A = eye(2);
Vector b = Vector_(2, -2.0, 2.0);
GaussianFactor::shared_ptr lf1(new GaussianFactor("x", A, b, sigma));
GaussianFactor::shared_ptr lf1(new GaussianFactor("x", A, b, sigma0_1));
// constraint 1
Matrix A11(2, 2);
@ -373,7 +370,7 @@ namespace example {
b1(0) = 1.0;
b1(1) = 2.0;
GaussianFactor::shared_ptr lc1(new GaussianFactor("x", A11, "y", A12, b1,
0.0));
constraintModel));
// constraint 2
Matrix A21(2, 2);
@ -392,7 +389,7 @@ namespace example {
b2(0) = 3.0;
b2(1) = 4.0;
GaussianFactor::shared_ptr lc2(new GaussianFactor("x", A21, "z", A22, b2,
0.0));
constraintModel));
// construct the graph
GaussianFactorGraph fg;

122
cpp/testGaussianFactor.cpp
View File

@ -27,13 +27,16 @@ using namespace gtsam;
using namespace example;
using namespace boost;
static sharedDiagonal
sigma0_1 = sharedSigma(2,0.1), sigma_02 = sharedSigma(2,0.2),
constraintModel = noiseModel::Constrained::All(2);
/* ************************************************************************* */
TEST( GaussianFactor, linearFactor )
{
Matrix I = eye(2);
Vector b = Vector_(2,0.2,-0.1);
double sigma = 0.1;
GaussianFactor expected("x1", -I, "x2", I, b, sigma);
GaussianFactor expected("x1", -I, "x2", I, b, sigma0_1);
// create a small linear factor graph
GaussianFactorGraph fg = createGaussianFactorGraph();
@ -50,8 +53,7 @@ TEST( GaussianFactor, operators )
{
Matrix I = eye(2);
Vector b = Vector_(2,0.2,-0.1);
double sigma = 0.1;
GaussianFactor lf("x1", -I, "x2", I, b, sigma);
GaussianFactor lf("x1", -I, "x2", I, b, sigma0_1);
VectorConfig c;
c.insert("x1",Vector_(2,10.,20.));
@ -171,26 +173,26 @@ TEST( NonlinearFactorGraph, combine2){
A11(1,0) = 0; A11(1,1) = 1;
Vector b(2);
b(0) = 2; b(1) = -1;
GaussianFactor::shared_ptr f1(new GaussianFactor("x1", A11, b*sigma1, sigma1));
GaussianFactor::shared_ptr f1(new GaussianFactor("x1", A11, b*sigma1, sharedSigma(2,sigma1)));
double sigma2 = 0.5;
A11(0,0) = 1; A11(0,1) = 0;
A11(1,0) = 0; A11(1,1) = -1;
b(0) = 4 ; b(1) = -5;
GaussianFactor::shared_ptr f2(new GaussianFactor("x1", A11, b*sigma2, sigma2));
GaussianFactor::shared_ptr f2(new GaussianFactor("x1", A11, b*sigma2, sharedSigma(2,sigma2)));
double sigma3 = 0.25;
A11(0,0) = 1; A11(0,1) = 0;
A11(1,0) = 0; A11(1,1) = -1;
b(0) = 3 ; b(1) = -88;
GaussianFactor::shared_ptr f3(new GaussianFactor("x1", A11, b*sigma3, sigma3));
GaussianFactor::shared_ptr f3(new GaussianFactor("x1", A11, b*sigma3, sharedSigma(2,sigma3)));
// TODO: find a real sigma value for this example
double sigma4 = 0.1;
A11(0,0) = 6; A11(0,1) = 0;
A11(1,0) = 0; A11(1,1) = 7;
b(0) = 5 ; b(1) = -6;
GaussianFactor::shared_ptr f4(new GaussianFactor("x1", A11*sigma4, b*sigma4, sigma4));
GaussianFactor::shared_ptr f4(new GaussianFactor("x1", A11*sigma4, b*sigma4, sharedSigma(2,sigma4)));
vector<GaussianFactor::shared_ptr> lfg;
lfg.push_back(f1);
@ -223,14 +225,15 @@ TEST( NonlinearFactorGraph, combine2){
TEST( GaussianFactor, linearFactorN){
Matrix I = eye(2);
vector<GaussianFactor::shared_ptr> f;
sharedDiagonal model = sharedSigma(2,1.0);
f.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x1", I, Vector_(2,
10.0, 5.0), 1)));
10.0, 5.0), model)));
f.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x1", -10 * I,
"x2", 10 * I, Vector_(2, 1.0, -2.0), 1)));
"x2", 10 * I, Vector_(2, 1.0, -2.0), model)));
f.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x2", -10 * I,
"x3", 10 * I, Vector_(2, 1.5, -1.5), 1)));
"x3", 10 * I, Vector_(2, 1.5, -1.5), model)));
f.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x3", -10 * I,
"x4", 10 * I, Vector_(2, 2.0, -1.0), 1)));
"x4", 10 * I, Vector_(2, 2.0, -1.0), model)));
GaussianFactor combinedFactor(f);
@ -622,25 +625,19 @@ TEST( GaussianFactor, size )
/* ************************************************************************* */
TEST( GaussianFactor, CONSTRUCTOR_GaussianConditional )
{
Matrix R11 = Matrix_(2,2,
1.00, 0.00,
0.00, 1.00
);
Matrix R11 = eye(2);
Matrix S12 = Matrix_(2,2,
-0.200001, 0.00,
+0.00,-0.200001
);
Vector d(2); d(0) = 2.23607; d(1) = -1.56525;
Vector sigmas(2);
sigmas(0) = 0.29907;
sigmas(1) = 0.29907;
Vector sigmas =repeat(2,0.29907);
GaussianConditional::shared_ptr CG(new GaussianConditional("x2",d,R11,"l11",S12,sigmas));
GaussianFactor actualLF(CG);
// actualLF.print();
GaussianFactor expectedLF("x2",R11,"l11",S12,d, sigmas);
// Call the constructor we are testing !
GaussianFactor actualLF(CG);
GaussianFactor expectedLF("x2",R11,"l11",S12,d, sigmas);
CHECK(assert_equal(expectedLF,actualLF,1e-5));
}
@ -659,8 +656,7 @@ TEST( GaussianFactor, alphaFactor )
// calculate expected
Matrix A = Matrix_(2,1,30.0,5.0);
Vector b = Vector_(2,-0.1,-0.1);
Vector sigmas = Vector_(2,0.1,0.1);
GaussianFactor expected(alphaKey,A,b,sigmas);
GaussianFactor expected(alphaKey,A,b,sigma0_1);
CHECK(assert_equal(expected,*actual));
}
@ -671,7 +667,7 @@ TEST ( GaussianFactor, constraint_eliminate1 )
// construct a linear constraint
Vector v(2); v(0)=1.2; v(1)=3.4;
string key = "x0";
GaussianFactor lc(key, eye(2), v, 0.0);
GaussianFactor lc(key, eye(2), v, constraintModel);
// eliminate it
GaussianConditional::shared_ptr actualCG;
@ -704,7 +700,7 @@ TEST ( GaussianFactor, constraint_eliminate2 )
A2(0,0) = 1.0 ; A2(0,1) = 2.0;
A2(1,0) = 2.0 ; A2(1,1) = 4.0;
GaussianFactor lc("x", A1, "y", A2, b, 0.0);
GaussianFactor lc("x", A1, "y", A2, b, constraintModel);
// eliminate x and verify results
GaussianConditional::shared_ptr actualCG;
@ -727,41 +723,41 @@ TEST ( GaussianFactor, constraint_eliminate2 )
CHECK(assert_equal(expectedCG, *actualCG, 1e-4));
}
/* ************************************************************************* *
TEST ( GaussianFactor, constraint_eliminate3 )
{
// This test shows that ordering matters if there are non-invertible
// blocks, as this example can be eliminated if x is first, but not
// if y is first.
// Construct a linear constraint
// RHS
Vector b(2); b(0)=3.0; b(1)=4.0;
// A1 - invertible
Matrix A1(2,2);
A1(0,0) = 1.0 ; A1(0,1) = 2.0;
A1(1,0) = 2.0 ; A1(1,1) = 1.0;
// A2 - not invertible
Matrix A2(2,2);
A2(0,0) = 1.0 ; A2(0,1) = 2.0;
A2(1,0) = 2.0 ; A2(1,1) = 4.0;
GaussianFactor lc("x", A1, "y", A2, b, 0.0);
// eliminate y from original graph
// NOTE: this will throw an exception, as
// the leading matrix is rank deficient
GaussianConditional::shared_ptr actualCG;
GaussianFactor::shared_ptr actualLF;
try {
boost::tie(actualCG, actualLF) = lc.eliminate("y");
CHECK(false);
} catch (domain_error) {
CHECK(true);
}
}
///* ************************************************************************* *
//TEST ( GaussianFactor, constraint_eliminate3 )
//{
// // This test shows that ordering matters if there are non-invertible
// // blocks, as this example can be eliminated if x is first, but not
// // if y is first.
//
// // Construct a linear constraint
// // RHS
// Vector b(2); b(0)=3.0; b(1)=4.0;
//
// // A1 - invertible
// Matrix A1(2,2);
// A1(0,0) = 1.0 ; A1(0,1) = 2.0;
// A1(1,0) = 2.0 ; A1(1,1) = 1.0;
//
// // A2 - not invertible
// Matrix A2(2,2);
// A2(0,0) = 1.0 ; A2(0,1) = 2.0;
// A2(1,0) = 2.0 ; A2(1,1) = 4.0;
//
// GaussianFactor lc("x", A1, "y", A2, b, 0.0);
//
// // eliminate y from original graph
// // NOTE: this will throw an exception, as
// // the leading matrix is rank deficient
// GaussianConditional::shared_ptr actualCG;
// GaussianFactor::shared_ptr actualLF;
// try {
// boost::tie(actualCG, actualLF) = lc.eliminate("y");
// CHECK(false);
// } catch (domain_error) {
// CHECK(true);
// }
//}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */

29
cpp/testGaussianFactorGraph.cpp
View File

@ -263,7 +263,7 @@ TEST( GaussianFactorGraph, add_priors )
GaussianFactorGraph expected = createGaussianFactorGraph();
Matrix A = eye(2);
Vector b = zero(2);
double sigma = 3.0;
sharedDiagonal sigma = sharedSigma(2,3.0);
expected.push_back(GaussianFactor::shared_ptr(new GaussianFactor("l1",A,b,sigma)));
expected.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x1",A,b,sigma)));
expected.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x2",A,b,sigma)));
@ -629,7 +629,7 @@ TEST( GaussianFactorGraph, elimination )
GaussianFactorGraph fg;
Matrix Ap = eye(1), An = eye(1) * -1;
Vector b = Vector_(1, 0.0);
double sigma = 1.0/0.5;
sharedDiagonal sigma = sharedSigma(2,2.0);
fg.add("x1", An, "x2", Ap, b, sigma);
fg.add("x1", Ap, b, sigma);
fg.add("x2", Ap, b, sigma);
@ -734,17 +734,20 @@ TEST( GaussianFactorGraph, constrained_multi2 )
}
/* ************************************************************************* */
sharedDiagonal model = sharedSigma(2,1);
TEST( GaussianFactorGraph, findMinimumSpanningTree )
{
GaussianFactorGraph g;
Matrix I = eye(2);
Vector b = Vector_(0, 0, 0);
g.add("x1", I, "x2", I, b, 0);
g.add("x1", I, "x3", I, b, 0);
g.add("x1", I, "x4", I, b, 0);
g.add("x2", I, "x3", I, b, 0);
g.add("x2", I, "x4", I, b, 0);
g.add("x3", I, "x4", I, b, 0);
g.add("x1", I, "x2", I, b, model);
g.add("x1", I, "x3", I, b, model);
g.add("x1", I, "x4", I, b, model);
g.add("x2", I, "x3", I, b, model);
g.add("x2", I, "x4", I, b, model);
g.add("x3", I, "x4", I, b, model);
map<string, string> tree = g.findMinimumSpanningTree<string, GaussianFactor>();
CHECK(tree["x1"].compare("x1")==0);
@ -759,11 +762,11 @@ TEST( GaussianFactorGraph, split )
GaussianFactorGraph g;
Matrix I = eye(2);
Vector b = Vector_(0, 0, 0);
g.add("x1", I, "x2", I, b, 0);
g.add("x1", I, "x3", I, b, 0);
g.add("x1", I, "x4", I, b, 0);
g.add("x2", I, "x3", I, b, 0);
g.add("x2", I, "x4", I, b, 0);
g.add("x1", I, "x2", I, b, model);
g.add("x1", I, "x3", I, b, model);
g.add("x1", I, "x4", I, b, model);
g.add("x2", I, "x3", I, b, model);
g.add("x2", I, "x4", I, b, model);
PredecessorMap<string> tree;
tree["x1"] = "x1";

85
cpp/testNoiseModel.cpp
View File

@ -113,7 +113,7 @@ 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::Mixed(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)));
CHECK(assert_equal(Vector_(3, 0.5, 0.0, 0.5),d->whiten(feasible)));
DOUBLES_EQUAL(inf,d->Mahalanobis(infeasible),1e-9);
@ -133,44 +133,59 @@ TEST(NoiseModel, ConstrainedAll )
DOUBLES_EQUAL(0.0,i->Mahalanobis(feasible),1e-9);
}
// Currently does not pass
///* ************************************************************************* */
//TEST( NoiseModel, QR )
//{
// // create a matrix to eliminate
// Matrix Ab1 = Matrix_(4, 6+1,
// -1., 0., 1., 0., 0., 0., -0.2,
// 0., -1., 0., 1., 0., 0., 0.3,
// 1., 0., 0., 0., -1., 0., 0.2,
// 0., 1., 0., 0., 0., -1., -0.1);
// Matrix Ab2 = Ab1; // otherwise overwritten !
// Vector sigmas = Vector_(4, 0.2, 0.2, 0.1, 0.1);
//
// // Expected result
// Vector expectedSigmas = Vector_(4, 0.0894427, 0.0894427, 0.223607, 0.223607);
// sharedDiagonal expectedModel = noiseModel::Diagonal::Sigmas(expectedSigmas);
//
// // Call Gaussian version
// sharedDiagonal diagonal = noiseModel::Diagonal::Sigmas(sigmas);
// sharedDiagonal actual1 = diagonal->QR(Ab1);
// sharedDiagonal expected = noiseModel::Unit::Create(4);
// CHECK(assert_equal(*expected,*actual1));
// Matrix expectedRd1 = Matrix_(4, 6+1,
// 11.1803, 0.0, -2.23607, 0.0, -8.94427, 0.0, 2.23607,
// 0.0, 11.1803, 0.0, -2.23607, 0.0, -8.94427,-1.56525,
// -0.618034, 0.0, 4.47214, 0.0, -4.47214, 0.0, 0.0,
// 0.0, -0.618034, 0.0, 4.47214, 0.0, -4.47214, 0.894427);
// CHECK(assert_equal(expectedRd1,Ab1,1e-4)); // Ab was modified in place !!!
//
// // Call Constrained version
// sharedDiagonal constrained = noiseModel::Constrained::MixedSigmas(sigmas);
// sharedDiagonal actual2 = constrained->QR(Ab2);
// CHECK(assert_equal(*expectedModel,*actual2));
// 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(assert_equal(expectedRd2,Ab2,1e-6)); // Ab was modified in place !!!
//}
/* ************************************************************************* */
TEST( NoiseModel, QR )
TEST(NoiseModel, QRNan )
{
// create a matrix to eliminate
Matrix Ab1 = Matrix_(4, 6+1,
-1., 0., 1., 0., 0., 0., -0.2,
0., -1., 0., 1., 0., 0., 0.3,
1., 0., 0., 0., -1., 0., 0.2,
0., 1., 0., 0., 0., -1., -0.1);
Matrix Ab2 = Ab1; // otherwise overwritten !
Vector sigmas = Vector_(4, 0.2, 0.2, 0.1, 0.1);
sharedDiagonal constrained = noiseModel::Constrained::All(2);
Matrix Ab = Matrix_(2, 5, 1., 2., 1., 2., 3., 2., 1., 2., 4., 4.);
// Expected result
Vector expectedSigmas = Vector_(4, 0.0894427, 0.0894427, 0.223607, 0.223607);
sharedDiagonal expectedModel = noiseModel::Diagonal::Sigmas(expectedSigmas);
sharedDiagonal expected = noiseModel::Constrained::All(2);
Matrix expectedAb = Matrix_(2, 5, 1., 2., 1., 2., 3., 0., 1., 0., 0., 2.0/3);
// Call Gaussian version
sharedDiagonal diagonal = noiseModel::Diagonal::Sigmas(sigmas);
sharedDiagonal actual1 = diagonal->QR(Ab1);
sharedDiagonal expected = noiseModel::Unit::Create(4);
CHECK(assert_equal(*expected,*actual1));
Matrix expectedRd1 = Matrix_(4, 6+1,
11.1803, 0.0, -2.23607, 0.0, -8.94427, 0.0, 2.23607,
0.0, 11.1803, 0.0, -2.23607, 0.0, -8.94427,-1.56525,
-0.618034, 0.0, 4.47214, 0.0, -4.47214, 0.0, 0.0,
0.0, -0.618034, 0.0, 4.47214, 0.0, -4.47214, 0.894427);
CHECK(assert_equal(expectedRd1,Ab1,1e-4)); // Ab was modified in place !!!
// Call Constrained version
sharedDiagonal constrained = noiseModel::Constrained::Mixed(sigmas);
sharedDiagonal actual2 = constrained->QR(Ab2);
CHECK(assert_equal(*expectedModel,*actual2));
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(assert_equal(expectedRd2,Ab2,1e-6)); // Ab was modified in place !!!
sharedDiagonal actual = constrained->QR(Ab);
CHECK(assert_equal(*expected,*actual));
CHECK(assert_equal(expectedAb,Ab));
}
/* ************************************************************************* */

18
cpp/testNonlinearConstraint.cpp
View File

@ -87,8 +87,10 @@ TEST( NonlinearConstraint1, unary_scalar_linearize ) {
boost::tie(actualFactor, actualConstraint) = c1.linearize(realconfig, lagrangeConfig);
// verify
GaussianFactor expectedFactor(x1, Matrix_(1,1, 6.0), "L1", eye(1), zero(1), 1.0);
GaussianFactor expectedConstraint(x1, Matrix_(1,1, 2.0), Vector_(1, 4.0), 0.0);
sharedDiagonal probModel = sharedSigma(p,1.0);
GaussianFactor expectedFactor(x1, Matrix_(1,1, 6.0), "L1", eye(1), zero(1), probModel);
sharedDiagonal constraintModel = noiseModel::Constrained::All(p);
GaussianFactor expectedConstraint(x1, Matrix_(1,1, 2.0), Vector_(1, 4.0), constraintModel);
CHECK(assert_equal(*actualFactor, expectedFactor));
CHECK(assert_equal(*actualConstraint, expectedConstraint));
}
@ -186,12 +188,14 @@ TEST( NonlinearConstraint2, binary_scalar_linearize ) {
boost::tie(actualFactor, actualConstraint) = c1.linearize(realconfig, lagrangeConfig);
// verify
sharedDiagonal probModel = sharedSigma(p,1.0);
GaussianFactor expectedFactor(x0, Matrix_(1,1, 6.0),
x1, Matrix_(1,1, -3.0),
"L12", eye(1), zero(1), 1.0);
"L12", eye(1), zero(1), probModel);
sharedDiagonal constraintModel = noiseModel::Constrained::All(p);
GaussianFactor expectedConstraint(x0, Matrix_(1,1, 2.0),
x1, Matrix_(1,1, -1.0),
Vector_(1, 6.0), 0.0);
Vector_(1, 6.0), constraintModel);
CHECK(assert_equal(*actualFactor, expectedFactor));
CHECK(assert_equal(*actualConstraint, expectedConstraint)); //FAILS - wrong b value
}
@ -285,8 +289,10 @@ TEST( NonlinearConstraint1, unary_inequality_linearize ) {
CHECK(c1.active(config2));
// verify
GaussianFactor expectedFactor(x0, Matrix_(1,1, 6.0), "L1", eye(1), zero(1), 1.0);
GaussianFactor expectedConstraint(x0, Matrix_(1,1, 2.0), Vector_(1, 4.0), 0.0);
sharedDiagonal probModel = sharedSigma(p,1.0);
GaussianFactor expectedFactor(x0, Matrix_(1,1, 6.0), "L1", eye(1), zero(1), probModel);
sharedDiagonal constraintModel = noiseModel::Constrained::All(p);
GaussianFactor expectedConstraint(x0, Matrix_(1,1, 2.0), Vector_(1, 4.0), constraintModel);
CHECK(assert_equal(*actualFactor2, expectedFactor));
CHECK(assert_equal(*actualConstraint2, expectedConstraint));
}

3
cpp/testNonlinearEquality.cpp
View File

@ -31,7 +31,8 @@ TEST ( NonlinearEquality, linearization ) {
shared_nle nle(new NLE(key, value,vector_compare));
// check linearize
GaussianFactor expLF(key, eye(2), zero(2), 0.0);
sharedDiagonal constraintModel = noiseModel::Constrained::All(2);
GaussianFactor expLF(key, eye(2), zero(2), constraintModel);
GaussianFactor::shared_ptr actualLF = nle->linearize(linearize);
CHECK(assert_equal(*actualLF, expLF));
}

3
cpp/testPose2Factor.cpp
View File

@ -114,7 +114,8 @@ TEST( Pose2Factor, linearize )
Vector expected_b = Vector_(3, 0.0, 0.0, 0.0);
// expected linear factor
GaussianFactor expected("x1", expectedH1, "x2", expectedH2, expected_b, 1.0);
sharedDiagonal probModel1 = noiseModel::Unit::Create(3);
GaussianFactor expected("x1", expectedH1, "x2", expectedH2, expected_b, probModel1);
// Actual linearization
boost::shared_ptr<GaussianFactor> actual = factor.linearize(x0);

5
cpp/testPose2SLAM.cpp
View File

@ -47,7 +47,7 @@ TEST( Pose2Graph, constructor )
}
/* ************************************************************************* */
TEST( Pose2Graph, linerization )
TEST( Pose2Graph, linearization )
{
// create a factor between unknown poses p1 and p2
Pose2 measured(2,2,M_PI_2);
@ -79,7 +79,8 @@ TEST( Pose2Graph, linerization )
double sigma = 1;
Vector b = Vector_(3,-0.1/sx,0.1/sy,0.0);
lfg_expected.add("x1", A1, "x2", A2, b, sigma);
sharedDiagonal probModel1 = noiseModel::Unit::Create(3);
lfg_expected.add("x1", A1, "x2", A2, b, probModel1);
CHECK(assert_equal(lfg_expected, lfg_linearized));
}

21
cpp/testSQP.cpp
View File

@ -35,6 +35,11 @@ using namespace gtsam;
using namespace boost;
using namespace boost::assign;
// Models to use
sharedDiagonal probModel1 = sharedSigma(1,1.0);
sharedDiagonal probModel2 = sharedSigma(2,1.0);
sharedDiagonal constraintModel1 = noiseModel::Constrained::All(1);
// trick from some reading group
#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
@ -102,12 +107,12 @@ TEST (SQP, problem1_cholesky ) {
// create a factor for the states
GaussianFactor::shared_ptr f1(new
GaussianFactor("x", H1, "y", H2, "L", gradG, gradL, 1.0));
GaussianFactor("x", H1, "y", H2, "L", gradG, gradL, probModel2));
// create a factor for the lagrange multiplier
GaussianFactor::shared_ptr f2(new
GaussianFactor("x", -sub(gradG, 0, 1, 0, 1),
"y", -sub(gradG, 1, 2, 0, 1), -gx, 0.0));
"y", -sub(gradG, 1, 2, 0, 1), -gx, constraintModel1));
// construct graph
GaussianFactorGraph fg;
@ -184,7 +189,7 @@ TEST (SQP, problem1_sqp ) {
new GaussianFactor("x", sub(A, 0,2, 0,1), // A(:,1)
"y", sub(A, 0,2, 1,2), // A(:,2)
b, // rhs of f(x)
1.0)); // arbitrary sigma
probModel2)); // arbitrary sigma
/** create the constraint-linear factor
* Provides a mechanism to use variable gain to force the constraint
@ -195,10 +200,10 @@ TEST (SQP, problem1_sqp ) {
Matrix gradG = Matrix_(1, 2,2*x, -1.0);
GaussianFactor::shared_ptr f2(
new GaussianFactor("x", lambda*sub(gradG, 0,1, 0,1), // scaled gradG(:,1)
"y", lambda*sub(gradG, 0,1, 1,2), // scaled gradG(:,2)
"L", eye(1), // dlambda term
Vector_(1, 0.0), // rhs is zero
1.0)); // arbitrary sigma
"y", lambda*sub(gradG, 0,1, 1,2), // scaled gradG(:,2)
"L", eye(1), // dlambda term
Vector_(1, 0.0), // rhs is zero
probModel1)); // arbitrary sigma
// create the actual constraint
// [gradG] [x; y] - g = 0
@ -207,7 +212,7 @@ TEST (SQP, problem1_sqp ) {
new GaussianFactor("x", sub(gradG, 0,1, 0,1), // slice first part of gradG
"y", sub(gradG, 0,1, 1,2), // slice second part of gradG
g, // value of constraint function
0.0)); // force to constraint
constraintModel1)); // force to constraint
// construct graph
GaussianFactorGraph fg;

3
cpp/testVSLAMFactor.cpp
View File

@ -52,7 +52,8 @@ TEST( ProjectionFactor, error )
Matrix Al1 = Matrix_(2, 3, 61.584, 0., 0., 0., 61.584, 0.);
Matrix Ax1 = Matrix_(2, 6, 0., -369.504, 0., -61.584, 0., 0., 369.504, 0., 0., 0., -61.584, 0.);
Vector b = Vector_(2,3.,0.);
GaussianFactor expected("l1", Al1, "x1", Ax1, b, 1);
sharedDiagonal probModel1 = noiseModel::Unit::Create(2);
GaussianFactor expected("l1", Al1, "x1", Ax1, b, probModel1);
GaussianFactor::shared_ptr actual = factor->linearize(config);
CHECK(assert_equal(expected,*actual,1e-3));

2
cpp/timeGaussianFactor.cpp
View File

@ -87,7 +87,7 @@ int main()
b2(7) = -1;
// time eliminate
GaussianFactor combined("x2", Ax2, "l1", Al1, "x1", Ax1, b2,1);
GaussianFactor combined("x2", Ax2, "l1", Al1, "x1", Ax1, b2,sharedSigma(8,1));
long timeLog = clock();
int n = 1000000;
GaussianConditional::shared_ptr conditional;

7
cpp/timeGaussianFactorGraph.cpp
View File

@ -74,9 +74,12 @@ TEST(timeGaussianFactorGraph, planar)
// (N = 100): 16.36
// Improved (int->size_t)
// (N = 100): 15.39
// Switch to 100*100 grid = 10K poses
// 1879: 15.6498 15.3851 15.5279
int N = 100;
double time = timePlanarSmoother(N); cout << "timeGaussianFactorGraph: " << time << endl;
// DOUBLES_EQUAL(5.97,time,0.1);
double time = timePlanarSmoother(N); cout << "timeGaussianFactorGraph : " << time << endl;
//DOUBLES_EQUAL(5.97,time,0.1);
}
/* ************************************************************************* */