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

Integrated householder-based QR into NoiseModel. Note that the examples for GFG have changed to ensure that they are actually a linearized version of the nonlinear example.

release/4.3a0
Alex Cunningham 2010-01-27 04:39:35 +00:00
parent 98b825ddbd
commit 59c7ce7e29
23 changed files with 999 additions and 975 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
cpp/GaussianBayesNet.cpp
View File

@ -29,7 +29,7 @@ namespace gtsam {
GaussianBayesNet scalarGaussian(const Symbol& key, double mu, double sigma) { GaussianBayesNet scalarGaussian(const Symbol& key, double mu, double sigma) {
GaussianBayesNet bn; GaussianBayesNet bn;
GaussianConditional::shared_ptr GaussianConditional::shared_ptr
conditional(new GaussianConditional(key, Vector_(1,mu), eye(1), Vector_(1,sigma))); conditional(new GaussianConditional(key, Vector_(1,mu)/sigma, eye(1)/sigma, ones(1)));
bn.push_back(conditional); bn.push_back(conditional);
return bn; return bn;
} }
@ -39,7 +39,7 @@ GaussianBayesNet simpleGaussian(const Symbol& key, const Vector& mu, double sigm
GaussianBayesNet bn; GaussianBayesNet bn;
size_t n = mu.size(); size_t n = mu.size();
GaussianConditional::shared_ptr GaussianConditional::shared_ptr
conditional(new GaussianConditional(key, mu, eye(n), repeat(n,sigma))); conditional(new GaussianConditional(key, mu/sigma, eye(n)/sigma, ones(n)));
bn.push_back(conditional); bn.push_back(conditional);
return bn; return bn;
} }

115
cpp/GaussianFactor.cpp
View File

@ -57,6 +57,7 @@ GaussianFactor::GaussianFactor(const vector<shared_ptr> & factors)
size_t pos = 0; // save last position inserted into the new rhs vector size_t pos = 0; // save last position inserted into the new rhs vector
// iterate over all factors // iterate over all factors
bool constrained = false;
BOOST_FOREACH(shared_ptr factor, factors){ BOOST_FOREACH(shared_ptr factor, factors){
if (verbose) factor->print(); if (verbose) factor->print();
// number of rows for factor f // number of rows for factor f
@ -72,10 +73,25 @@ GaussianFactor::GaussianFactor(const vector<shared_ptr> & factors)
// update the matrices // update the matrices
append_factor(factor,m,pos); append_factor(factor,m,pos);
// check if there are constraints
if (verbose) factor->model_->print("Checking for zeros");
if (!constrained && factor->model_->isConstrained()) {
constrained = true;
if (verbose) cout << "Found a constraint!" << endl;
}
pos += mf; pos += mf;
} }
if (verbose) cout << "GaussianFactor::GaussianFactor done" << endl;
model_ = noiseModel::Diagonal::Sigmas(sigmas); if (verbose) cout << "GaussianFactor::GaussianFactor done" << endl;
if (constrained) {
model_ = noiseModel::Constrained::MixedSigmas(sigmas);
if (verbose) model_->print("Just created Constraint ^");
} else {
model_ = noiseModel::Diagonal::Sigmas(sigmas);
if (verbose) model_->print("Just created Diagonal");
}
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -304,6 +320,7 @@ void GaussianFactor::append_factor(GaussianFactor::shared_ptr f, size_t m, size_
pair<GaussianConditional::shared_ptr, GaussianFactor::shared_ptr> pair<GaussianConditional::shared_ptr, GaussianFactor::shared_ptr>
GaussianFactor::eliminate(const Symbol& key) const GaussianFactor::eliminate(const Symbol& key) const
{ {
bool verbose = false;
// if this factor does not involve key, we exit with empty CG and LF // if this factor does not involve key, we exit with empty CG and LF
const_iterator it = As_.find(key); const_iterator it = As_.find(key);
if (it==As_.end()) { if (it==As_.end()) {
@ -323,7 +340,9 @@ GaussianFactor::eliminate(const Symbol& key) const
Matrix Ab = matrix_augmented(ordering,false); Matrix Ab = matrix_augmented(ordering,false);
// Use in-place QR on dense Ab appropriate to NoiseModel // Use in-place QR on dense Ab appropriate to NoiseModel
if (verbose) model_->print("Before QR");
SharedDiagonal noiseModel = model_->QR(Ab); SharedDiagonal noiseModel = model_->QR(Ab);
if (verbose) model_->print("After QR");
// get dimensions of the eliminated variable // get dimensions of the eliminated variable
// TODO: this is another map find that should be avoided ! // TODO: this is another map find that should be avoided !
@ -355,96 +374,16 @@ GaussianFactor::eliminate(const Symbol& key) const
if (cur_key!=key) { if (cur_key!=key) {
size_t dim = getDim(cur_key); // TODO avoid find ! size_t dim = getDim(cur_key); // TODO avoid find !
conditional->add(cur_key, sub(Ab, 0, n1, j, j+dim)); conditional->add(cur_key, sub(Ab, 0, n1, j, j+dim));
factor->insert(cur_key, sub(Ab, n1, maxRank, j, j+dim)); factor->insert(cur_key, sub(Ab, n1, maxRank, j, j+dim)); // TODO: handle zeros properly
j+=dim; j+=dim;
} }
// Set sigmas // Set sigmas
factor->model_ = noiseModel::Diagonal::Sigmas(sub(noiseModel->sigmas(),n1,maxRank)); // set the right model here
if (noiseModel->isConstrained())
// extract ds vector for the new b factor->model_ = noiseModel::Constrained::MixedSigmas(sub(noiseModel->sigmas(),n1,maxRank));
factor->set_b(sub(d, n1, maxRank)); else
factor->model_ = noiseModel::Diagonal::Sigmas(sub(noiseModel->sigmas(),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
{
// 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)
// TODO: get Ab as augmented matrix
// Matrix Ab = matrix_augmented(ordering,false);
Matrix A; Vector b;
boost::tie(A, b) = matrix(ordering, false);
size_t n = A.size2();
// Do in-place QR to get R, d of the augmented system
std::list<boost::tuple<Vector, double, double> > solution =
weighted_eliminate(A, b, model_->sigmas());
// get dimensions of the eliminated variable
// TODO: this is another map find that should be avoided !
size_t n1 = getDim(key);
// if m<n1, this factor cannot be eliminated
size_t maxRank = solution.size();
if (maxRank<n1)
throw(domain_error("GaussianFactor::eliminate: fewer constraints than unknowns"));
// unpack the solutions
Matrix R(maxRank, n);
Vector r, d(maxRank), newSigmas(maxRank); double di, sigma;
Matrix::iterator2 Rit = R.begin2();
size_t i = 0;
BOOST_FOREACH(boost::tie(r, di, sigma), solution) {
copy(r.begin(), r.end(), Rit); // copy r vector
d(i) = di; // copy in rhs
newSigmas(i) = sigma; // copy in new sigmas
Rit += n; i += 1;
}
// create base conditional Gaussian
GaussianConditional::shared_ptr conditional(new GaussianConditional(key,
sub(d, 0, n1), // form d vector
sub(R, 0, n1, 0, n1), // form R matrix
sub(newSigmas, 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);
conditional->add(cur_key, sub(R, 0, n1, j, j+dim));
factor->insert(cur_key, sub(R, n1, maxRank, j, j+dim));
j+=dim;
}
// Set sigmas
factor->model_ = noiseModel::Diagonal::Sigmas(sub(newSigmas,n1,maxRank));
// extract ds vector for the new b // extract ds vector for the new b
factor->set_b(sub(d, n1, maxRank)); factor->set_b(sub(d, n1, maxRank));

5
cpp/GaussianFactorGraph.cpp
View File

@ -108,6 +108,11 @@ GaussianFactorGraph::eliminate(const Ordering& ordering)
/* ************************************************************************* */ /* ************************************************************************* */
VectorConfig GaussianFactorGraph::optimize(const Ordering& ordering) VectorConfig GaussianFactorGraph::optimize(const Ordering& ordering)
{ {
bool verbose = false;
if (verbose)
BOOST_FOREACH(sharedFactor factor,factors_)
factor->get_model()->print("Starting model");
// eliminate all nodes in the given ordering -> chordal Bayes net // eliminate all nodes in the given ordering -> chordal Bayes net
GaussianBayesNet chordalBayesNet = eliminate(ordering); GaussianBayesNet chordalBayesNet = eliminate(ordering);

8
cpp/Makefile.am
View File

@ -274,7 +274,13 @@ AM_LDFLAGS = -L../CppUnitLite -lCppUnitLite $(BOOST_LDFLAGS) $(boost_serializati
# TO ENABLE GSL AND ATLAS, uncomment this part! # TO ENABLE GSL AND ATLAS, uncomment this part!
# AM_LDFLAGS += -lgsl -lcblas -latlas # AM_LDFLAGS += -lgsl -lcblas -latlas
# libgtsam_la_LDFLAGS += -lgsl -lcblas -latlas # libgtsam_la_LDFLAGS += -lgsl -lcblas -latlas # Note: order of libraries is critical
# AM_CXXFLAGS += -DGSL
# libgtsam_la_CPPFLAGS += -DGSL
# TO ENABLE JUST GSL, uncomment this part!
# AM_LDFLAGS += -lgsl -lgslcblas
# libgtsam_la_LDFLAGS += -lgsl -lgslcblas
# AM_CXXFLAGS += -DGSL # AM_CXXFLAGS += -DGSL
# libgtsam_la_CPPFLAGS += -DGSL # libgtsam_la_CPPFLAGS += -DGSL

592
cpp/NoiseModel.cpp
View File

@ -33,299 +33,319 @@ static double inf = std::numeric_limits<double>::infinity();
using namespace std; using namespace std;
namespace gtsam { namespace gtsam {
namespace noiseModel { namespace noiseModel {
/* ************************************************************************* */ /* ************************************************************************* */
// update A, b // update A, b
// A' \define A_{S}-ar and b'\define b-ad // A' \define A_{S}-ar and b'\define b-ad
// Linear algebra: takes away projection on latest orthogonal // Linear algebra: takes away projection on latest orthogonal
// Graph: make a new factor on the separator S // Graph: make a new factor on the separator S
// __attribute__ ((noinline)) // uncomment to prevent inlining when profiling // __attribute__ ((noinline)) // uncomment to prevent inlining when profiling
static void updateAb(Matrix& Ab, int j, const Vector& a, const Vector& rd) { static void updateAb(Matrix& Ab, int j, const Vector& a, const Vector& rd) {
size_t m = Ab.size1(), n = Ab.size2()-1; size_t m = Ab.size1(), n = Ab.size2()-1;
#ifdef GSL #ifdef GSL
// Ab(0:m,j+1:n) = Ab(0:m,j+1:n) - a(0:m)*rd(j+1:end)' // Ab(0:m,j+1:n) = Ab(0:m,j+1:n) - a(0:m)*rd(j+1:end)'
// get a view for Ab // get a view for Ab
gsl_matrix_view Abg = gsl_matrix_view_array(Ab.data().begin(), m, n+1); gsl_matrix_view Abg = gsl_matrix_view_array(Ab.data().begin(), m, n+1);
gsl_matrix_view Abg_view = gsl_matrix_submatrix (&(Abg.matrix), 0, j+1, m, n-j); gsl_matrix_view Abg_view = gsl_matrix_submatrix (&(Abg.matrix), 0, j+1, m, n-j);
// get a view for a // get a view for a
gsl_vector_const_view ag = gsl_vector_const_view_array(a.data().begin(), m); gsl_vector_const_view ag = gsl_vector_const_view_array(a.data().begin(), m);
// get a view for r // get a view for r
gsl_vector_const_view rdg = gsl_vector_const_view_array(rd.data().begin()+j+1, n-j); gsl_vector_const_view rdg = gsl_vector_const_view_array(rd.data().begin()+j+1, n-j);
// rank one update // rank one update
gsl_blas_dger (-1.0, &(ag.vector), &(rdg.vector), &(Abg_view.matrix)); gsl_blas_dger (-1.0, &(ag.vector), &(rdg.vector), &(Abg_view.matrix));
#else #else
for (int i = 0; i < m; i++) { // update all rows for (int i = 0; i < m; i++) { // update all rows
double ai = a(i); double ai = a(i);
double *Aij = Ab.data().begin() + i * (n+1) + j + 1; double *Aij = Ab.data().begin() + i * (n+1) + j + 1;
const double *rptr = rd.data().begin() + j + 1; const double *rptr = rd.data().begin() + j + 1;
// Ab(i,j+1:end) -= ai*rd(j+1:end) // Ab(i,j+1:end) -= ai*rd(j+1:end)
for (int j2 = j + 1; j2 < n+1; j2++, Aij++, rptr++) for (int j2 = j + 1; j2 < n+1; j2++, Aij++, rptr++)
*Aij -= ai * (*rptr); *Aij -= ai * (*rptr);
}
#endif
}
/* ************************************************************************* */
Gaussian::shared_ptr Gaussian::Covariance(const Matrix& covariance, bool smart) {
size_t m = covariance.size1(), n = covariance.size2();
if (m != n) throw invalid_argument("Gaussian::Covariance: covariance not square");
if (smart) {
// check all non-diagonal entries
int i,j;
for (i = 0; i < m; i++)
for (j = 0; j < n; j++)
if (i != j && fabs(covariance(i, j) > 1e-9)) goto full;
Vector variances(n);
for (j = 0; j < n; j++) variances(j) = covariance(j,j);
return Diagonal::Variances(variances,true);
}
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;
if (typeid(*this) != typeid(*p)) return false;
//if (!sqrt_information_) return true; // ALEX todo;
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;
}
// General QR, see also special version in Constrained
SharedDiagonal Gaussian::QR(Matrix& Ab) const {
// get size(A) and maxRank
// TODO: really no rank problems ?
size_t m = Ab.size1(), n = Ab.size2()-1;
size_t maxRank = min(m,n);
// pre-whiten everything (cheaply if possible)
WhitenInPlace(Ab);
// Perform in-place Householder
householder_(Ab, maxRank);
return Unit::Create(maxRank);
}
/* ************************************************************************* */
// TODO: can we avoid calling reciprocal twice ?
Diagonal::Diagonal(const Vector& sigmas) :
Gaussian(sigmas.size()), invsigmas_(reciprocal(sigmas)), sigmas_(
sigmas) {
}
Diagonal::shared_ptr Diagonal::Variances(const Vector& variances, bool smart) {
if (smart) {
// check whether all the same entry
int j, n = variances.size();
for (j = 1; j < n; j++)
if (variances(j) != variances(0)) goto full;
return Isotropic::Variance(n, variances(0), true);
}
full: return shared_ptr(new Diagonal(esqrt(variances)));
}
void Diagonal::print(const string& name) const {
gtsam::print(sigmas_, "Diagonal sigmas " + name);
}
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 {
H = vector_scale(invsigmas_, H);
}
Vector Diagonal::sample() const {
Vector result(dim_);
for (int i = 0; i < dim_; i++) {
typedef boost::normal_distribution<double> Normal;
Normal dist(0.0, this->sigmas_(i));
boost::variate_generator<boost::minstd_rand&, Normal> norm(generator, dist);
result(i) = norm();
}
return result;
}
/* ************************************************************************* */
void Constrained::print(const std::string& name) const {
gtsam::print(sigmas_, "Constrained sigmas " + name);
}
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");
}
// 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 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);
// create storage for [R d]
typedef boost::tuple<size_t, Vector, double> Triple;
list<Triple> Rd;
Vector pseudo(m); // allocate storage for pseudo-inverse
Vector weights = emul(invsigmas_,invsigmas_); // calculate weights once
// 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 (size_t j=0; j<n; ++j) {
// extract the first column of A
// ublas::matrix_column is slower ! TODO Really, why ????
// AGC: if you use column() you will automatically call ublas, use
// column_() to actually use the one in our library
Vector a(column(Ab, j));
// Calculate weighted pseudo-inverse and corresponding precision
double precision = weightedPseudoinverse(a, weights, pseudo);
// If precision is zero, no information on this column
// This is actually not limited to constraints, could happen in Gaussian::QR
// In that case, we're probably hosed. TODO: make sure Householder is rank-revealing
if (precision < 1e-8) continue;
// create solution [r d], rhs is automatically r(n)
Vector rd(n+1); // uninitialized !
rd(j)=1.0; // put 1 on diagonal
for (size_t j2=j+1; j2<n+1; ++j2) // and fill in remainder with dot-products
rd(j2) = inner_prod(pseudo, ublas::matrix_column<Matrix>(Ab, j2));
// construct solution (r, d, sigma)
Rd.push_back(boost::make_tuple(j, rd, precision));
// exit after rank exhausted
if (Rd.size()>=maxRank) break;
// update Ab, expensive, using outer product
updateAb(Ab, j, a, rd);
}
// Create storage for precisions
Vector precisions(Rd.size());
// Write back result in Ab, imperative as we are
// TODO: test that is correct if a column was skipped !!!!
size_t i = 0; // start with first row
bool mixed = false;
BOOST_FOREACH(const Triple& t, Rd) {
const size_t& j = t.get<0>();
const Vector& rd = t.get<1>();
precisions(i) = t.get<2>();
if (precisions(i)==inf) mixed = true;
for (size_t j2=0; j2<j; ++j2) Ab(i,j2) = 0.0; // fill in zeros below diagonal anway
for (size_t j2=j; j2<n+1; ++j2) // copy the j-the row TODO memcpy
Ab(i,j2) = rd(j2);
i+=1;
}
return mixed ? Constrained::MixedPrecisions(precisions) : Diagonal::Precisions(precisions);
}
/* ************************************************************************* */
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 << "Isotropic sigma " << name << " " << 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_;
}
// faster version
Vector Isotropic::sample() const {
typedef boost::normal_distribution<double> Normal;
Normal dist(0.0, this->sigma_);
boost::variate_generator<boost::minstd_rand&, Normal> norm(generator, dist);
Vector result(dim_);
for (int i = 0; i < dim_; i++)
result(i) = norm();
return result;
}
/* ************************************************************************* */
void Unit::print(const std::string& name) const {
cout << "Unit (" << dim_ << ") " << name << endl;
}
/* ************************************************************************* */
} }
#endif
}
/* ************************************************************************* */
Gaussian::shared_ptr Gaussian::Covariance(const Matrix& covariance, bool smart) {
size_t m = covariance.size1(), n = covariance.size2();
if (m != n) throw invalid_argument("Gaussian::Covariance: covariance not square");
if (smart) {
// check all non-diagonal entries
int i,j;
for (i = 0; i < m; i++)
for (j = 0; j < n; j++)
if (i != j && fabs(covariance(i, j) > 1e-9)) goto full;
Vector variances(n);
for (j = 0; j < n; j++) variances(j) = covariance(j,j);
return Diagonal::Variances(variances,true);
}
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;
if (typeid(*this) != typeid(*p)) return false;
//if (!sqrt_information_) return true; // ALEX todo;
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;
}
// General QR, see also special version in Constrained
SharedDiagonal Gaussian::QR(Matrix& Ab) const {
bool verbose = false;
if (verbose) cout << "\nIn Gaussian::QR" << endl;
// get size(A) and maxRank
// TODO: really no rank problems ?
size_t m = Ab.size1(), n = Ab.size2()-1;
size_t maxRank = min(m,n);
// pre-whiten everything (cheaply if possible)
if (verbose) gtsam::print(Ab, "Ab before whitening");
WhitenInPlace(Ab);
if (verbose) gtsam::print(Ab, "Ab after whitening");
// Perform in-place Householder
householder(Ab, maxRank);
if (verbose) gtsam::print(Ab, "Ab before householder");
return Unit::Create(maxRank);
}
/* ************************************************************************* */
Diagonal::Diagonal(const Vector& sigmas) :
Gaussian(sigmas.size()), invsigmas_(reciprocal(sigmas)), sigmas_(sigmas) {
}
Diagonal::shared_ptr Diagonal::Variances(const Vector& variances, bool smart) {
if (smart) {
// check whether all the same entry
int j, n = variances.size();
for (j = 1; j < n; j++)
if (variances(j) != variances(0)) goto full;
return Isotropic::Variance(n, variances(0), true);
}
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
for (size_t i=0; i<sigmas.size(); ++i)
if (sigmas(i)<1e-8)
return Constrained::MixedSigmas(sigmas);
}
return Diagonal::shared_ptr(new Diagonal(sigmas));
}
void Diagonal::print(const string& name) const {
gtsam::print(sigmas_, "Diagonal sigmas " + name);
}
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 {
H = vector_scale(invsigmas_, H);
}
Vector Diagonal::sample() const {
Vector result(dim_);
for (int i = 0; i < dim_; i++) {
typedef boost::normal_distribution<double> Normal;
Normal dist(0.0, this->sigmas_(i));
boost::variate_generator<boost::minstd_rand&, Normal> norm(generator, dist);
result(i) = norm();
}
return result;
}
/* ************************************************************************* */
void Constrained::print(const std::string& name) const {
gtsam::print(sigmas_, "Constrained sigmas " + name);
}
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");
}
// 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
// Previously Diagonal::QR
SharedDiagonal Constrained::QR(Matrix& Ab) const {
bool verbose = false;
if (verbose) cout << "\nStarting Constrained::QR" << endl;
// get size(A) and maxRank
size_t m = Ab.size1(), n = Ab.size2()-1;
size_t maxRank = min(m,n);
// create storage for [R d]
typedef boost::tuple<size_t, Vector, double> Triple;
list<Triple> Rd;
Vector pseudo(m); // allocate storage for pseudo-inverse
Vector weights = emul(invsigmas_,invsigmas_); // calculate weights once
// 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 (size_t j=0; j<n; ++j) {
// extract the first column of A
// ublas::matrix_column is slower ! TODO Really, why ????
// AGC: if you use column() you will automatically call ublas, use
// column_() to actually use the one in our library
Vector a(column(Ab, j));
// Calculate weighted pseudo-inverse and corresponding precision
double precision = weightedPseudoinverse(a, weights, pseudo);
// If precision is zero, no information on this column
// This is actually not limited to constraints, could happen in Gaussian::QR
// In that case, we're probably hosed. TODO: make sure Householder is rank-revealing
if (precision < 1e-8) continue;
// create solution [r d], rhs is automatically r(n)
Vector rd(n+1); // uninitialized !
rd(j)=1.0; // put 1 on diagonal
for (size_t j2=j+1; j2<n+1; ++j2) // and fill in remainder with dot-products
rd(j2) = inner_prod(pseudo, ublas::matrix_column<Matrix>(Ab, j2));
// construct solution (r, d, sigma)
Rd.push_back(boost::make_tuple(j, rd, precision));
// exit after rank exhausted
if (Rd.size()>=maxRank) break;
// update Ab, expensive, using outer product
updateAb(Ab, j, a, rd);
}
// Create storage for precisions
Vector precisions(Rd.size());
// Write back result in Ab, imperative as we are
// TODO: test that is correct if a column was skipped !!!!
size_t i = 0; // start with first row
bool mixed = false;
BOOST_FOREACH(const Triple& t, Rd) {
const size_t& j = t.get<0>();
const Vector& rd = t.get<1>();
precisions(i) = t.get<2>();
if (precisions(i)==inf) mixed = true;
for (size_t j2=0; j2<j; ++j2) Ab(i,j2) = 0.0; // fill in zeros below diagonal anway
for (size_t j2=j; j2<n+1; ++j2) // copy the j-the row TODO memcpy
Ab(i,j2) = rd(j2);
i+=1;
}
return mixed ? Constrained::MixedPrecisions(precisions) : Diagonal::Precisions(precisions);
}
/* ************************************************************************* */
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 << "Isotropic sigma " << name << " " << 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_;
}
// faster version
Vector Isotropic::sample() const {
typedef boost::normal_distribution<double> Normal;
Normal dist(0.0, this->sigma_);
boost::variate_generator<boost::minstd_rand&, Normal> norm(generator, dist);
Vector result(dim_);
for (int i = 0; i < dim_; i++)
result(i) = norm();
return result;
}
/* ************************************************************************* */
void Unit::print(const std::string& name) const {
cout << "Unit (" << dim_ << ") " << name << endl;
}
/* ************************************************************************* */
}
} // gtsam } // gtsam

592
cpp/NoiseModel.h
View File

@ -15,222 +15,216 @@
namespace gtsam { namespace gtsam {
class SharedDiagonal; // forward declare, defined at end class SharedDiagonal; // forward declare, defined at end
namespace noiseModel { namespace noiseModel {
/** /**
* noiseModel::Base is the abstract base class for all noise models. * noiseModel::Base is the abstract base class for all noise models.
* *
* Noise models must implement a 'whiten' function to normalize an error vector, * Noise models must implement a 'whiten' function to normalize an error vector,
* and an 'unwhiten' function to unnormalize an error vector. * and an 'unwhiten' function to unnormalize an error vector.
*/ */
class Base : public Testable<Base> { class Base : public Testable<Base> {
protected: protected:
size_t dim_; size_t dim_;
public: public:
Base(size_t dim):dim_(dim) {} Base(size_t dim):dim_(dim) {}
virtual ~Base() {} virtual ~Base() {}
/** /**
* Dimensionality * Dimensionality
*/
inline size_t dim() const { return dim_;}
/**
* Whiten an error vector.
*/
virtual Vector whiten(const Vector& v) const = 0;
/**
* Unwhiten an error vector.
*/
virtual Vector unwhiten(const Vector& v) const = 0;
/** in-place whiten, override if can be done more efficiently */
virtual void whitenInPlace(Vector& v) const {
v = whiten(v);
}
/** in-place unwhiten, override if can be done more efficiently */
virtual void unwhitenInPlace(Vector& v) const {
v = unwhiten(v);
}
};
/**
* Gaussian implements the mathematical model
* |R*x|^2 = |y|^2 with R'*R=inv(Sigma)
* where
* y = whiten(x) = R*x
* x = unwhiten(x) = inv(R)*y
* as indeed
* |y|^2 = y'*y = x'*R'*R*x
* Various derived classes are available that are more efficient.
*/ */
struct Gaussian: public Base { inline size_t dim() const { return dim_;}
private: /**
* Whiten an error vector.
*/
virtual Vector whiten(const Vector& v) const = 0;
// TODO: store as boost upper-triangular or whatever is passed from above /**
/* Matrix square root of information matrix (R) */ * Unwhiten an error vector.
boost::optional<Matrix> sqrt_information_; */
virtual Vector unwhiten(const Vector& v) const = 0;
/** /** in-place whiten, override if can be done more efficiently */
* Return R itself, but note that Whiten(H) is cheaper than R*H virtual void whitenInPlace(Vector& v) const {
*/ v = whiten(v);
const Matrix& thisR() const { }
// should never happen
if (!sqrt_information_) throw std::runtime_error("Gaussian: has no R matrix");
return *sqrt_information_;
}
protected: /** in-place unwhiten, override if can be done more efficiently */
virtual void unwhitenInPlace(Vector& v) const {
v = unwhiten(v);
}
};
/** protected constructor takes square root information matrix */ /**
Gaussian(size_t dim, const boost::optional<Matrix>& sqrt_information = boost::none) : * Gaussian implements the mathematical model
Base(dim), sqrt_information_(sqrt_information) { * |R*x|^2 = |y|^2 with R'*R=inv(Sigma)
} * where
* y = whiten(x) = R*x
* x = unwhiten(x) = inv(R)*y
* as indeed
* |y|^2 = y'*y = x'*R'*R*x
* Various derived classes are available that are more efficient.
*/
struct Gaussian: public Base {
public: private:
typedef boost::shared_ptr<Gaussian> shared_ptr; // TODO: store as boost upper-triangular or whatever is passed from above
/* Matrix square root of information matrix (R) */
boost::optional<Matrix> sqrt_information_;
/** /**
* A Gaussian noise model created by specifying a square root information matrix. * Return R itself, but note that Whiten(H) is cheaper than R*H
* @param smart, check if can be simplified to derived class */
*/ const Matrix& thisR() const {
static shared_ptr SqrtInformation(const Matrix& R) { // should never happen
return shared_ptr(new Gaussian(R.size1(),R)); if (!sqrt_information_) throw std::runtime_error("Gaussian: has no R matrix");
} return *sqrt_information_;
}
/** protected:
* A Gaussian noise model created by specifying a covariance matrix.
* @param smart, check if can be simplified to derived class
*/
static shared_ptr Covariance(const Matrix& covariance, bool smart=false);
/** /** protected constructor takes square root information matrix */
* A Gaussian noise model created by specifying an information matrix. Gaussian(size_t dim, const boost::optional<Matrix>& sqrt_information = boost::none) :
*/ Base(dim), sqrt_information_(sqrt_information) {
static shared_ptr Information(const Matrix& Q) { }
return shared_ptr(new Gaussian(Q.size1(),square_root_positive(Q)));
}
virtual void print(const std::string& name) const; public:
virtual bool equals(const Base& expected, double tol) const;
virtual Vector whiten(const Vector& v) const;
virtual Vector unwhiten(const Vector& v) const;
/** typedef boost::shared_ptr<Gaussian> shared_ptr;
* Mahalanobis distance v'*R'*R*v = <R*v,R*v>
*/
virtual double Mahalanobis(const Vector& v) const;
/** /**
* Multiply a derivative with R (derivative of whiten) * A Gaussian noise model created by specifying a square root information matrix.
* Equivalent to whitening each column of the input matrix. * @param smart, check if can be simplified to derived class
*/ */
virtual Matrix Whiten(const Matrix& H) const; static shared_ptr SqrtInformation(const Matrix& R) {
return shared_ptr(new Gaussian(R.size1(),R));
}
/** /**
* In-place version * A Gaussian noise model created by specifying a covariance matrix.
*/ * @param smart, check if can be simplified to derived class
virtual void WhitenInPlace(Matrix& H) const; */
static shared_ptr Covariance(const Matrix& covariance, bool smart=false);
/** /**
* Whiten a system, in place as well * A Gaussian noise model created by specifying an information matrix.
*/ */
inline void WhitenSystem(Matrix& A, Vector& b) const { static shared_ptr Information(const Matrix& Q) {
WhitenInPlace(A); return shared_ptr(new Gaussian(Q.size1(),square_root_positive(Q)));
whitenInPlace(b); }
}
/** virtual void print(const std::string& name) const;
* Apply appropriately weighted QR factorization to the system [A b] virtual bool equals(const Base& expected, double tol) const;
* Q' * [A b] = [R d] virtual Vector whiten(const Vector& v) const;
* Dimensions: (r*m) * m*(n+1) = r*(n+1) virtual Vector unwhiten(const Vector& v) const;
* @param Ab is the m*(n+1) augmented system matrix [A b]
* @return in-place QR factorization [R d]. Below-diagonal is undefined !!!!!
*/
virtual SharedDiagonal QR(Matrix& Ab) const;
/** /**
* Return R itself, but note that Whiten(H) is cheaper than R*H * Mahalanobis distance v'*R'*R*v = <R*v,R*v>
*/ */
virtual Matrix R() const { return thisR();} virtual double Mahalanobis(const Vector& v) const;
}; // Gaussian /**
* Multiply a derivative with R (derivative of whiten)
* Equivalent to whitening each column of the input matrix.
*/
virtual Matrix Whiten(const Matrix& H) const;
// FD: does not work, ambiguous overload :-( /**
// inline Vector operator*(const Gaussian& R, const Vector& v) {return R.whiten(v);} * In-place version
*/
virtual void WhitenInPlace(Matrix& H) const;
/** /**
* A diagonal noise model implements a diagonal covariance matrix, with the * Whiten a system, in place as well
* elements of the diagonal specified in a Vector. This class has no public */
* constructors, instead, use the static constructor functions Sigmas etc... inline void WhitenSystem(Matrix& A, Vector& b) const {
*/ WhitenInPlace(A);
class Diagonal : public Gaussian { whitenInPlace(b);
protected: }
/** sigmas and reciprocal */ /**
Vector sigmas_, invsigmas_; * Apply appropriately weighted QR factorization to the system [A b]
* Q' * [A b] = [R d]
* Dimensions: (r*m) * m*(n+1) = r*(n+1)
* @param Ab is the m*(n+1) augmented system matrix [A b]
* @return in-place QR factorization [R d]. Below-diagonal is undefined !!!!!
*/
virtual SharedDiagonal QR(Matrix& Ab) const;
/** protected constructor takes sigmas */ /**
Diagonal(const Vector& sigmas); * Return R itself, but note that Whiten(H) is cheaper than R*H
*/
virtual Matrix R() const { return thisR();}
public: }; // Gaussian
// FD: does not work, ambiguous overload :-(
// inline Vector operator*(const Gaussian& R, const Vector& v) {return R.whiten(v);}
/**
* 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 Diagonal : public Gaussian {
protected:
/** sigmas and reciprocal */
Vector sigmas_, invsigmas_;
/** protected constructor takes sigmas */
Diagonal(const Vector& sigmas);
public:
typedef boost::shared_ptr<Diagonal> shared_ptr; typedef boost::shared_ptr<Diagonal> shared_ptr;
/** /**
* A diagonal noise model created by specifying a Vector of sigmas, i.e. * A diagonal noise model created by specifying a Vector of sigmas, i.e.
* standard devations, the diagonal of the square root covariance matrix. * standard devations, the diagonal of the square root covariance matrix.
*/ */
static shared_ptr Sigmas(const Vector& sigmas) { static shared_ptr Sigmas(const Vector& sigmas, bool smart=false);
return shared_ptr(new Diagonal(sigmas));
}
/** /**
* A diagonal noise model created by specifying a Vector of variances, i.e. * A diagonal noise model created by specifying a Vector of variances, i.e.
* i.e. the diagonal of the covariance matrix. * i.e. the diagonal of the covariance matrix.
* @param smart, check if can be simplified to derived class * @param smart, check if can be simplified to derived class
*/ */
static shared_ptr Variances(const Vector& variances, bool smart = false); static shared_ptr Variances(const Vector& variances, bool smart = false);
/** /**
* A diagonal noise model created by specifying a Vector of precisions, i.e. * A diagonal noise model created by specifying a Vector of precisions, i.e.
* i.e. the diagonal of the information matrix, i.e., weights * i.e. the diagonal of the information matrix, i.e., weights
*/ */
static shared_ptr Precisions(const Vector& precisions) { static shared_ptr Precisions(const Vector& precisions) {
return Variances(reciprocal(precisions)); return Variances(reciprocal(precisions));
} }
virtual void print(const std::string& name) const; virtual void print(const std::string& name) const;
virtual Vector whiten(const Vector& v) const; virtual Vector whiten(const Vector& v) const;
virtual Vector unwhiten(const Vector& v) const; virtual Vector unwhiten(const Vector& v) const;
virtual Matrix Whiten(const Matrix& H) const; virtual Matrix Whiten(const Matrix& H) const;
virtual void WhitenInPlace(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) * Return standard deviations (sqrt of diagonal)
*/ */
inline const Vector& sigmas() const { return sigmas_; } inline const Vector& sigmas() const { return sigmas_; }
inline double sigma(size_t i) const { return sigmas_(i); } inline double sigma(size_t i) const { return sigmas_(i); }
/** /**
* generate random variate * generate random variate
*/ */
virtual Vector sample() const; virtual Vector sample() const;
/** /**
* Return R itself, but note that Whiten(H) is cheaper than R*H * Return R itself, but note that Whiten(H) is cheaper than R*H
@ -238,155 +232,171 @@ namespace gtsam {
virtual Matrix R() const { virtual Matrix R() const {
return diag(invsigmas_); return diag(invsigmas_);
} }
}; // Diagonal
/**
* Simple check for constrained-ness
*/
virtual bool isConstrained() {return false;}
/** }; // Diagonal
* A Constrained constrained model is a specialization of Diagonal which allows
* some or all of the sigmas to be zero, forcing the error to be zero there.
* All other Gaussian models are guaranteed to have a non-singular square-root
* information matrix, but this class is specifically equipped to deal with
* singular noise models, specifically: whiten will return zero on those
* components that have zero sigma *and* zero error, infinity otherwise.
*/
class Constrained : public Diagonal {
protected:
// Constrained does not have member variables /**
// Instead (possibly zero) sigmas are stored in Diagonal Base class * A Constrained constrained model is a specialization of Diagonal which allows
* some or all of the sigmas to be zero, forcing the error to be zero there.
* All other Gaussian models are guaranteed to have a non-singular square-root
* information matrix, but this class is specifically equipped to deal with
* singular noise models, specifically: whiten will return zero on those
* components that have zero sigma *and* zero error, infinity otherwise.
*/
class Constrained : public Diagonal {
protected:
/** protected constructor takes sigmas */ // Constrained does not have member variables
Constrained(const Vector& sigmas) :Diagonal(sigmas) {} // Instead (possibly zero) sigmas are stored in Diagonal Base class
public: /** protected constructor takes sigmas */
Constrained(const Vector& sigmas) :Diagonal(sigmas) {}
typedef boost::shared_ptr<Constrained> shared_ptr; public:
/** typedef boost::shared_ptr<Constrained> shared_ptr;
* A diagonal noise model created by specifying a Vector of
* standard devations, some of which might be zero
*/
static shared_ptr MixedSigmas(const Vector& sigmas) {
return shared_ptr(new Constrained(sigmas));
}
/** /**
* A diagonal noise model created by specifying a Vector of * A diagonal noise model created by specifying a Vector of
* standard devations, some of which might be zero * standard devations, some of which might be zero
*/ * TODO: make smart - check for zeros
static shared_ptr MixedVariances(const Vector& variances) { */
return shared_ptr(new Constrained(esqrt(variances))); static shared_ptr MixedSigmas(const Vector& sigmas, bool smart = false) {
} return shared_ptr(new Constrained(sigmas));
}
/** /**
* A diagonal noise model created by specifying a Vector of * A diagonal noise model created by specifying a Vector of
* precisions, some of which might be inf * standard devations, some of which might be zero
*/ */
static shared_ptr MixedPrecisions(const Vector& precisions) { static shared_ptr MixedVariances(const Vector& variances) {
return MixedVariances(reciprocal(precisions)); return shared_ptr(new Constrained(esqrt(variances)));
} }
/** /**
* Fully constrained. TODO: subclass ? * A diagonal noise model created by specifying a Vector of
*/ * precisions, some of which might be inf
static shared_ptr All(size_t dim) { */
return MixedSigmas(repeat(dim,0)); static shared_ptr MixedPrecisions(const Vector& precisions) {
} return MixedVariances(reciprocal(precisions));
}
virtual void print(const std::string& name) const; /**
virtual Vector whiten(const Vector& v) const; * Fully constrained. TODO: subclass ?
*/
static shared_ptr All(size_t dim) {
return MixedSigmas(repeat(dim,0));
}
// Whitening Jacobians does not make sense for possibly constrained virtual void print(const std::string& name) const;
// noise model and will throw an exception. virtual Vector whiten(const Vector& v) const;
virtual Matrix Whiten(const Matrix& H) const; // Whitening Jacobians does not make sense for possibly constrained
virtual void WhitenInPlace(Matrix& H) const; // noise model and will throw an exception.
}; // Constrained virtual Matrix Whiten(const Matrix& H) const;
virtual void WhitenInPlace(Matrix& H) const;
/** /**
* An isotropic noise model corresponds to a scaled diagonal covariance * Apply QR factorization to the system [A b], taking into account constraints
* To construct, use one of the static methods */
*/ virtual SharedDiagonal QR(Matrix& Ab) const;
class Isotropic : public Diagonal {
protected:
double sigma_, invsigma_;
/** protected constructor takes sigma */ /**
Isotropic(size_t dim, double sigma) : * Not constrained
Diagonal(repeat(dim, sigma)),sigma_(sigma),invsigma_(1.0/sigma) {} */
virtual bool isConstrained() {return true;}
public: }; // Constrained
typedef boost::shared_ptr<Isotropic> shared_ptr; /**
* An isotropic noise model corresponds to a scaled diagonal covariance
* To construct, use one of the static methods
*/
class Isotropic : public Diagonal {
protected:
double sigma_, invsigma_;
/** /** protected constructor takes sigma */
* An isotropic noise model created by specifying a standard devation sigma Isotropic(size_t dim, double sigma) :
*/ Diagonal(repeat(dim, sigma)),sigma_(sigma),invsigma_(1.0/sigma) {}
static shared_ptr Sigma(size_t dim, double sigma) {
return shared_ptr(new Isotropic(dim, sigma));
}
/** public:
* An isotropic noise model created by specifying a variance = sigma^2.
* @param smart, check if can be simplified to derived class
*/
static shared_ptr Variance(size_t dim, double variance, bool smart = false);
/** typedef boost::shared_ptr<Isotropic> shared_ptr;
* 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 void print(const std::string& name) const; /**
virtual double Mahalanobis(const Vector& v) const; * An isotropic noise model created by specifying a standard devation sigma
virtual Vector whiten(const Vector& v) const; */
virtual Vector unwhiten(const Vector& v) const; static shared_ptr Sigma(size_t dim, double sigma) {
virtual Matrix Whiten(const Matrix& H) const; return shared_ptr(new Isotropic(dim, sigma));
virtual void WhitenInPlace(Matrix& H) const; }
/** /**
* Return standard deviation * An isotropic noise model created by specifying a variance = sigma^2.
*/ * @param smart, check if can be simplified to derived class
inline double sigma() const { return sigma_; } */
static shared_ptr Variance(size_t dim, double variance, bool smart = false);
/** /**
* generate random variate * An isotropic noise model created by specifying a precision
*/ */
virtual Vector sample() const; static shared_ptr Precision(size_t dim, double precision) {
return Variance(dim, 1.0/precision);
}
}; virtual void print(const std::string& name) const;
virtual double Mahalanobis(const Vector& v) const;
virtual Vector whiten(const Vector& v) const;
virtual Vector unwhiten(const Vector& v) const;
virtual Matrix Whiten(const Matrix& H) const;
virtual void WhitenInPlace(Matrix& H) const;
/** /**
* Unit: i.i.d. unit-variance noise on all m dimensions. * Return standard deviation
*/ */
class Unit : public Isotropic { inline double sigma() const { return sigma_; }
protected:
Unit(size_t dim): Isotropic(dim,1.0) {} /**
* generate random variate
*/
virtual Vector sample() const;
public: };
typedef boost::shared_ptr<Unit> shared_ptr; /**
* Unit: i.i.d. unit-variance noise on all m dimensions.
*/
class Unit : public Isotropic {
protected:
/** Unit(size_t dim): Isotropic(dim,1.0) {}
* Create a unit covariance noise model
*/
static shared_ptr Create(size_t dim) {
return shared_ptr(new Unit(dim));
}
virtual void print(const std::string& name) const; public:
virtual double Mahalanobis(const Vector& v) const {return inner_prod(v,v); }
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 {}
};
} // namespace noiseModel typedef boost::shared_ptr<Unit> shared_ptr;
/**
* Create a unit covariance noise model
*/
static shared_ptr Create(size_t dim) {
return shared_ptr(new Unit(dim));
}
virtual void print(const std::string& name) const;
virtual double Mahalanobis(const Vector& v) const {return inner_prod(v,v); }
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 {}
};
} // namespace noiseModel
using namespace noiseModel;
} // namespace gtsam } // namespace gtsam

5
cpp/NonlinearFactor.h
View File

@ -85,6 +85,11 @@ namespace gtsam {
return keys_; return keys_;
} }
/** access to the noise model */
SharedGaussian get_noiseModel() const {
return noiseModel_;
}
/** get the size of the factor */ /** get the size of the factor */
std::size_t size() const { std::size_t size() const {
return keys_.size(); return keys_.size();

18
cpp/smallExample.cpp
View File

@ -126,19 +126,28 @@ namespace example {
// linearized prior on x1: c["x1"]+x1=0 i.e. x1=-c["x1"] // linearized prior on x1: c["x1"]+x1=0 i.e. x1=-c["x1"]
Vector b1 = -Vector_(2,0.1, 0.1); Vector b1 = -Vector_(2,0.1, 0.1);
fg.add("x1", I, b1, sigma0_1); //fg.add("x1", I, b1, sigma0_1);
fg.add("x1", 10*eye(2), -1.0*ones(2), noiseModel::Unit::Create(2));
// odometry between x1 and x2: x2-x1=[0.2;-0.1] // odometry between x1 and x2: x2-x1=[0.2;-0.1]
Vector b2 = Vector_(2, 0.2, -0.1); Vector b2 = Vector_(2, 0.2, -0.1);
fg.add("x1", -I, "x2", I, b2, sigma0_1); //fg.add("x1", -I, "x2", I, b2, sigma0_1);
fg.add("x1", -10*eye(2),"x2", 10*eye(2), Vector_(2, 2.0, -1.0),
noiseModel::Unit::Create(2));
// measurement between x1 and l1: l1-x1=[0.0;0.2] // measurement between x1 and l1: l1-x1=[0.0;0.2]
Vector b3 = Vector_(2, 0.0, 0.2); Vector b3 = Vector_(2, 0.0, 0.2);
fg.add("x1", -I, "l1", I, b3, sigma0_2); //fg.add("x1", -I, "l1", I, b3, sigma0_2);
fg.add("x1", -5*eye(2),
"l1", 5*eye(2), Vector_(2, 0.0, 1.0),
noiseModel::Unit::Create(2));
// measurement between x2 and l1: l1-x2=[-0.2;0.3] // measurement between x2 and l1: l1-x2=[-0.2;0.3]
Vector b4 = Vector_(2, -0.2, 0.3); Vector b4 = Vector_(2, -0.2, 0.3);
fg.add("x2", -I, "l1", I, b4, sigma0_2); //fg.add("x2", -I, "l1", I, b4, sigma0_2);
fg.add("x2", -5*eye(2),
"l1", 5*eye(2), Vector_(2, -1.0, 1.5),
noiseModel::Unit::Create(2));
return fg; return fg;
} }
@ -306,6 +315,7 @@ namespace example {
Vector b1(2); Vector b1(2);
b1(0) = 1.0; b1(0) = 1.0;
b1(1) = -1.0; b1(1) = -1.0;
//GaussianFactor::shared_ptr f1(new GaussianFactor("x", sigma0_1->Whiten(Ax), sigma0_1->whiten(b1), sigma0_1));
GaussianFactor::shared_ptr f1(new GaussianFactor("x", Ax, b1, sigma0_1)); GaussianFactor::shared_ptr f1(new GaussianFactor("x", Ax, b1, sigma0_1));
// create binary constraint factor // create binary constraint factor

92
cpp/testGaussianFactor.cpp
View File

@ -35,8 +35,8 @@ static SharedDiagonal
TEST( GaussianFactor, linearFactor ) TEST( GaussianFactor, linearFactor )
{ {
Matrix I = eye(2); Matrix I = eye(2);
Vector b = Vector_(2,0.2,-0.1); Vector b = Vector_(2, 2.0, -1.0);
GaussianFactor expected("x1", -I, "x2", I, b, sigma0_1); GaussianFactor expected("x1", -10*I,"x2", 10*I, b, noiseModel::Unit::Create(2));
// create a small linear factor graph // create a small linear factor graph
GaussianFactorGraph fg = createGaussianFactorGraph(); GaussianFactorGraph fg = createGaussianFactorGraph();
@ -131,37 +131,37 @@ TEST( GaussianFactor, combine )
// the expected combined linear factor // the expected combined linear factor
Matrix Ax2 = Matrix_(4, 2, // x2 Matrix Ax2 = Matrix_(4, 2, // x2
-1., 0., -5., 0.,
+0., -1., +0., -5.,
1., 0., 10., 0.,
+0., 1.); +0., 10.);
Matrix Al1 = Matrix_(4, 2, // l1 Matrix Al1 = Matrix_(4, 2, // l1
1., 0., 5., 0.,
0., 1., 0., 5.,
0., 0., 0., 0.,
0., 0.); 0., 0.);
Matrix Ax1 = Matrix_(4, 2, // x1 Matrix Ax1 = Matrix_(4, 2, // x1
0.00, 0., // f4 0.00, 0., // f4
0.00, 0., // f4 0.00, 0., // f4
-1., 0., // f2 -10., 0., // f2
0.00, -1. // f2 0.00, -10. // f2
); );
// the RHS // the RHS
Vector b2(4); Vector b2(4);
b2(0) = -0.2; b2(0) = -1.0;
b2(1) = 0.3; b2(1) = 1.5;
b2(2) = 0.2; b2(2) = 2.0;
b2(3) = -0.1; b2(3) = -1.0;
// use general constructor for making arbitrary factors // use general constructor for making arbitrary factors
vector<pair<Symbol, Matrix> > meas; vector<pair<Symbol, Matrix> > meas;
meas.push_back(make_pair("x2", Ax2)); meas.push_back(make_pair("x2", Ax2));
meas.push_back(make_pair("l1", Al1)); meas.push_back(make_pair("l1", Al1));
meas.push_back(make_pair("x1", Ax1)); meas.push_back(make_pair("x1", Ax1));
GaussianFactor expected(meas, b2, sigmas); GaussianFactor expected(meas, b2, noiseModel::Diagonal::Sigmas(ones(4)));
CHECK(assert_equal(expected,combined)); CHECK(assert_equal(expected,combined));
} }
@ -320,23 +320,24 @@ TEST( GaussianFactor, eliminate )
boost::tie(actualCG,actualLF) = combined.eliminate("x2"); boost::tie(actualCG,actualLF) = combined.eliminate("x2");
// create expected Conditional Gaussian // create expected Conditional Gaussian
Matrix I = eye(2); Matrix I = eye(2)*sqrt(125.0);
Matrix R11 = I, S12 = -0.2*I, S13 = -0.8*I; Matrix R11 = I, S12 = -0.2*I, S13 = -0.8*I;
Vector d(2); d(0) = 0.2; d(1) = -0.14; Vector d = I*Vector_(2,0.2,-0.14);
// Check the conditional Gaussian // Check the conditional Gaussian
GaussianConditional GaussianConditional
expectedCG("x2", d, R11, "l1", S12, "x1", S13, repeat(2, 1 / sqrt(125.0))); expectedCG("x2", d, R11, "l1", S12, "x1", S13, repeat(2, 1.0));
// the expected linear factor // the expected linear factor
I = eye(2)/0.2236;
Matrix Bl1 = I, Bx1 = -I; Matrix Bl1 = I, Bx1 = -I;
Vector b1(2); b1(0) = 0.0; b1(1) = 0.2; Vector b1 = I*Vector_(2,0.0,0.2);
GaussianFactor expectedLF("l1", Bl1, "x1", Bx1, b1, repeat(2,0.2236)); GaussianFactor expectedLF("l1", Bl1, "x1", Bx1, b1, repeat(2,1.0));
// check if the result matches // check if the result matches
CHECK(assert_equal(expectedCG,*actualCG,1e-4)); CHECK(assert_equal(expectedCG,*actualCG,1e-3));
CHECK(assert_equal(expectedLF,*actualLF,1e-5)); CHECK(assert_equal(expectedLF,*actualLF,1e-3));
} }
@ -383,21 +384,18 @@ TEST( GaussianFactor, eliminate2 )
boost::tie(actualCG,actualLF) = combined.eliminate("x2"); boost::tie(actualCG,actualLF) = combined.eliminate("x2");
// create expected Conditional Gaussian // create expected Conditional Gaussian
double oldSigma = 0.0894427; // from when R was made unit
Matrix R11 = Matrix_(2,2, Matrix R11 = Matrix_(2,2,
1.00, 0.00, 1.00, 0.00,
0.00, 1.00 0.00, 1.00
); )/oldSigma;
Matrix S12 = Matrix_(2,4, Matrix S12 = Matrix_(2,4,
-0.20, 0.00,-0.80, 0.00, -0.20, 0.00,-0.80, 0.00,
+0.00,-0.20,+0.00,-0.80 +0.00,-0.20,+0.00,-0.80
); )/oldSigma;
Vector d(2); d(0) = 0.2; d(1) = -0.14; Vector d = Vector_(2,0.2,-0.14)/oldSigma;
GaussianConditional expectedCG("x2",d,R11,"l11",S12,ones(2));
Vector x2Sigmas(2); CHECK(assert_equal(expectedCG,*actualCG,1e-4));
x2Sigmas(0) = 0.0894427;
x2Sigmas(1) = 0.0894427;
GaussianConditional expectedCG("x2",d,R11,"l11",S12,x2Sigmas);
// the expected linear factor // the expected linear factor
double sigma = 0.2236; double sigma = 0.2236;
@ -405,16 +403,10 @@ TEST( GaussianFactor, eliminate2 )
// l1 x1 // l1 x1
1.00, 0.00, -1.00, 0.00, 1.00, 0.00, -1.00, 0.00,
0.00, 1.00, +0.00, -1.00 0.00, 1.00, +0.00, -1.00
); )/sigma;
Vector b1 =Vector_(2,0.0,0.894427);
// the RHS GaussianFactor expectedLF("l11", Bl1x1, b1, repeat(2,1.0));
Vector b1(2); b1(0) = 0.0; b1(1) = 0.894427; CHECK(assert_equal(expectedLF,*actualLF,1e-3));
GaussianFactor expectedLF("l11", Bl1x1, b1*sigma, repeat(2,sigma));
// check if the result matches
CHECK(assert_equal(expectedCG,*actualCG,1e-4));
CHECK(assert_equal(expectedLF,*actualLF,1e-5));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -463,7 +455,10 @@ TEST( GaussianFactor, matrix )
GaussianFactorGraph fg = createGaussianFactorGraph(); GaussianFactorGraph fg = createGaussianFactorGraph();
// get the factor "f2" from the factor graph // get the factor "f2" from the factor graph
GaussianFactor::shared_ptr lf = fg[1]; //GaussianFactor::shared_ptr lf = fg[1]; // NOTE: using the older version
Vector b2 = Vector_(2, 0.2, -0.1);
Matrix I = eye(2);
GaussianFactor::shared_ptr lf(new GaussianFactor("x1", -I, "x2", I, b2, sigma0_1));
// render with a given ordering // render with a given ordering
Ordering ord; Ordering ord;
@ -489,7 +484,7 @@ TEST( GaussianFactor, matrix )
Matrix A2 = Matrix_(2,4, Matrix A2 = Matrix_(2,4,
-1.0, 0.0, 1.0, 0.0, -1.0, 0.0, 1.0, 0.0,
000.0,-1.0, 0.0, 1.0 ); 000.0,-1.0, 0.0, 1.0 );
Vector b2 = Vector_(2, 0.2, -0.1); //Vector b2 = Vector_(2, 2.0, -1.0);
EQUALITY(A_act2,A2); EQUALITY(A_act2,A2);
EQUALITY(b_act2,b2); EQUALITY(b_act2,b2);
@ -508,7 +503,10 @@ TEST( GaussianFactor, matrix_aug )
GaussianFactorGraph fg = createGaussianFactorGraph(); GaussianFactorGraph fg = createGaussianFactorGraph();
// get the factor "f2" from the factor graph // get the factor "f2" from the factor graph
GaussianFactor::shared_ptr lf = fg[1]; //GaussianFactor::shared_ptr lf = fg[1];
Vector b2 = Vector_(2, 0.2, -0.1);
Matrix I = eye(2);
GaussianFactor::shared_ptr lf(new GaussianFactor("x1", -I, "x2", I, b2, sigma0_1));
// render with a given ordering // render with a given ordering
Ordering ord; Ordering ord;
@ -654,9 +652,9 @@ TEST( GaussianFactor, alphaFactor )
GaussianFactor::shared_ptr actual = factor->alphaFactor(alphaKey,x,d); GaussianFactor::shared_ptr actual = factor->alphaFactor(alphaKey,x,d);
// calculate expected // calculate expected
Matrix A = Matrix_(2,1,30.0,5.0); Matrix A = Matrix_(2,1,300.0,50.0);
Vector b = Vector_(2,-0.1,-0.1); Vector b = Vector_(2,-1.0,-1.0);
GaussianFactor expected(alphaKey,A,b,sigma0_1); GaussianFactor expected(alphaKey,A,b,noiseModel::Unit::Create(2));
CHECK(assert_equal(expected,*actual)); CHECK(assert_equal(expected,*actual));
} }

125
cpp/testGaussianFactorGraph.cpp
View File

@ -28,7 +28,7 @@ using namespace boost::assign;
using namespace gtsam; using namespace gtsam;
using namespace example; using namespace example;
double tol=1e-4; double tol=1e-5;
/* ************************************************************************* */ /* ************************************************************************* */
/* unit test for equals (GaussianFactorGraph1 == GaussianFactorGraph2) */ /* unit test for equals (GaussianFactorGraph1 == GaussianFactorGraph2) */
@ -77,12 +77,6 @@ TEST( GaussianFactorGraph, combine_factors_x1 )
// create a small example for a linear factor graph // create a small example for a linear factor graph
GaussianFactorGraph fg = createGaussianFactorGraph(); GaussianFactorGraph fg = createGaussianFactorGraph();
// create sigmas
double sigma1 = 0.1;
double sigma2 = 0.1;
double sigma3 = 0.2;
Vector sigmas = Vector_(6, sigma1, sigma1, sigma2, sigma2, sigma3, sigma3);
// combine all factors // combine all factors
GaussianFactor::shared_ptr actual = removeAndCombineFactors(fg,"x1"); GaussianFactor::shared_ptr actual = removeAndCombineFactors(fg,"x1");
@ -92,42 +86,42 @@ TEST( GaussianFactorGraph, combine_factors_x1 )
0., 0., 0., 0.,
0., 0., 0., 0.,
0., 0., 0., 0.,
1., 0., 5., 0.,
0., 1. 0., 5.
); );
Matrix Ax1 = Matrix_(6,2, Matrix Ax1 = Matrix_(6,2,
1., 0., 10., 0.,
0., 1., 0., 10.,
-1., 0., -10., 0.,
0.,-1., 0.,-10.,
-1., 0., -5., 0.,
0.,-1. 0.,-5.
); );
Matrix Ax2 = Matrix_(6,2, Matrix Ax2 = Matrix_(6,2,
0., 0., 0., 0.,
0., 0., 0., 0.,
1., 0., 10., 0.,
0., 1., 0., 10.,
0., 0., 0., 0.,
0., 0. 0., 0.
); );
// the expected RHS vector // the expected RHS vector
Vector b(6); Vector b(6);
b(0) = -1*sigma1; b(0) = -1;
b(1) = -1*sigma1; b(1) = -1;
b(2) = 2*sigma2; b(2) = 2;
b(3) = -1*sigma2; b(3) = -1;
b(4) = 0*sigma3; b(4) = 0;
b(5) = 1*sigma3; b(5) = 1;
vector<pair<Symbol, Matrix> > meas; vector<pair<Symbol, Matrix> > meas;
meas.push_back(make_pair("l1", Al1)); meas.push_back(make_pair("l1", Al1));
meas.push_back(make_pair("x1", Ax1)); meas.push_back(make_pair("x1", Ax1));
meas.push_back(make_pair("x2", Ax2)); meas.push_back(make_pair("x2", Ax2));
GaussianFactor expected(meas, b, sigmas); GaussianFactor expected(meas, b, ones(6));
//GaussianFactor expected("l1", Al1, "x1", Ax1, "x2", Ax2, b); //GaussianFactor expected("l1", Al1, "x1", Ax1, "x2", Ax2, b);
// check if the two factors are the same // check if the two factors are the same
@ -140,11 +134,6 @@ TEST( GaussianFactorGraph, combine_factors_x2 )
// create a small example for a linear factor graph // create a small example for a linear factor graph
GaussianFactorGraph fg = createGaussianFactorGraph(); GaussianFactorGraph fg = createGaussianFactorGraph();
// determine sigmas
double sigma1 = 0.1;
double sigma2 = 0.2;
Vector sigmas = Vector_(4, sigma1, sigma1, sigma2, sigma2);
// combine all factors // combine all factors
GaussianFactor::shared_ptr actual = removeAndCombineFactors(fg,"x2"); GaussianFactor::shared_ptr actual = removeAndCombineFactors(fg,"x2");
@ -153,38 +142,38 @@ TEST( GaussianFactorGraph, combine_factors_x2 )
// l1 // l1
0., 0., 0., 0.,
0., 0., 0., 0.,
1., 0., 5., 0.,
0., 1. 0., 5.
); );
Matrix Ax1 = Matrix_(4,2, Matrix Ax1 = Matrix_(4,2,
// x1 // x1
-1., 0., // f2 -10., 0., // f2
0.,-1., // f2 0.,-10., // f2
0., 0., // f4 0., 0., // f4
0., 0. // f4 0., 0. // f4
); );
Matrix Ax2 = Matrix_(4,2, Matrix Ax2 = Matrix_(4,2,
// x2 // x2
1., 0., 10., 0.,
0., 1., 0., 10.,
-1., 0., -5., 0.,
0.,-1. 0.,-5.
); );
// the expected RHS vector // the expected RHS vector
Vector b(4); Vector b(4);
b(0) = 2*sigma1; b(0) = 2;
b(1) = -1*sigma1; b(1) = -1;
b(2) = -1 *sigma2; b(2) = -1;
b(3) = 1.5*sigma2; b(3) = 1.5;
vector<pair<Symbol, Matrix> > meas; vector<pair<Symbol, Matrix> > meas;
meas.push_back(make_pair("l1", Al1)); meas.push_back(make_pair("l1", Al1));
meas.push_back(make_pair("x1", Ax1)); meas.push_back(make_pair("x1", Ax1));
meas.push_back(make_pair("x2", Ax2)); meas.push_back(make_pair("x2", Ax2));
GaussianFactor expected(meas, b, sigmas); GaussianFactor expected(meas, b, ones(4));
// check if the two factors are the same // check if the two factors are the same
CHECK(assert_equal(expected,*actual)); CHECK(assert_equal(expected,*actual));
@ -197,9 +186,9 @@ TEST( GaussianFactorGraph, eliminateOne_x1 )
GaussianConditional::shared_ptr actual = fg.eliminateOne("x1"); GaussianConditional::shared_ptr actual = fg.eliminateOne("x1");
// create expected Conditional Gaussian // create expected Conditional Gaussian
Matrix I = eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I; Matrix I = 15*eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I;
Vector d = Vector_(2, -0.133333, -0.0222222), sigma = repeat(2, 1./15); Vector d = Vector_(2, -0.133333, -0.0222222), sigma = ones(2);
GaussianConditional expected("x1",d,R11,"l1",S12,"x2",S13,sigma); GaussianConditional expected("x1",15*d,R11,"l1",S12,"x2",S13,sigma);
CHECK(assert_equal(expected,*actual,tol)); CHECK(assert_equal(expected,*actual,tol));
} }
@ -211,8 +200,9 @@ TEST( GaussianFactorGraph, eliminateOne_x2 )
GaussianConditional::shared_ptr actual = fg.eliminateOne("x2"); GaussianConditional::shared_ptr actual = fg.eliminateOne("x2");
// create expected Conditional Gaussian // create expected Conditional Gaussian
Matrix I = eye(2), R11 = I, S12 = -0.2*I, S13 = -0.8*I; double sig = 0.0894427;
Vector d = Vector_(2, 0.2, -0.14), sigma = repeat(2, 0.0894427); Matrix I = eye(2)/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I;
Vector d = Vector_(2, 0.2, -0.14)/sig, sigma = ones(2);
GaussianConditional expected("x2",d,R11,"l1",S12,"x1",S13,sigma); GaussianConditional expected("x2",d,R11,"l1",S12,"x1",S13,sigma);
CHECK(assert_equal(expected,*actual,tol)); CHECK(assert_equal(expected,*actual,tol));
@ -225,8 +215,9 @@ TEST( GaussianFactorGraph, eliminateOne_l1 )
GaussianConditional::shared_ptr actual = fg.eliminateOne("l1"); GaussianConditional::shared_ptr actual = fg.eliminateOne("l1");
// create expected Conditional Gaussian // create expected Conditional Gaussian
Matrix I = eye(2), R11 = I, S12 = -0.5*I, S13 = -0.5*I; double sig = sqrt(2)/10.;
Vector d = Vector_(2, -0.1, 0.25), sigma = repeat(2, 0.141421); Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I;
Vector d = Vector_(2, -0.1, 0.25)/sig, sigma = ones(2);
GaussianConditional expected("l1",d,R11,"x1",S12,"x2",S13,sigma); GaussianConditional expected("l1",d,R11,"x1",S12,"x2",S13,sigma);
CHECK(assert_equal(expected,*actual,tol)); CHECK(assert_equal(expected,*actual,tol));
@ -235,24 +226,26 @@ TEST( GaussianFactorGraph, eliminateOne_l1 )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( GaussianFactorGraph, eliminateAll ) TEST( GaussianFactorGraph, eliminateAll )
{ {
// create expected Chordal bayes Net // create expected Chordal bayes Net
Matrix I = eye(2); Matrix I = eye(2);
Vector d1 = Vector_(2, -0.1,-0.1); Vector d1 = Vector_(2, -0.1,-0.1);
GaussianBayesNet expected = simpleGaussian("x1",d1,0.1); GaussianBayesNet expected = simpleGaussian("x1",d1,0.1);
Vector d2 = Vector_(2, 0.0, 0.2), sigma2 = repeat(2,0.149071); double sig1 = 0.149071;
push_front(expected,"l1",d2, I,"x1", (-1)*I,sigma2); Vector d2 = Vector_(2, 0.0, 0.2)/sig1, sigma2 = ones(2);
push_front(expected,"l1",d2, I/sig1,"x1", (-1)*I/sig1,sigma2);
Vector d3 = Vector_(2, 0.2, -0.14), sigma3 = repeat(2,0.0894427); double sig2 = 0.0894427;
push_front(expected,"x2",d3, I,"l1", (-0.2)*I, "x1", (-0.8)*I, sigma3); Vector d3 = Vector_(2, 0.2, -0.14)/sig2, sigma3 = ones(2);
push_front(expected,"x2",d3, I/sig2,"l1", (-0.2)*I/sig2, "x1", (-0.8)*I/sig2, sigma3);
// Check one ordering // Check one ordering
GaussianFactorGraph fg1 = createGaussianFactorGraph(); GaussianFactorGraph fg1 = createGaussianFactorGraph();
Ordering ordering; Ordering ordering;
ordering += "x2","l1","x1"; ordering += "x2","l1","x1";
GaussianBayesNet actual = fg1.eliminate(ordering); GaussianBayesNet actual = fg1.eliminate(ordering);
CHECK(assert_equal(expected,actual,tol)); CHECK(assert_equal(expected,actual,tol));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -267,7 +260,7 @@ TEST( GaussianFactorGraph, add_priors )
expected.push_back(GaussianFactor::shared_ptr(new GaussianFactor("l1",A,b,sigma))); 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("x1",A,b,sigma)));
expected.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x2",A,b,sigma))); expected.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x2",A,b,sigma)));
CHECK(assert_equal(expected,actual)); // Fails CHECK(assert_equal(expected,actual));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -640,14 +633,14 @@ TEST( GaussianFactorGraph, elimination )
GaussianBayesNet bayesNet = fg.eliminate(ord); GaussianBayesNet bayesNet = fg.eliminate(ord);
// Check sigma // Check sigma
DOUBLES_EQUAL(1.0/0.612372,bayesNet["x2"]->get_sigmas()(0),1e-5); DOUBLES_EQUAL(1.0,bayesNet["x2"]->get_sigmas()(0),1e-5);
// Check matrix // Check matrix
Matrix R;Vector d; Matrix R;Vector d;
boost::tie(R,d) = matrix(bayesNet); boost::tie(R,d) = matrix(bayesNet);
Matrix expected = Matrix_(2,2, Matrix expected = Matrix_(2,2,
0.707107, -0.353553, 0.707107, -0.353553,
0.0, 0.612372); 0.0, 0.612372);
CHECK(assert_equal(expected,R,1e-6)); CHECK(assert_equal(expected,R,1e-6));
} }

131
cpp/testGaussianISAM.cpp
View File

@ -25,9 +25,11 @@ using namespace example;
/* ************************************************************************* */ /* ************************************************************************* */
// Some numbers that should be consistent among all smoother tests // Some numbers that should be consistent among all smoother tests
double sigmax1 = 0.786153, sigmax2 = 0.687131, sigmax3 = 0.671512, sigmax4 = double sigmax1 = 0.786153, sigmax2 = 1.0/1.47292, sigmax3 = 0.671512, sigmax4 =
0.669534, sigmax5 = sigmax3, sigmax6 = sigmax2, sigmax7 = sigmax1; 0.669534, sigmax5 = sigmax3, sigmax6 = sigmax2, sigmax7 = sigmax1;
const double tol = 1e-4;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( ISAM, iSAM_smoother ) TEST( ISAM, iSAM_smoother )
{ {
@ -112,30 +114,30 @@ TEST( BayesTree, linear_smoother_shortcuts )
GaussianBayesNet empty; GaussianBayesNet empty;
GaussianISAM::sharedClique R = bayesTree.root(); GaussianISAM::sharedClique R = bayesTree.root();
GaussianBayesNet actual1 = R->shortcut<GaussianFactor>(R); GaussianBayesNet actual1 = R->shortcut<GaussianFactor>(R);
CHECK(assert_equal(empty,actual1,1e-4)); CHECK(assert_equal(empty,actual1,tol));
// Check the conditional P(C2|Root) // Check the conditional P(C2|Root)
GaussianISAM::sharedClique C2 = bayesTree["x5"]; GaussianISAM::sharedClique C2 = bayesTree["x5"];
GaussianBayesNet actual2 = C2->shortcut<GaussianFactor>(R); GaussianBayesNet actual2 = C2->shortcut<GaussianFactor>(R);
CHECK(assert_equal(empty,actual2,1e-4)); CHECK(assert_equal(empty,actual2,tol));
// Check the conditional P(C3|Root) // Check the conditional P(C3|Root)
Vector sigma3 = repeat(2, 0.61808); double sigma3 = 0.61808;
Matrix A56 = Matrix_(2,2,-0.382022,0.,0.,-0.382022); Matrix A56 = Matrix_(2,2,-0.382022,0.,0.,-0.382022);
GaussianBayesNet expected3; GaussianBayesNet expected3;
push_front(expected3,"x5", zero(2), eye(2), "x6", A56, sigma3); push_front(expected3,"x5", zero(2), eye(2)/sigma3, "x6", A56/sigma3, ones(2));
GaussianISAM::sharedClique C3 = bayesTree["x4"]; GaussianISAM::sharedClique C3 = bayesTree["x4"];
GaussianBayesNet actual3 = C3->shortcut<GaussianFactor>(R); GaussianBayesNet actual3 = C3->shortcut<GaussianFactor>(R);
CHECK(assert_equal(expected3,actual3,1e-4)); CHECK(assert_equal(expected3,actual3,tol));
// Check the conditional P(C4|Root) // Check the conditional P(C4|Root)
Vector sigma4 = repeat(2, 0.661968); double sigma4 = 0.661968;
Matrix A46 = Matrix_(2,2,-0.146067,0.,0.,-0.146067); Matrix A46 = Matrix_(2,2,-0.146067,0.,0.,-0.146067);
GaussianBayesNet expected4; GaussianBayesNet expected4;
push_front(expected4,"x4", zero(2), eye(2), "x6", A46, sigma4); push_front(expected4,"x4", zero(2), eye(2)/sigma4, "x6", A46/sigma4, ones(2));
GaussianISAM::sharedClique C4 = bayesTree["x3"]; GaussianISAM::sharedClique C4 = bayesTree["x3"];
GaussianBayesNet actual4 = C4->shortcut<GaussianFactor>(R); GaussianBayesNet actual4 = C4->shortcut<GaussianFactor>(R);
CHECK(assert_equal(expected4,actual4,1e-4)); CHECK(assert_equal(expected4,actual4,tol));
} }
/* ************************************************************************* * /* ************************************************************************* *
@ -167,40 +169,43 @@ TEST( BayesTree, balanced_smoother_marginals )
// eliminate using a "nested dissection" ordering // eliminate using a "nested dissection" ordering
GaussianBayesNet chordalBayesNet = smoother.eliminate(ordering); GaussianBayesNet chordalBayesNet = smoother.eliminate(ordering);
VectorConfig expectedSolution; VectorConfig expectedSolution;
BOOST_FOREACH(string key, ordering) BOOST_FOREACH(string key, ordering)
expectedSolution.insert(key,zero(2)); expectedSolution.insert(key,zero(2));
VectorConfig actualSolution = optimize(chordalBayesNet); VectorConfig actualSolution = optimize(chordalBayesNet);
CHECK(assert_equal(expectedSolution,actualSolution,1e-4)); CHECK(assert_equal(expectedSolution,actualSolution,tol));
// Create the Bayes tree // Create the Bayes tree
GaussianISAM bayesTree(chordalBayesNet); GaussianISAM bayesTree(chordalBayesNet);
LONGS_EQUAL(4,bayesTree.size()); LONGS_EQUAL(4,bayesTree.size());
double tol=1e-5;
// Check marginal on x1 // Check marginal on x1
GaussianBayesNet expected1 = simpleGaussian("x1", zero(2), sigmax1); GaussianBayesNet expected1 = simpleGaussian("x1", zero(2), sigmax1);
GaussianBayesNet actual1 = bayesTree.marginalBayesNet<GaussianFactor>("x1"); GaussianBayesNet actual1 = bayesTree.marginalBayesNet<GaussianFactor>("x1");
CHECK(assert_equal(expected1,actual1,1e-4)); CHECK(assert_equal(expected1,actual1,tol));
// Check marginal on x2 // Check marginal on x2
GaussianBayesNet expected2 = simpleGaussian("x2", zero(2), sigmax2); double sigx2 = 0.68712938; // FIXME: this should be corrected analytically
GaussianBayesNet expected2 = simpleGaussian("x2", zero(2), sigx2);
GaussianBayesNet actual2 = bayesTree.marginalBayesNet<GaussianFactor>("x2"); GaussianBayesNet actual2 = bayesTree.marginalBayesNet<GaussianFactor>("x2");
CHECK(assert_equal(expected2,actual2,1e-4)); CHECK(assert_equal(expected2,actual2,tol)); // FAILS
// Check marginal on x3 // Check marginal on x3
GaussianBayesNet expected3 = simpleGaussian("x3", zero(2), sigmax3); GaussianBayesNet expected3 = simpleGaussian("x3", zero(2), sigmax3);
GaussianBayesNet actual3 = bayesTree.marginalBayesNet<GaussianFactor>("x3"); GaussianBayesNet actual3 = bayesTree.marginalBayesNet<GaussianFactor>("x3");
CHECK(assert_equal(expected3,actual3,1e-4)); CHECK(assert_equal(expected3,actual3,tol));
// Check marginal on x4 // Check marginal on x4
GaussianBayesNet expected4 = simpleGaussian("x4", zero(2), sigmax4); GaussianBayesNet expected4 = simpleGaussian("x4", zero(2), sigmax4);
GaussianBayesNet actual4 = bayesTree.marginalBayesNet<GaussianFactor>("x4"); GaussianBayesNet actual4 = bayesTree.marginalBayesNet<GaussianFactor>("x4");
CHECK(assert_equal(expected4,actual4,1e-4)); CHECK(assert_equal(expected4,actual4,tol));
// Check marginal on x7 (should be equal to x1) // Check marginal on x7 (should be equal to x1)
GaussianBayesNet expected7 = simpleGaussian("x7", zero(2), sigmax7); GaussianBayesNet expected7 = simpleGaussian("x7", zero(2), sigmax7);
GaussianBayesNet actual7 = bayesTree.marginalBayesNet<GaussianFactor>("x7"); GaussianBayesNet actual7 = bayesTree.marginalBayesNet<GaussianFactor>("x7");
CHECK(assert_equal(expected7,actual7,1e-4)); CHECK(assert_equal(expected7,actual7,tol));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -219,19 +224,19 @@ TEST( BayesTree, balanced_smoother_shortcuts )
GaussianBayesNet empty; GaussianBayesNet empty;
GaussianISAM::sharedClique R = bayesTree.root(); GaussianISAM::sharedClique R = bayesTree.root();
GaussianBayesNet actual1 = R->shortcut<GaussianFactor>(R); GaussianBayesNet actual1 = R->shortcut<GaussianFactor>(R);
CHECK(assert_equal(empty,actual1,1e-4)); CHECK(assert_equal(empty,actual1,tol));
// Check the conditional P(C2|Root) // Check the conditional P(C2|Root)
GaussianISAM::sharedClique C2 = bayesTree["x3"]; GaussianISAM::sharedClique C2 = bayesTree["x3"];
GaussianBayesNet actual2 = C2->shortcut<GaussianFactor>(R); GaussianBayesNet actual2 = C2->shortcut<GaussianFactor>(R);
CHECK(assert_equal(empty,actual2,1e-4)); CHECK(assert_equal(empty,actual2,tol));
// Check the conditional P(C3|Root), which should be equal to P(x2|x4) // Check the conditional P(C3|Root), which should be equal to P(x2|x4)
GaussianConditional::shared_ptr p_x2_x4 = chordalBayesNet["x2"]; GaussianConditional::shared_ptr p_x2_x4 = chordalBayesNet["x2"];
GaussianBayesNet expected3; expected3.push_back(p_x2_x4); GaussianBayesNet expected3; expected3.push_back(p_x2_x4);
GaussianISAM::sharedClique C3 = bayesTree["x1"]; GaussianISAM::sharedClique C3 = bayesTree["x1"];
GaussianBayesNet actual3 = C3->shortcut<GaussianFactor>(R); GaussianBayesNet actual3 = C3->shortcut<GaussianFactor>(R);
CHECK(assert_equal(expected3,actual3,1e-4)); CHECK(assert_equal(expected3,actual3,tol));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -247,14 +252,13 @@ TEST( BayesTree, balanced_smoother_clique_marginals )
GaussianISAM bayesTree(chordalBayesNet); GaussianISAM bayesTree(chordalBayesNet);
// Check the clique marginal P(C3) // Check the clique marginal P(C3)
GaussianBayesNet expected = simpleGaussian("x2",zero(2),sigmax2); double sigmax2_alt = 1/1.45533; // THIS NEEDS TO BE CHECKED!
Vector sigma = repeat(2, 0.707107); GaussianBayesNet expected = simpleGaussian("x2",zero(2),sigmax2_alt);
Matrix A12 = (-0.5)*eye(2); push_front(expected,"x1", zero(2), eye(2)*sqrt(2), "x2", -eye(2)*sqrt(2)/2, ones(2));
push_front(expected,"x1", zero(2), eye(2), "x2", A12, sigma);
GaussianISAM::sharedClique R = bayesTree.root(), C3 = bayesTree["x1"]; GaussianISAM::sharedClique R = bayesTree.root(), C3 = bayesTree["x1"];
FactorGraph<GaussianFactor> marginal = C3->marginal<GaussianFactor>(R); FactorGraph<GaussianFactor> marginal = C3->marginal<GaussianFactor>(R);
GaussianBayesNet actual = eliminate<GaussianFactor,GaussianConditional>(marginal,C3->keys()); GaussianBayesNet actual = eliminate<GaussianFactor,GaussianConditional>(marginal,C3->keys());
CHECK(assert_equal(expected,actual,1e-4)); CHECK(assert_equal(expected,actual,tol));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -265,41 +269,58 @@ TEST( BayesTree, balanced_smoother_joint )
Ordering ordering; Ordering ordering;
ordering += "x1","x3","x5","x7","x2","x6","x4"; ordering += "x1","x3","x5","x7","x2","x6","x4";
// Create the Bayes tree // Create the Bayes tree, expected to look like:
// x5 x6 x4
// x3 x2 : x4
// x1 : x2
// x7 : x6
GaussianBayesNet chordalBayesNet = smoother.eliminate(ordering); GaussianBayesNet chordalBayesNet = smoother.eliminate(ordering);
GaussianISAM bayesTree(chordalBayesNet); GaussianISAM bayesTree(chordalBayesNet);
// Conditional density elements reused by both tests // Conditional density elements reused by both tests
Vector sigma = repeat(2, 0.786146); const Vector sigma = ones(2);
Matrix I = eye(2), A = -0.00429185*I; const Matrix I = eye(2), A = -0.00429185*I;
// Check the joint density P(x1,x7) factored as P(x1|x7)P(x7) // Check the joint density P(x1,x7) factored as P(x1|x7)P(x7)
GaussianBayesNet expected1 = simpleGaussian("x7", zero(2), sigmax7); GaussianBayesNet expected1;
push_front(expected1,"x1", zero(2), I, "x7", A, sigma); // Why does the sign get flipped on the prior?
GaussianConditional::shared_ptr
parent1(new GaussianConditional("x7", zero(2), -1*I/sigmax7, ones(2)));
expected1.push_front(parent1);
push_front(expected1,"x1", zero(2), I/sigmax7, "x7", A/sigmax7, sigma);
GaussianBayesNet actual1 = bayesTree.jointBayesNet<GaussianFactor>("x1","x7"); GaussianBayesNet actual1 = bayesTree.jointBayesNet<GaussianFactor>("x1","x7");
CHECK(assert_equal(expected1,actual1,1e-4)); CHECK(assert_equal(expected1,actual1,tol));
// Check the joint density P(x7,x1) factored as P(x7|x1)P(x1) // Check the joint density P(x7,x1) factored as P(x7|x1)P(x1)
GaussianBayesNet expected2 = simpleGaussian("x1", zero(2), sigmax1); GaussianBayesNet expected2;
push_front(expected2,"x7", zero(2), I, "x1", A, sigma); GaussianConditional::shared_ptr
parent2(new GaussianConditional("x1", zero(2), -1*I/sigmax1, ones(2)));
expected2.push_front(parent2);
push_front(expected2,"x7", zero(2), I/sigmax1, "x1", A/sigmax1, sigma);
GaussianBayesNet actual2 = bayesTree.jointBayesNet<GaussianFactor>("x7","x1"); GaussianBayesNet actual2 = bayesTree.jointBayesNet<GaussianFactor>("x7","x1");
CHECK(assert_equal(expected2,actual2,1e-4)); CHECK(assert_equal(expected2,actual2,tol));
// Check the joint density P(x1,x4), i.e. with a root variable // Check the joint density P(x1,x4), i.e. with a root variable
GaussianBayesNet expected3 = simpleGaussian("x4", zero(2), sigmax4); GaussianBayesNet expected3;
Vector sigma14 = repeat(2, 0.784465); GaussianConditional::shared_ptr
Matrix A14 = -0.0769231*I; parent3(new GaussianConditional("x4", zero(2), I/sigmax4, ones(2)));
push_front(expected3,"x1", zero(2), I, "x4", A14, sigma14); expected3.push_front(parent3);
double sig14 = 0.784465;
Matrix A14 = -0.0769231*I;
push_front(expected3,"x1", zero(2), I/sig14, "x4", A14/sig14, sigma);
GaussianBayesNet actual3 = bayesTree.jointBayesNet<GaussianFactor>("x1","x4"); GaussianBayesNet actual3 = bayesTree.jointBayesNet<GaussianFactor>("x1","x4");
CHECK(assert_equal(expected3,actual3,1e-4)); CHECK(assert_equal(expected3,actual3,tol));
// Check the joint density P(x4,x1), i.e. with a root variable, factored the other way // Check the joint density P(x4,x1), i.e. with a root variable, factored the other way
GaussianBayesNet expected4 = simpleGaussian("x1", zero(2), sigmax1); GaussianBayesNet expected4;
Vector sigma41 = repeat(2, 0.668096); GaussianConditional::shared_ptr
Matrix A41 = -0.055794*I; parent4(new GaussianConditional("x1", zero(2), -1.0*I/sigmax1, ones(2)));
push_front(expected4,"x4", zero(2), I, "x1", A41, sigma41); expected4.push_front(parent4);
double sig41 = 0.668096;
Matrix A41 = -0.055794*I;
push_front(expected4,"x4", zero(2), I/sig41, "x1", A41/sig41, sigma);
GaussianBayesNet actual4 = bayesTree.jointBayesNet<GaussianFactor>("x4","x1"); GaussianBayesNet actual4 = bayesTree.jointBayesNet<GaussianFactor>("x4","x1");
CHECK(assert_equal(expected4,actual4,1e-4)); CHECK(assert_equal(expected4,actual4,tol));
} }
/* ************************************************************************* */ /* ************************************************************************* */

6
cpp/testGaussianISAM2.cpp
View File

@ -22,6 +22,8 @@ using namespace std;
using namespace gtsam; using namespace gtsam;
using namespace example; using namespace example;
const double tol = 1e-4;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( ISAM2, solving ) TEST( ISAM2, solving )
{ {
@ -34,10 +36,10 @@ TEST( ISAM2, solving )
GaussianISAM2 btree(nlfg, ordering, noisy); GaussianISAM2 btree(nlfg, ordering, noisy);
VectorConfig actualDelta = optimize2(btree); VectorConfig actualDelta = optimize2(btree);
VectorConfig delta = createCorrectDelta(); VectorConfig delta = createCorrectDelta();
CHECK(assert_equal(delta, actualDelta)); CHECK(assert_equal(delta, actualDelta, 0.01));
Config actualSolution = noisy.expmap(actualDelta); Config actualSolution = noisy.expmap(actualDelta);
Config solution = createConfig(); Config solution = createConfig();
CHECK(assert_equal(solution, actualSolution)); CHECK(assert_equal(solution, actualSolution, tol));
} }
/* ************************************************************************* */ /* ************************************************************************* */

2
cpp/testInference.cpp
View File

@ -47,7 +47,7 @@ TEST( Inference, marginals )
BayesNet<GaussianConditional> actual = eliminate<GaussianFactor,GaussianConditional>(fg,keys); BayesNet<GaussianConditional> actual = eliminate<GaussianFactor,GaussianConditional>(fg,keys);
// expected is just scalar Gaussian on x // expected is just scalar Gaussian on x
GaussianBayesNet expected = scalarGaussian("x",4,sqrt(2)); GaussianBayesNet expected = scalarGaussian("x", 4, sqrt(2));
CHECK(assert_equal(expected,actual)); CHECK(assert_equal(expected,actual));
} }

10
cpp/testIterative.cpp
View File

@ -10,7 +10,7 @@ using namespace boost::assign;
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
// TODO: DANGEROUS, create shared pointers // TODO: DANGEROUS, create shared pointers
#define GTSAM_DANGEROUS_GAUSSIAN 3 #define GTSAM_MAGIC_GAUSSIAN 3
#define GTSAM_MAGIC_KEY #define GTSAM_MAGIC_KEY
#include "Ordering.h" #include "Ordering.h"
@ -114,8 +114,8 @@ TEST( Iterative, conjugateGradientDescent_soft_constraint )
config.insert(2, Pose2(1.5,0.,0.)); config.insert(2, Pose2(1.5,0.,0.));
Pose2Graph graph; Pose2Graph graph;
graph.addPrior(1, Pose2(0.,0.,0.), sharedSigma(3, 1e-10)); graph.addPrior(1, Pose2(0.,0.,0.), Isotropic::Sigma(3, 1e-10));
graph.addConstraint(1,2, Pose2(1.,0.,0.), sharedSigma(3, 1)); graph.addConstraint(1,2, Pose2(1.,0.,0.), Isotropic::Sigma(3, 1));
VectorConfig zeros; VectorConfig zeros;
zeros.insert("x1",zero(3)); zeros.insert("x1",zero(3));
@ -140,8 +140,8 @@ TEST( Iterative, subgraphPCG )
theta_bar.insert(2, Pose2(1.5,0.,0.)); theta_bar.insert(2, Pose2(1.5,0.,0.));
Pose2Graph graph; Pose2Graph graph;
graph.addPrior(1, Pose2(0.,0.,0.), sharedSigma(3, 1e-10)); graph.addPrior(1, Pose2(0.,0.,0.), Isotropic::Sigma(3, 1e-10));
graph.addConstraint(1,2, Pose2(1.,0.,0.), sharedSigma(3, 1)); graph.addConstraint(1,2, Pose2(1.,0.,0.), Isotropic::Sigma(3, 1));
VectorConfig zeros; VectorConfig zeros;
zeros.insert("x1",zero(3)); zeros.insert("x1",zero(3));

109
cpp/testMatrix.cpp
View File

@ -502,32 +502,33 @@ TEST( matrix, backsubtitution )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, houseHolder ) TEST( matrix, houseHolder )
{ {
double data[] = {-5, 0, 5, 0, 0, 0, -1, double data[] = {
00, -5, 0, 5, 0, 0, 1.5, -5, 0, 5, 0, 0, 0, -1,
10, 0, 0, 0,-10, 0, 2, 00, -5, 0, 5, 0, 0, 1.5,
00, 10, 0, 0, 0,-10, -1}; 10, 0, 0, 0,-10, 0, 2,
00, 10, 0, 0, 0,-10, -1};
// check in-place householder, with v vectors below diagonal // check in-place householder, with v vectors below diagonal
double data1[] = { double data1[] = {
11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236,
0, 11.1803, 0, -2.2361, 0, -8.9443, -1.565, 0, 11.1803, 0, -2.2361, 0, -8.9443, -1.565,
-0.618034, 0, 4.4721, 0, -4.4721, 0, 0, -0.618034, 0, 4.4721, 0, -4.4721, 0, 0,
0, -0.618034, 0, 4.4721, 0, -4.4721, 0.894 }; 0, -0.618034, 0, 4.4721, 0, -4.4721, 0.894 };
Matrix expected1 = Matrix_(4,7, data1); Matrix expected1 = Matrix_(4,7, data1);
Matrix A1 = Matrix_(4, 7, data); Matrix A1 = Matrix_(4, 7, data);
householder_(A1,3); householder_(A1,3);
CHECK(assert_equal(expected1, A1, 1e-3)); CHECK(assert_equal(expected1, A1, 1e-3));
// in-place, with zeros below diagonal // in-place, with zeros below diagonal
double data2[] = { double data2[] = {
11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236,
0, 11.1803, 0, -2.2361, 0, -8.9443, -1.565, 0, 11.1803, 0, -2.2361, 0, -8.9443, -1.565,
0, 0, 4.4721, 0, -4.4721, 0, 0, 0, 0, 4.4721, 0, -4.4721, 0, 0,
0, 0, 0, 4.4721, 0, -4.4721, 0.894 }; 0, 0, 0, 4.4721, 0, -4.4721, 0.894 };
Matrix expected = Matrix_(4,7, data2); Matrix expected = Matrix_(4,7, data2);
Matrix A2 = Matrix_(4, 7, data); Matrix A2 = Matrix_(4, 7, data);
householder(A2,3); householder(A2,3);
CHECK(assert_equal(expected, A2, 1e-3)); CHECK(assert_equal(expected, A2, 1e-3));
} }
/* ************************************************************************* */ /* ************************************************************************* */
// unit test for qr factorization (and hence householder) // unit test for qr factorization (and hence householder)
@ -535,39 +536,39 @@ TEST( matrix, houseHolder )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, qr ) TEST( matrix, qr )
{ {
double data[] = {-5, 0, 5, 0, double data[] = {-5, 0, 5, 0,
00, -5, 0, 5, 00, -5, 0, 5,
10, 0, 0, 0, 10, 0, 0, 0,
00, 10, 0, 0, 00, 10, 0, 0,
00, 0, 0,-10, 00, 0, 0,-10,
10, 0,-10, 0}; 10, 0,-10, 0};
Matrix A = Matrix_(6, 4, data); Matrix A = Matrix_(6, 4, data);
double dataQ[] = { double dataQ[] = {
-0.3333, 0, 0.2981, 0, 0, -0.8944, -0.3333, 0, 0.2981, 0, 0, -0.8944,
0000000, -0.4472, 0, 0.3651, -0.8165, 0, 0000000, -0.4472, 0, 0.3651, -0.8165, 0,
00.6667, 0, 0.7454, 0, 0, 0, 00.6667, 0, 0.7454, 0, 0, 0,
0000000, 0.8944, 0, 0.1826, -0.4082, 0, 0000000, 0.8944, 0, 0.1826, -0.4082, 0,
0000000, 0, 0, -0.9129, -0.4082, 0, 0000000, 0, 0, -0.9129, -0.4082, 0,
00.6667, 0, -0.5963, 0, 0, -0.4472, 00.6667, 0, -0.5963, 0, 0, -0.4472,
}; };
Matrix expectedQ = Matrix_(6,6, dataQ); Matrix expectedQ = Matrix_(6,6, dataQ);
double dataR[] = { double dataR[] = {
15, 0, -8.3333, 0, 15, 0, -8.3333, 0,
00, 11.1803, 0, -2.2361, 00, 11.1803, 0, -2.2361,
00, 0, 7.4536, 0, 00, 0, 7.4536, 0,
00, 0, 0, 10.9545, 00, 0, 0, 10.9545,
00, 0, 0, 0, 00, 0, 0, 0,
00, 0, 0, 0, 00, 0, 0, 0,
}; };
Matrix expectedR = Matrix_(6,4, dataR); Matrix expectedR = Matrix_(6,4, dataR);
Matrix Q,R; Matrix Q,R;
boost::tie(Q,R) = qr(A); boost::tie(Q,R) = qr(A);
CHECK(assert_equal(expectedQ, Q, 1e-4)); CHECK(assert_equal(expectedQ, Q, 1e-4));
CHECK(assert_equal(expectedR, R, 1e-4)); CHECK(assert_equal(expectedR, R, 1e-4));
CHECK(assert_equal(A, Q*R, 1e-14)); CHECK(assert_equal(A, Q*R, 1e-14));
} }
/* ************************************************************************* */ /* ************************************************************************* */

80
cpp/testNoiseModel.cpp
View File

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

2
cpp/testNonlinearConstraint.cpp
View File

@ -197,7 +197,7 @@ TEST( NonlinearConstraint2, binary_scalar_linearize ) {
x1, Matrix_(1,1, -1.0), x1, Matrix_(1,1, -1.0),
Vector_(1, 6.0), constraintModel); Vector_(1, 6.0), constraintModel);
CHECK(assert_equal(*actualFactor, expectedFactor)); CHECK(assert_equal(*actualFactor, expectedFactor));
CHECK(assert_equal(*actualConstraint, expectedConstraint)); //FAILS - wrong b value CHECK(assert_equal(*actualConstraint, expectedConstraint));
} }
/* ************************************************************************* */ /* ************************************************************************* */

36
cpp/testNonlinearFactor.cpp
View File

@ -85,16 +85,15 @@ TEST( NonlinearFactor, NonlinearFactor )
DOUBLES_EQUAL(expected,actual,0.00000001); DOUBLES_EQUAL(expected,actual,0.00000001);
} }
/* ************************************************************************* * /* ************************************************************************* */
TEST( NonlinearFactor, linearize_f1 ) TEST( NonlinearFactor, linearize_f1 )
{ {
// Grab a non-linear factor // Grab a non-linear factor
Graph nfg = createNonlinearFactorGraph(); Graph nfg = createNonlinearFactorGraph();
boost::shared_ptr<NonlinearFactor1> nlf = Graph::sharedFactor nlf = nfg[0];
boost::static_pointer_cast<NonlinearFactor1>(nfg[0]);
// We linearize at noisy config from SmallExample // We linearize at noisy config from SmallExample
VectorConfig c = createNoisyConfig(); Config c = createNoisyConfig();
GaussianFactor::shared_ptr actual = nlf->linearize(c); GaussianFactor::shared_ptr actual = nlf->linearize(c);
GaussianFactorGraph lfg = createGaussianFactorGraph(); GaussianFactorGraph lfg = createGaussianFactorGraph();
@ -104,61 +103,58 @@ TEST( NonlinearFactor, linearize_f1 )
// The error |A*dx-b| approximates (h(x0+dx)-z) = -error_vector // The error |A*dx-b| approximates (h(x0+dx)-z) = -error_vector
// Hence i.e., b = approximates z-h(x0) = error_vector(x0) // Hence i.e., b = approximates z-h(x0) = error_vector(x0)
CHECK(assert_equal(nlf->error_vector(c),actual->get_b())); //CHECK(assert_equal(nlf->error_vector(c),actual->get_b()));
} }
/* ************************************************************************* * /* ************************************************************************* */
TEST( NonlinearFactor, linearize_f2 ) TEST( NonlinearFactor, linearize_f2 )
{ {
// Grab a non-linear factor // Grab a non-linear factor
Graph nfg = createNonlinearFactorGraph(); Graph nfg = createNonlinearFactorGraph();
boost::shared_ptr<NonlinearFactor1> nlf = Graph::sharedFactor nlf = nfg[1];
boost::static_pointer_cast<NonlinearFactor1>(nfg[1]);
// We linearize at noisy config from SmallExample // We linearize at noisy config from SmallExample
VectorConfig c = createNoisyConfig(); Config c = createNoisyConfig();
GaussianFactor::shared_ptr actual = nlf->linearize(c); GaussianFactor::shared_ptr actual = nlf->linearize(c);
GaussianFactorGraph lfg = createGaussianFactorGraph(); GaussianFactorGraph lfg = createGaussianFactorGraph();
GaussianFactor::shared_ptr expected = lfg[1]; GaussianFactor::shared_ptr expected = lfg[1];
CHECK(expected->equals(*actual)); CHECK(assert_equal(*expected,*actual));
} }
/* ************************************************************************* * /* ************************************************************************* */
TEST( NonlinearFactor, linearize_f3 ) TEST( NonlinearFactor, linearize_f3 )
{ {
// Grab a non-linear factor // Grab a non-linear factor
Graph nfg = createNonlinearFactorGraph(); Graph nfg = createNonlinearFactorGraph();
boost::shared_ptr<NonlinearFactor1> nlf = Graph::sharedFactor nlf = nfg[2];
boost::static_pointer_cast<NonlinearFactor1>(nfg[2]);
// We linearize at noisy config from SmallExample // We linearize at noisy config from SmallExample
VectorConfig c = createNoisyConfig(); Config c = createNoisyConfig();
GaussianFactor::shared_ptr actual = nlf->linearize(c); GaussianFactor::shared_ptr actual = nlf->linearize(c);
GaussianFactorGraph lfg = createGaussianFactorGraph(); GaussianFactorGraph lfg = createGaussianFactorGraph();
GaussianFactor::shared_ptr expected = lfg[2]; GaussianFactor::shared_ptr expected = lfg[2];
CHECK(expected->equals(*actual)); CHECK(assert_equal(*expected,*actual));
} }
/* ************************************************************************* * /* ************************************************************************* */
TEST( NonlinearFactor, linearize_f4 ) TEST( NonlinearFactor, linearize_f4 )
{ {
// Grab a non-linear factor // Grab a non-linear factor
Graph nfg = createNonlinearFactorGraph(); Graph nfg = createNonlinearFactorGraph();
boost::shared_ptr<NonlinearFactor1> nlf = Graph::sharedFactor nlf = nfg[3];
boost::static_pointer_cast<NonlinearFactor1>(nfg[3]);
// We linearize at noisy config from SmallExample // We linearize at noisy config from SmallExample
VectorConfig c = createNoisyConfig(); Config c = createNoisyConfig();
GaussianFactor::shared_ptr actual = nlf->linearize(c); GaussianFactor::shared_ptr actual = nlf->linearize(c);
GaussianFactorGraph lfg = createGaussianFactorGraph(); GaussianFactorGraph lfg = createGaussianFactorGraph();
GaussianFactor::shared_ptr expected = lfg[3]; GaussianFactor::shared_ptr expected = lfg[3];
CHECK(expected->equals(*actual)); CHECK(assert_equal(*expected,*actual));
} }
/* ************************************************************************* */ /* ************************************************************************* */

5
cpp/testNonlinearFactorGraph.cpp
View File

@ -68,15 +68,14 @@ TEST( Graph, probPrime )
DOUBLES_EQUAL(expected,actual,0); DOUBLES_EQUAL(expected,actual,0);
} }
/* ************************************************************************* * /* ************************************************************************* */
// TODO: Commented out until noise model is passed to Gaussian factor graph
TEST( Graph, linearize ) TEST( Graph, linearize )
{ {
Graph fg = createNonlinearFactorGraph(); Graph fg = createNonlinearFactorGraph();
Config initial = createNoisyConfig(); Config initial = createNoisyConfig();
GaussianFactorGraph linearized = fg.linearize(initial); GaussianFactorGraph linearized = fg.linearize(initial);
GaussianFactorGraph expected = createGaussianFactorGraph(); GaussianFactorGraph expected = createGaussianFactorGraph();
CHECK(assert_equal(expected,linearized)); CHECK(assert_equal(expected,linearized)); // Needs correct linearizations
} }
/* ************************************************************************* */ /* ************************************************************************* */

30
cpp/testNonlinearOptimizer.cpp
View File

@ -32,6 +32,9 @@ using namespace boost;
using namespace gtsam; using namespace gtsam;
using namespace example; using namespace example;
// FIXME: this tolerance is too high - something is wrong with the noisemodel
const double tol = 1e-6;
typedef NonlinearOptimizer<Graph,Config> Optimizer; typedef NonlinearOptimizer<Graph,Config> Optimizer;
/* ************************************************************************* */ /* ************************************************************************* */
@ -57,12 +60,14 @@ TEST( NonlinearOptimizer, delta )
dx2(1) = -0.2; dx2(1) = -0.2;
expected.insert("x2", dx2); expected.insert("x2", dx2);
Optimizer::shared_solver solver;
// Check one ordering // Check one ordering
shared_ptr<Ordering> ord1(new Ordering()); shared_ptr<Ordering> ord1(new Ordering());
*ord1 += "x2","l1","x1"; *ord1 += "x2","l1","x1";
Optimizer::shared_solver solver;
solver = Optimizer::shared_solver(new Optimizer::solver(ord1)); solver = Optimizer::shared_solver(new Optimizer::solver(ord1));
Optimizer optimizer1(fg, initial, solver); Optimizer optimizer1(fg, initial, solver);
VectorConfig actual1 = optimizer1.linearizeAndOptimizeForDelta(); VectorConfig actual1 = optimizer1.linearizeAndOptimizeForDelta();
CHECK(assert_equal(actual1,expected)); CHECK(assert_equal(actual1,expected));
@ -71,6 +76,7 @@ TEST( NonlinearOptimizer, delta )
*ord2 += "x1","x2","l1"; *ord2 += "x1","x2","l1";
solver = Optimizer::shared_solver(new Optimizer::solver(ord2)); solver = Optimizer::shared_solver(new Optimizer::solver(ord2));
Optimizer optimizer2(fg, initial, solver); Optimizer optimizer2(fg, initial, solver);
VectorConfig actual2 = optimizer2.linearizeAndOptimizeForDelta(); VectorConfig actual2 = optimizer2.linearizeAndOptimizeForDelta();
CHECK(assert_equal(actual2,expected)); CHECK(assert_equal(actual2,expected));
@ -79,8 +85,18 @@ TEST( NonlinearOptimizer, delta )
*ord3 += "l1","x1","x2"; *ord3 += "l1","x1","x2";
solver = Optimizer::shared_solver(new Optimizer::solver(ord3)); solver = Optimizer::shared_solver(new Optimizer::solver(ord3));
Optimizer optimizer3(fg, initial, solver); Optimizer optimizer3(fg, initial, solver);
VectorConfig actual3 = optimizer3.linearizeAndOptimizeForDelta(); VectorConfig actual3 = optimizer3.linearizeAndOptimizeForDelta();
CHECK(assert_equal(actual3,expected)); CHECK(assert_equal(actual3,expected));
// More...
shared_ptr<Ordering> ord4(new Ordering());
*ord4 += "x1","x2", "l1";
solver = Optimizer::shared_solver(new Optimizer::solver(ord4));
Optimizer optimizer4(fg, initial, solver);
VectorConfig actual4 = optimizer4.linearizeAndOptimizeForDelta();
CHECK(assert_equal(actual4,expected));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -152,12 +168,12 @@ TEST( NonlinearOptimizer, optimize )
// Gauss-Newton // Gauss-Newton
Optimizer actual1 = optimizer.gaussNewton(relativeThreshold, Optimizer actual1 = optimizer.gaussNewton(relativeThreshold,
absoluteThreshold); absoluteThreshold);
DOUBLES_EQUAL(0,fg->error(*(actual1.config())),1e-3); DOUBLES_EQUAL(0,fg->error(*(actual1.config())),tol);
// Levenberg-Marquardt // Levenberg-Marquardt
Optimizer actual2 = optimizer.levenbergMarquardt(relativeThreshold, Optimizer actual2 = optimizer.levenbergMarquardt(relativeThreshold,
absoluteThreshold, Optimizer::SILENT); absoluteThreshold, Optimizer::SILENT);
DOUBLES_EQUAL(0,fg->error(*(actual2.config())),1e-3); DOUBLES_EQUAL(0,fg->error(*(actual2.config())),tol);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -170,8 +186,8 @@ TEST( NonlinearOptimizer, Factorization )
config->insert(2, Pose2(1.5,0.,0.)); config->insert(2, Pose2(1.5,0.,0.));
boost::shared_ptr<Pose2Graph> graph(new Pose2Graph); boost::shared_ptr<Pose2Graph> graph(new Pose2Graph);
graph->addPrior(1, Pose2(0.,0.,0.), sharedSigma(3, 1e-10)); graph->addPrior(1, Pose2(0.,0.,0.), Isotropic::Sigma(3, 1e-10));
graph->addConstraint(1,2, Pose2(1.,0.,0.), sharedSigma(3, 1)); graph->addConstraint(1,2, Pose2(1.,0.,0.), Isotropic::Sigma(3, 1));
boost::shared_ptr<Ordering> ordering(new Ordering); boost::shared_ptr<Ordering> ordering(new Ordering);
ordering->push_back(Pose2Config::Key(1)); ordering->push_back(Pose2Config::Key(1));
@ -197,8 +213,8 @@ TEST( NonlinearOptimizer, SubgraphPCG )
config->insert(2, Pose2(1.5,0.,0.)); config->insert(2, Pose2(1.5,0.,0.));
boost::shared_ptr<Pose2Graph> graph(new Pose2Graph); boost::shared_ptr<Pose2Graph> graph(new Pose2Graph);
graph->addPrior(1, Pose2(0.,0.,0.), sharedSigma(3, 1e-10)); graph->addPrior(1, Pose2(0.,0.,0.), Isotropic::Sigma(3, 1e-10));
graph->addConstraint(1,2, Pose2(1.,0.,0.), sharedSigma(3, 1)); graph->addConstraint(1,2, Pose2(1.,0.,0.), Isotropic::Sigma(3, 1));
double relativeThreshold = 1e-5; double relativeThreshold = 1e-5;
double absoluteThreshold = 1e-5; double absoluteThreshold = 1e-5;

2
cpp/testSQPOptimizer.cpp
View File

@ -372,7 +372,7 @@ TEST ( SQPOptimizer, inequality_avoid_iterative ) {
// verify // verify
Config2D exp2(feasible); Config2D exp2(feasible);
exp2.insert(x2, Point2(5.0, 0.5)); exp2.insert(x2, Point2(5.0, 0.5));
CHECK(assert_equal(exp2, *(final.config()))); // FAILS CHECK(assert_equal(exp2, *(final.config())));
} }
/* ************************************************************************* */ /* ************************************************************************* */

1
cpp/timeGaussianFactor.cpp
View File

@ -27,6 +27,7 @@ using namespace boost::assign;
* Alex's Machine * Alex's Machine
* Results for Eliminate: * Results for Eliminate:
* Initial (1891): 17.91 sec, 55834.7 calls/sec * Initial (1891): 17.91 sec, 55834.7 calls/sec
* NoiseQR : 12.58 sec
* *
* Results for matrix_augmented: * Results for matrix_augmented:
* Initial (1891) : 0.85 sec, 1.17647e+06 calls/sec * Initial (1891) : 0.85 sec, 1.17647e+06 calls/sec

2
cpp/timeGaussianFactorGraph.cpp
View File

@ -75,6 +75,8 @@ TEST(timeGaussianFactorGraph, planar)
// Improved (int->size_t) // Improved (int->size_t)
// (N = 100) : 15.39 // (N = 100) : 15.39
// Using GSL/BLAS for updateAb : 12.87 // Using GSL/BLAS for updateAb : 12.87
// Using NoiseQR : 16.33
// Using correct system : 10.00
// Switch to 100*100 grid = 10K poses // Switch to 100*100 grid = 10K poses
// 1879: 15.6498 15.3851 15.5279 // 1879: 15.6498 15.3851 15.5279