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

case change: SharedGaussian and SharedDiagonal are now classes with their own header file. Needed for MATLAB TORO hail Mary

release/4.3a0
Frank Dellaert 2010-01-22 17:36:57 +00:00
parent 490791cd48
commit 351cdd18c2
49 changed files with 371 additions and 363 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cpp/BetweenFactor.h
View File

@ -33,7 +33,7 @@ namespace gtsam {
/** Constructor */
BetweenFactor(const Key& key1, const Key& key2, const T& measured,
const sharedGaussian& model) :
const SharedGaussian& model) :
Base(model, key1, key2), measured_(measured) {
}

2
cpp/GaussianFactor.cpp
View File

@ -323,7 +323,7 @@ GaussianFactor::eliminate(const Symbol& key) const
Matrix Ab = matrix_augmented(ordering,false);
// Use in-place QR on dense Ab appropriate to NoiseModel
sharedDiagonal noiseModel = model_->QR(Ab);
SharedDiagonal noiseModel = model_->QR(Ab);
// get dimensions of the eliminated variable
// TODO: this is another map find that should be avoided !

14
cpp/GaussianFactor.h
View File

@ -18,7 +18,7 @@
#include "Factor.h"
#include "Matrix.h"
#include "VectorConfig.h"
#include "NoiseModel.h"
#include "SharedDiagonal.h"
#include "SymbolMap.h"
namespace gtsam {
@ -40,7 +40,7 @@ public:
protected:
sharedDiagonal model_; // Gaussian noise model with diagonal covariance matrix
SharedDiagonal model_; // Gaussian noise model with diagonal covariance matrix
SymbolMap<Matrix> As_; // linear matrices
Vector b_; // right-hand-side
@ -57,7 +57,7 @@ public:
/** Construct unary factor */
GaussianFactor(const Symbol& key1, const Matrix& A1,
const Vector& b, const sharedDiagonal& model) :
const Vector& b, const SharedDiagonal& model) :
b_(b), model_(model) {
As_.insert(make_pair(key1, A1));
}
@ -65,7 +65,7 @@ public:
/** Construct binary factor */
GaussianFactor(const Symbol& key1, const Matrix& A1,
const Symbol& key2, const Matrix& A2,
const Vector& b, const sharedDiagonal& model) :
const Vector& b, const SharedDiagonal& model) :
b_(b), model_(model) {
As_.insert(make_pair(key1, A1));
As_.insert(make_pair(key2, A2));
@ -75,7 +75,7 @@ public:
GaussianFactor(const Symbol& key1, const Matrix& A1,
const Symbol& key2, const Matrix& A2,
const Symbol& key3, const Matrix& A3,
const Vector& b, const sharedDiagonal& model) :
const Vector& b, const SharedDiagonal& model) :
b_(b), model_(model) {
As_.insert(make_pair(key1, A1));
As_.insert(make_pair(key2, A2));
@ -84,7 +84,7 @@ public:
/** Construct an n-ary factor */
GaussianFactor(const std::vector<std::pair<Symbol, Matrix> > &terms,
const Vector &b, const sharedDiagonal& model) :
const Vector &b, const SharedDiagonal& model) :
b_(b), model_(model) {
for(unsigned int i=0; i<terms.size(); i++)
As_.insert(terms[i]);
@ -127,7 +127,7 @@ public:
const Vector& get_sigmas() const { return model_->sigmas(); }
/** get a copy of model */
const sharedDiagonal& get_model() const { return model_; }
const SharedDiagonal& get_model() const { return model_; }
/**
* get a copy of the A matrix from a specific node

2
cpp/GaussianFactorGraph.cpp
View File

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

8
cpp/GaussianFactorGraph.h
View File

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

2
cpp/Makefile.am
View File

@ -88,7 +88,7 @@ testBinaryBayesNet_SOURCES = testBinaryBayesNet.cpp
testBinaryBayesNet_LDADD = libgtsam.la
# Gaussian inference
headers += GaussianFactorSet.h
headers += GaussianFactorSet.h SharedGaussian.h SharedDiagonal.h
sources += NoiseModel.cpp Errors.cpp VectorConfig.cpp GaussianFactor.cpp GaussianFactorGraph.cpp GaussianConditional.cpp GaussianBayesNet.cpp
check_PROGRAMS += testVectorConfig testGaussianFactor testGaussianFactorGraph testGaussianConditional testGaussianBayesNet testNoiseModel
testVectorConfig_SOURCES = testVectorConfig.cpp

5
cpp/NoiseModel.cpp
View File

@ -24,6 +24,7 @@
#include <boost/random/variate_generator.hpp>
#include "NoiseModel.h"
#include "SharedDiagonal.h"
namespace ublas = boost::numeric::ublas;
typedef ublas::matrix_column<Matrix> column;
@ -121,7 +122,7 @@ namespace gtsam {
}
// General QR, see also special version in Constrained
sharedDiagonal Gaussian::QR(Matrix& Ab) const {
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;
@ -207,7 +208,7 @@ namespace gtsam {
// 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 {
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);

45
cpp/NoiseModel.h
View File

@ -15,7 +15,7 @@
namespace gtsam {
class sharedDiagonal; // forward declare, defined at end
class SharedDiagonal; // forward declare, defined at end
namespace noiseModel {
@ -146,7 +146,7 @@ namespace gtsam {
* @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;
virtual SharedDiagonal QR(Matrix& Ab) const;
/**
* Return R itself, but note that Whiten(H) is cheaper than R*H
@ -210,7 +210,7 @@ namespace gtsam {
/**
* Apply QR factorization to the system [A b], taking into account constraints
*/
virtual sharedDiagonal QR(Matrix& Ab) const;
virtual SharedDiagonal QR(Matrix& Ab) const;
/**
* Return standard deviations (sqrt of diagonal)
@ -373,42 +373,5 @@ namespace gtsam {
} // namespace noiseModel
using namespace noiseModel;
// TODO: very ugly, just keep shared pointers and use Sigma/Sigmas everywhere
// A useful convenience class to refer to a shared Gaussian model
// Define GTSAM_MAGIC_GAUSSIAN to desired dimension to have access to slightly
// dangerous and non-shared (inefficient, wasteful) noise models. Only in tests!
struct sharedGaussian : public Gaussian::shared_ptr {
sharedGaussian() {}
sharedGaussian(const Gaussian::shared_ptr& p):Gaussian::shared_ptr(p) {}
sharedGaussian(const Diagonal::shared_ptr& p):Gaussian::shared_ptr(p) {}
sharedGaussian(const Constrained::shared_ptr& p):Gaussian::shared_ptr(p) {}
sharedGaussian(const Isotropic::shared_ptr& p):Gaussian::shared_ptr(p) {}
sharedGaussian(const Unit::shared_ptr& p):Gaussian::shared_ptr(p) {}
#ifdef GTSAM_MAGIC_GAUSSIAN
sharedGaussian(const Matrix& covariance):Gaussian::shared_ptr(Gaussian::Covariance(covariance)) {}
sharedGaussian(const Vector& sigmas):Gaussian::shared_ptr(Diagonal::Sigmas(sigmas)) {}
sharedGaussian(const double& s):Gaussian::shared_ptr(Isotropic::Sigma(GTSAM_MAGIC_GAUSSIAN,s)) {}
#endif
};
// A useful convenience class to refer to a shared Diagonal model
// There are (somewhat dangerous) constructors from Vector and pair<size_t,double>
// that call Sigmas and Sigma, respectively.
struct sharedDiagonal : public Diagonal::shared_ptr {
sharedDiagonal() {}
sharedDiagonal(const Diagonal::shared_ptr& p):Diagonal::shared_ptr(p) {}
sharedDiagonal(const Constrained::shared_ptr& p):Diagonal::shared_ptr(p) {}
sharedDiagonal(const Isotropic::shared_ptr& p):Diagonal::shared_ptr(p) {}
sharedDiagonal(const Unit::shared_ptr& p):Diagonal::shared_ptr(p) {}
sharedDiagonal(const Vector& sigmas):Diagonal::shared_ptr(Diagonal::Sigmas(sigmas)) {}
};
// TODO: make these the ones really used in unit tests
inline sharedDiagonal sharedSigmas(const Vector& sigmas) { return Diagonal::Sigmas(sigmas);}
inline sharedDiagonal sharedSigma(int dim, double sigma) { return Isotropic::Sigma(dim,sigma);}
} // namespace gtsam

8
cpp/NonlinearConstraint-inl.h
View File

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

2
cpp/NonlinearEquality.h
View File

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

10
cpp/NonlinearFactor.h
View File

@ -18,7 +18,7 @@
#include "Factor.h"
#include "Vector.h"
#include "Matrix.h"
#include "NoiseModel.h"
#include "SharedGaussian.h"
#include "GaussianFactor.h"
#define INSTANTIATE_NONLINEAR_FACTOR1(C,J,X) \
@ -43,7 +43,7 @@ namespace gtsam {
typedef NonlinearFactor<Config> This;
sharedGaussian noiseModel_; /** Noise model */
SharedGaussian noiseModel_; /** Noise model */
std::list<Symbol> keys_; /** cached keys */
public:
@ -56,7 +56,7 @@ namespace gtsam {
* Constructor
* @param noiseModel shared pointer to a noise model
*/
NonlinearFactor(const sharedGaussian& noiseModel) :
NonlinearFactor(const SharedGaussian& noiseModel) :
noiseModel_(noiseModel) {
}
@ -148,7 +148,7 @@ namespace gtsam {
* @param z measurement
* @param key by which to look up X value in Config
*/
NonlinearFactor1(const sharedGaussian& noiseModel,
NonlinearFactor1(const SharedGaussian& noiseModel,
const Key& key1) :
Base(noiseModel), key_(key1) {
this->keys_.push_back(key_);
@ -242,7 +242,7 @@ namespace gtsam {
* @param j1 key of the first variable
* @param j2 key of the second variable
*/
NonlinearFactor2(const sharedGaussian& noiseModel, Key1 j1,
NonlinearFactor2(const SharedGaussian& noiseModel, Key1 j1,
Key2 j2) :
Base(noiseModel), key1_(j1), key2_(j2) {
this->keys_.push_back(key1_);

2
cpp/PriorFactor.h
View File

@ -31,7 +31,7 @@ namespace gtsam {
/** Constructor */
PriorFactor(const Key& key, const T& prior,
const sharedGaussian& model) :
const SharedGaussian& model) :
Base(model, key), prior_(prior) {
}

45
cpp/SharedDiagonal.h Normal file
View File

@ -0,0 +1,45 @@
/*
* SharedDiagonal.h
* @brief Class that wraps a shared noise model with diagonal covariance
* @Author: Frank Dellaert
* Created on: Jan 22, 2010
*/
#pragma once
#include "NoiseModel.h"
namespace gtsam { // note, deliberately not in noiseModel namespace
// A useful convenience class to refer to a shared Diagonal model
// There are (somewhat dangerous) constructors from Vector and pair<size_t,double>
// that call Sigmas and Sigma, respectively.
struct SharedDiagonal: public noiseModel::Diagonal::shared_ptr {
SharedDiagonal() {
}
SharedDiagonal(const noiseModel::Diagonal::shared_ptr& p) :
noiseModel::Diagonal::shared_ptr(p) {
}
SharedDiagonal(const noiseModel::Constrained::shared_ptr& p) :
noiseModel::Diagonal::shared_ptr(p) {
}
SharedDiagonal(const noiseModel::Isotropic::shared_ptr& p) :
noiseModel::Diagonal::shared_ptr(p) {
}
SharedDiagonal(const noiseModel::Unit::shared_ptr& p) :
noiseModel::Diagonal::shared_ptr(p) {
}
SharedDiagonal(const Vector& sigmas) :
noiseModel::Diagonal::shared_ptr(noiseModel::Diagonal::Sigmas(sigmas)) {
}
};
// TODO: make these the ones really used in unit tests
inline SharedDiagonal sharedSigmas(const Vector& sigmas) {
return noiseModel::Diagonal::Sigmas(sigmas);
}
inline SharedDiagonal sharedSigma(int dim, double sigma) {
return noiseModel::Isotropic::Sigma(dim, sigma);
}
}

59
cpp/SharedGaussian.h Normal file
View File

@ -0,0 +1,59 @@
/*
* SharedGaussian.h
* @brief Class that wraps a shared noise model with diagonal covariance
* @Author: Frank Dellaert
* Created on: Jan 22, 2010
*/
#pragma once
#include "NoiseModel.h"
namespace gtsam { // note, deliberately not in noiseModel namespace
/**
* A useful convenience class to refer to a shared Gaussian model
* Also needed to make noise models in matlab
*/
struct SharedGaussian: public noiseModel::Gaussian::shared_ptr {
SharedGaussian() {
}
// TODO: better way ?
SharedGaussian(const noiseModel::Gaussian::shared_ptr& p) :
noiseModel::Gaussian::shared_ptr(p) {
}
SharedGaussian(const noiseModel::Diagonal::shared_ptr& p) :
noiseModel::Gaussian::shared_ptr(p) {
}
SharedGaussian(const noiseModel::Constrained::shared_ptr& p) :
noiseModel::Gaussian::shared_ptr(p) {
}
SharedGaussian(const noiseModel::Isotropic::shared_ptr& p) :
noiseModel::Gaussian::shared_ptr(p) {
}
SharedGaussian(const noiseModel::Unit::shared_ptr& p) :
noiseModel::Gaussian::shared_ptr(p) {
}
// Define GTSAM_MAGIC_GAUSSIAN to have access to slightly
// dangerous and non-shared (inefficient, wasteful) noise models.
// Intended to be used only in tests (if you must) and the MATLAB wrapper
#ifdef GTSAM_MAGIC_GAUSSIAN
SharedGaussian(const Matrix& covariance) :
noiseModel::Gaussian::shared_ptr(noiseModel::Gaussian::Covariance(covariance)) {
}
SharedGaussian(const Vector& sigmas) :
noiseModel::Gaussian::shared_ptr(noiseModel::Diagonal::Sigmas(sigmas)) {
}
#endif
// Define GTSAM_DANGEROUS_GAUSSIAN to have access to bug-prone fixed dimension Gaussians
// Not intended for human use, only for backwards compatibility of old unit tests
#ifdef GTSAM_DANGEROUS_GAUSSIAN
SharedGaussian(const double& s) :
noiseModel::Gaussian::shared_ptr(noiseModel::Isotropic::Sigma(
GTSAM_DANGEROUS_GAUSSIAN, s)) {
}
#endif
};
}

4
cpp/Simulated3D.h
View File

@ -49,7 +49,7 @@ namespace simulated3D {
Vector z_;
Point2Prior3D(const Vector& z,
const sharedGaussian& model, const PoseKey& j) :
const SharedGaussian& model, const PoseKey& j) :
NonlinearFactor1<VectorConfig, PoseKey, Vector> (model, j), z_(z) {
}
@ -66,7 +66,7 @@ namespace simulated3D {
Vector z_;
Simulated3DMeasurement(const Vector& z,
const sharedGaussian& model, PoseKey& j1, PointKey j2) :
const SharedGaussian& model, PoseKey& j1, PointKey j2) :
z_(z),
NonlinearFactor2<VectorConfig, PoseKey, Vector, PointKey, Vector> (
model, j1, j2) {

60
cpp/gtsam-broken.h Normal file
View File

@ -0,0 +1,60 @@
// These are currently broken
// Solve by parsing a namespace pose2SLAM::Config and making a Pose2SLAMConfig class
// We also have to solve the shared pointer mess to avoid duplicate methods
class Point2Prior {
Point2Prior(Vector mu, double sigma, string key);
Vector error_vector(const VectorConfig& c) const;
GaussianFactor* linearize(const VectorConfig& c) const;
double sigma();
Vector measurement();
double error(const VectorConfig& c) const;
void print(string s) const;
};
class Simulated2DOdometry {
Simulated2DOdometry(Vector odo, double sigma, string key, string key2);
Vector error_vector(const VectorConfig& c) const;
GaussianFactor* linearize(const VectorConfig& c) const;
double sigma();
Vector measurement();
double error(const VectorConfig& c) const;
void print(string s) const;
};
class Simulated2DMeasurement {
Simulated2DMeasurement(Vector odo, double sigma, string key, string key2);
Vector error_vector(const VectorConfig& c) const;
GaussianFactor* linearize(const VectorConfig& c) const;
double sigma();
Vector measurement();
double error(const VectorConfig& c) const;
void print(string s) const;
};
class Pose2Config{
Pose2Config();
Pose2 get(string key) const;
void insert(string name, const Pose2& val);
void print(string s) const;
void clear();
int size();
};
class Pose2Factor {
Pose2Factor(string key1, string key2,
const Pose2& measured, Matrix measurement_covariance);
void print(string name) const;
double error(const Pose2Config& c) const;
size_t size() const;
GaussianFactor* linearize(const Pose2Config& config) const;
};
class Pose2Graph{
Pose2Graph();
void print(string s) const;
GaussianFactorGraph* linearize_(const Pose2Config& config) const;
void push_back(Pose2Factor* factor);
};

82
cpp/gtsam.h
View File

@ -1,3 +1,12 @@
class SharedGaussian {
SharedGaussian(Matrix covariance);
SharedGaussian(Vector sigmas);
};
class SharedDiagonal {
SharedDiagonal(Vector sigmas);
};
class Ordering {
Ordering();
Ordering(string key);
@ -25,22 +34,17 @@ class VectorConfig {
void clear();
};
class GaussianFactorSet {
GaussianFactorSet();
void push_back(GaussianFactor* factor);
};
class GaussianFactor {
GaussianFactor(string key1,
Matrix A1,
Vector b_in,
double sigma);
const SharedDiagonal& model);
GaussianFactor(string key1,
Matrix A1,
string key2,
Matrix A2,
Vector b_in,
double sigma);
const SharedDiagonal& model);
GaussianFactor(string key1,
Matrix A1,
string key2,
@ -48,7 +52,7 @@ class GaussianFactor {
string key3,
Matrix A3,
Vector b_in,
double sigma);
const SharedDiagonal& model);
bool empty() const;
Vector get_b() const;
Matrix get_A(string key) const;
@ -59,6 +63,11 @@ class GaussianFactor {
pair<Matrix,Vector> matrix(const Ordering& ordering) const;
};
class GaussianFactorSet {
GaussianFactorSet();
void push_back(GaussianFactor* factor);
};
class GaussianConditional {
GaussianConditional();
GaussianConditional(string key,
@ -132,36 +141,6 @@ class Point3 {
void print(string s) const;
};
class Point2Prior {
Point2Prior(Vector mu, double sigma, string key);
Vector error_vector(const VectorConfig& c) const;
GaussianFactor* linearize(const VectorConfig& c) const;
double sigma();
Vector measurement();
double error(const VectorConfig& c) const;
void print(string s) const;
};
class Simulated2DOdometry {
Simulated2DOdometry(Vector odo, double sigma, string key, string key2);
Vector error_vector(const VectorConfig& c) const;
GaussianFactor* linearize(const VectorConfig& c) const;
double sigma();
Vector measurement();
double error(const VectorConfig& c) const;
void print(string s) const;
};
class Simulated2DMeasurement {
Simulated2DMeasurement(Vector odo, double sigma, string key, string key2);
Vector error_vector(const VectorConfig& c) const;
GaussianFactor* linearize(const VectorConfig& c) const;
double sigma();
Vector measurement();
double error(const VectorConfig& c) const;
void print(string s) const;
};
class Pose2 {
Pose2();
Pose2(const Pose2& pose);
@ -179,30 +158,3 @@ class Pose2 {
Point2 t() const;
Rot2 r() const;
};
class Pose2Config{
Pose2Config();
Pose2 get(string key) const;
void insert(string name, const Pose2& val);
void print(string s) const;
void clear();
int size();
};
class Pose2Factor {
Pose2Factor(string key1, string key2,
const Pose2& measured, Matrix measurement_covariance);
void print(string name) const;
double error(const Pose2Config& c) const;
size_t size() const;
GaussianFactor* linearize(const Pose2Config& config) const;
};
class Pose2Graph{
Pose2Graph();
void print(string s) const;
GaussianFactorGraph* linearize_(const Pose2Config& config) const;
void push_back(Pose2Factor* factor);
};

6
cpp/planarSLAM.cpp
View File

@ -26,19 +26,19 @@ namespace gtsam {
}
void Graph::addOdometry(const PoseKey& i, const PoseKey& j, const Pose2& z,
const sharedGaussian& model) {
const SharedGaussian& model) {
sharedFactor factor(new Odometry(i, j, z, model));
push_back(factor);
}
void Graph::addBearing(const PoseKey& i, const PointKey& j, const Rot2& z,
const sharedGaussian& model) {
const SharedGaussian& model) {
sharedFactor factor(new Bearing(i, j, z, model));
push_back(factor);
}
void Graph::addRange(const PoseKey& i, const PointKey& j, double z,
const sharedGaussian& model) {
const SharedGaussian& model) {
sharedFactor factor(new Range(i, j, z, model));
push_back(factor);
}

10
cpp/planarSLAM.h
View File

@ -35,7 +35,7 @@ namespace gtsam {
BearingFactor(); /* Default constructor */
BearingFactor(const PoseKey& i, const PointKey& j, const Rot2& z,
const sharedGaussian& model) :
const SharedGaussian& model) :
Base(model, i, j), z_(z) {
}
@ -64,7 +64,7 @@ namespace gtsam {
RangeFactor(); /* Default constructor */
RangeFactor(const PoseKey& i, const PointKey& j, double z,
const sharedGaussian& model) :
const SharedGaussian& model) :
Base(model, i, j), z_(z) {
}
@ -94,11 +94,11 @@ namespace gtsam {
struct Graph: public NonlinearFactorGraph<Config> {
void addPoseConstraint(const PoseKey& i, const Pose2& p);
void addOdometry(const PoseKey& i, const PoseKey& j, const Pose2& z,
const sharedGaussian& model);
const SharedGaussian& model);
void addBearing(const PoseKey& i, const PointKey& j, const Rot2& z,
const sharedGaussian& model);
const SharedGaussian& model);
void addRange(const PoseKey& i, const PointKey& j, double z,
const sharedGaussian& model);
const SharedGaussian& model);
};
// Optimizer

4
cpp/pose2SLAM.cpp
View File

@ -30,13 +30,13 @@ namespace gtsam {
/* ************************************************************************* */
void Graph::addPrior(const Key& i, const Pose2& p,
const sharedGaussian& model) {
const SharedGaussian& model) {
sharedFactor factor(new Prior(i, p, model));
push_back(factor);
}
void Graph::addConstraint(const Key& i, const Key& j, const Pose2& z,
const sharedGaussian& model) {
const SharedGaussian& model) {
sharedFactor factor(new Constraint(i, j, z, model));
push_back(factor);
}

4
cpp/pose2SLAM.h
View File

@ -42,8 +42,8 @@ namespace gtsam {
struct Graph: public NonlinearFactorGraph<Config> {
typedef BetweenFactor<Config, Key, Pose2> Constraint;
typedef Pose2 Pose;
void addPrior(const Key& i, const Pose2& p, const sharedGaussian& model);
void addConstraint(const Key& i, const Key& j, const Pose2& z, const sharedGaussian& model);
void addPrior(const Key& i, const Pose2& p, const SharedGaussian& model);
void addConstraint(const Key& i, const Key& j, const Pose2& z, const SharedGaussian& model);
void addHardConstraint(const Key& i, const Pose2& p);
};

4
cpp/pose3SLAM.cpp
View File

@ -33,13 +33,13 @@ namespace gtsam {
/* ************************************************************************* */
void Graph::addPrior(const Key& i, const Pose3& p,
const sharedGaussian& model) {
const SharedGaussian& model) {
sharedFactor factor(new Prior(i, p, model));
push_back(factor);
}
void Graph::addConstraint(const Key& i, const Key& j, const Pose3& z,
const sharedGaussian& model) {
const SharedGaussian& model) {
sharedFactor factor(new Constraint(i, j, z, model));
push_back(factor);
}

4
cpp/pose3SLAM.h
View File

@ -41,9 +41,9 @@ namespace gtsam {
// Graph
struct Graph: public NonlinearFactorGraph<Config> {
void addPrior(const Key& i, const Pose3& p,
const sharedGaussian& model);
const SharedGaussian& model);
void addConstraint(const Key& i, const Key& j, const Pose3& z,
const sharedGaussian& model);
const SharedGaussian& model);
void addHardConstraint(const Key& i, const Pose3& p);
};

6
cpp/simulated2D.h
View File

@ -56,7 +56,7 @@ namespace gtsam {
Point2 z_;
Prior(const Point2& z, const sharedGaussian& model, const PoseKey& key) :
Prior(const Point2& z, const SharedGaussian& model, const PoseKey& key) :
NonlinearFactor1<Config, PoseKey, Point2> (model, key), z_(z) {
}
@ -74,7 +74,7 @@ namespace gtsam {
Point2> {
Point2 z_;
Odometry(const Point2& z, const sharedGaussian& model, const PoseKey& j1,
Odometry(const Point2& z, const SharedGaussian& model, const PoseKey& j1,
const PoseKey& j2) :
z_(z), NonlinearFactor2<Config, PoseKey, Point2, PoseKey, Point2> (
model, j1, j2) {
@ -95,7 +95,7 @@ namespace gtsam {
Point2 z_;
Measurement(const Point2& z, const sharedGaussian& model,
Measurement(const Point2& z, const SharedGaussian& model,
const PoseKey& j1, const PointKey& j2) :
z_(z), NonlinearFactor2<Config, PoseKey, Point2, PointKey, Point2> (
model, j1, j2) {

41
cpp/smallExample.cpp
View File

@ -12,8 +12,6 @@
using namespace std;
// TODO: DANGEROUS, create shared pointers
#define GTSAM_MAGIC_GAUSSIAN 2
#define GTSAM_MAGIC_KEY
#include "Ordering.h"
@ -30,9 +28,10 @@ namespace example {
typedef boost::shared_ptr<NonlinearFactor<Config> > shared;
static sharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1);
static sharedDiagonal sigma0_2 = noiseModel::Isotropic::Sigma(2,0.2);
static sharedDiagonal constraintModel = noiseModel::Constrained::All(2);
static SharedDiagonal sigma1_0 = noiseModel::Isotropic::Sigma(2,1.0);
static SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1);
static SharedDiagonal sigma0_2 = noiseModel::Isotropic::Sigma(2,0.2);
static SharedDiagonal constraintModel = noiseModel::Constrained::All(2);
/* ************************************************************************* */
boost::shared_ptr<const Graph> sharedNonlinearFactorGraph() {
@ -41,27 +40,23 @@ namespace example {
new Graph);
// prior on x1
double sigma1 = 0.1;
Point2 mu;
shared f1(new simulated2D::Prior(mu, sigma1, 1));
shared f1(new simulated2D::Prior(mu, sigma0_1, 1));
nlfg->push_back(f1);
// odometry between x1 and x2
double sigma2 = 0.1;
Point2 z2(1.5, 0);
shared f2(new simulated2D::Odometry(z2, sigma2, 1, 2));
shared f2(new simulated2D::Odometry(z2, sigma0_1, 1, 2));
nlfg->push_back(f2);
// measurement between x1 and l1
double sigma3 = 0.2;
Point2 z3(0, -1);
shared f3(new simulated2D::Measurement(z3, sigma3, 1, 1));
shared f3(new simulated2D::Measurement(z3, sigma0_2, 1, 1));
nlfg->push_back(f3);
// measurement between x2 and l1
double sigma4 = 0.2;
Point2 z4(-1.5, -1.);
shared f4(new simulated2D::Measurement(z4, sigma4, 2, 1));
shared f4(new simulated2D::Measurement(z4, sigma0_2, 2, 1));
nlfg->push_back(f4);
return nlfg;
@ -193,7 +188,7 @@ namespace example {
Point2 z_;
UnaryFactor(const Point2& z, const sharedGaussian& model,
UnaryFactor(const Point2& z, const SharedGaussian& model,
const simulated2D::PoseKey& key) :
gtsam::NonlinearFactor1<Config, simulated2D::PoseKey,
Point2>(model, key), z_(z) {
@ -228,28 +223,25 @@ namespace example {
/* ************************************************************************* */
pair<Graph, Config> createNonlinearSmoother(int T) {
// noise on measurements and odometry, respectively
double sigma1 = 1, sigma2 = 1;
// Create
Graph nlfg;
Config poses;
// prior on x1
Point2 x1(1.0, 0.0);
shared prior(new simulated2D::Prior(x1, sigma1, 1));
shared prior(new simulated2D::Prior(x1, sigma1_0, 1));
nlfg.push_back(prior);
poses.insert(simulated2D::PoseKey(1), x1);
for (int t = 2; t <= T; t++) {
// odometry between x_t and x_{t-1}
Point2 odo(1.0, 0.0);
shared odometry(new simulated2D::Odometry(odo, sigma2, t - 1, t));
shared odometry(new simulated2D::Odometry(odo, sigma1_0, t - 1, t));
nlfg.push_back(odometry);
// measurement on x_t is like perfect GPS
Point2 xt(t, 0);
shared measurement(new simulated2D::Prior(xt, sigma1, t));
shared measurement(new simulated2D::Prior(xt, sigma1_0, t));
nlfg.push_back(measurement);
// initial estimate
@ -524,17 +516,14 @@ namespace example {
NonlinearFactorGraph<Config> nlfg;
// Create almost hard constraint on x11, sigma=0 will work for PCG not for normal
double sigma0 = 1e-3;
shared constraint(new simulated2D::Prior(Point2(1.0, 1.0), sigma0, key(1,1)));
shared constraint(new simulated2D::Prior(Point2(1.0, 1.0), sharedSigma(2,1e-3), key(1,1)));
nlfg.push_back(constraint);
double sigma = 0.01;
// Create horizontal constraints, 1...N*(N-1)
Point2 z1(1.0, 0.0); // move right
for (size_t x = 1; x < N; x++)
for (size_t y = 1; y <= N; y++) {
shared f(new simulated2D::Odometry(z1, sigma, key(x, y), key(x + 1, y)));
shared f(new simulated2D::Odometry(z1, sharedSigma(2,0.01), key(x, y), key(x + 1, y)));
nlfg.push_back(f);
}
@ -542,7 +531,7 @@ namespace example {
Point2 z2(0.0, 1.0); // move up
for (size_t x = 1; x <= N; x++)
for (size_t y = 1; y < N; y++) {
shared f(new simulated2D::Odometry(z2, sigma, key(x, y), key(x, y + 1)));
shared f(new simulated2D::Odometry(z2, sharedSigma(2,0.01), key(x, y), key(x, y + 1)));
nlfg.push_back(f);
}

4
cpp/testGaussianFactor.cpp
View File

@ -27,7 +27,7 @@ using namespace gtsam;
using namespace example;
using namespace boost;
static sharedDiagonal
static SharedDiagonal
sigma0_1 = sharedSigma(2,0.1), sigma_02 = sharedSigma(2,0.2),
constraintModel = noiseModel::Constrained::All(2);
@ -225,7 +225,7 @@ TEST( NonlinearFactorGraph, combine2){
TEST( GaussianFactor, linearFactorN){
Matrix I = eye(2);
vector<GaussianFactor::shared_ptr> f;
sharedDiagonal model = sharedSigma(2,1.0);
SharedDiagonal model = sharedSigma(2,1.0);
f.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x1", I, Vector_(2,
10.0, 5.0), model)));
f.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x1", -10 * I,

6
cpp/testGaussianFactorGraph.cpp
View File

@ -263,7 +263,7 @@ TEST( GaussianFactorGraph, add_priors )
GaussianFactorGraph expected = createGaussianFactorGraph();
Matrix A = eye(2);
Vector b = zero(2);
sharedDiagonal sigma = sharedSigma(2,3.0);
SharedDiagonal sigma = sharedSigma(2,3.0);
expected.push_back(GaussianFactor::shared_ptr(new GaussianFactor("l1",A,b,sigma)));
expected.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x1",A,b,sigma)));
expected.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x2",A,b,sigma)));
@ -629,7 +629,7 @@ TEST( GaussianFactorGraph, elimination )
GaussianFactorGraph fg;
Matrix Ap = eye(1), An = eye(1) * -1;
Vector b = Vector_(1, 0.0);
sharedDiagonal sigma = sharedSigma(2,2.0);
SharedDiagonal sigma = sharedSigma(2,2.0);
fg.add("x1", An, "x2", Ap, b, sigma);
fg.add("x1", Ap, b, sigma);
fg.add("x2", Ap, b, sigma);
@ -735,7 +735,7 @@ TEST( GaussianFactorGraph, constrained_multi2 )
/* ************************************************************************* */
sharedDiagonal model = sharedSigma(2,1);
SharedDiagonal model = sharedSigma(2,1);
TEST( GaussianFactorGraph, findMinimumSpanningTree )
{

10
cpp/testIterative.cpp
View File

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

107
cpp/testMatrix.cpp
View File

@ -11,7 +11,6 @@
#include <boost/foreach.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include "Matrix.h"
#include "NoiseModel.h"
using namespace std;
using namespace gtsam;
@ -419,6 +418,8 @@ TEST( matrix, inverse )
A(2,0)= 1; A(2,1)=0; A(2,2)=6;
Matrix Ainv = inverse(A);
CHECK(assert_equal(eye(3), A*Ainv));
CHECK(assert_equal(eye(3), Ainv*A));
Matrix expected(3,3);
expected(0,0)= 1.0909; expected(0,1)=-0.5454; expected(0,2)=-0.0909;
@ -426,6 +427,26 @@ TEST( matrix, inverse )
expected(2,0)= -0.1818; expected(2,1)= 0.0909; expected(2,2)=0.1818;
CHECK(assert_equal(expected, Ainv, 1e-4));
// These two matrices failed before version 2003 because we called LU incorrectly
Matrix lMg(Matrix_(3,3,
0.0, 1.0,-2.0,
-1.0, 0.0, 1.0,
0.0, 0.0, 1.0));
CHECK(assert_equal(Matrix_(3,3,
0.0, -1.0, 1.0,
1.0, 0.0, 2.0,
0.0, 0.0, 1.0),
inverse(lMg)));
Matrix gMl(Matrix_(3,3,
0.0, -1.0, 1.0,
1.0, 0.0, 2.0,
0.0, 0.0, 1.0));
CHECK(assert_equal(Matrix_(3,3,
0.0, 1.0,-2.0,
-1.0, 0.0, 1.0,
0.0, 0.0, 1.0),
inverse(gMl)));
}
/* ************************************************************************* */
@ -623,69 +644,6 @@ static void updateAb(Matrix& A, Vector& b, int j, const Vector& a,
}
}
/* ************************************************************************* */
list<boost::tuple<Vector, double, double> >
weighted_eliminate2(Matrix& A, Vector& b, const sharedGaussian& model) {
size_t m = A.size1(), n = A.size2(); // get size(A)
size_t maxRank = min(m,n);
// pre-whiten everything
model->WhitenInPlace(A);
b = model->whiten(b);
// create list
list<boost::tuple<Vector, double, double> > results;
// We loop over all columns, because the columns that can be eliminated
// are not necessarily contiguous. For each one, estimate the corresponding
// scalar variable x as d-rS, with S the separator (remaining columns).
// Then update A and b by substituting x with d-rS, zero-ing out x's column.
for (int j=0; j<n; ++j) {
// extract the first column of A
Vector a(column(A, j)); // ublas::matrix_column is slower !
// Calculate weighted pseudo-inverse and corresponding precision
double precision = dot(a,a);
Vector pseudo = a/precision;
// if precision is zero, no information on this column
if (precision < 1e-8) continue;
// create solution and copy into r
Vector r(basis(n, j));
for (int j2=j+1; j2<n; ++j2) // expensive !!
r(j2) = inner_prod(pseudo, boost::numeric::ublas::matrix_column<Matrix>(A, j2));
// create the rhs
double d = inner_prod(pseudo, b);
// construct solution (r, d, sigma)
// TODO: avoid sqrt, store precision or at least variance
results.push_back(boost::make_tuple(r, d, 1./sqrt(precision)));
// exit after rank exhausted
if (results.size()>=maxRank) break;
// update A, b, expensive, suing outer product
// A' \define A_{S}-a*r and b'\define b-d*a
updateAb(A, b, j, a, r, d);
}
return results;
}
/* ************************************************************************* */
void weighted_eliminate3(Matrix& A, Vector& b, const sharedGaussian& model) {
size_t m = A.size1(), n = A.size2(); // get size(A)
size_t maxRank = min(m,n);
// pre-whiten everything
model->WhitenInPlace(A);
b = model->whiten(b);
householder_(A, maxRank);
}
/* ************************************************************************* */
TEST( matrix, weighted_elimination )
{
@ -727,27 +685,6 @@ TEST( matrix, weighted_elimination )
DOUBLES_EQUAL(newSigmas(i), sigma, 1e-5); // verify sigma
i += 1;
}
// perform elimination with NoiseModel
Matrix A2 = A; Vector b2 = b;
sharedGaussian model = noiseModel::Diagonal::Sigmas(sigmas);
std::list<boost::tuple<Vector, double, double> > solution2 =
weighted_eliminate2(A2, b2, model);
// unpack and verify
i=0;
BOOST_FOREACH(boost::tie(r, di, sigma), solution2) {
CHECK(assert_equal(r, row(expectedR, i))); // verify r
DOUBLES_EQUAL(d(i), di, 1e-8); // verify d
DOUBLES_EQUAL(newSigmas(i), sigma, 1e-5); // verify sigma
i += 1;
}
// perform elimination with NoiseModel
weighted_eliminate3(A, b, model);
sharedGaussian newModel = noiseModel::Diagonal::Sigmas(newSigmas);
// print(A);
// print(newModel->Whiten(expectedR));
}
/* ************************************************************************* */

28
cpp/testNoiseModel.cpp
View File

@ -11,6 +11,8 @@
#include <boost/foreach.hpp>
#include <iostream>
#include "NoiseModel.h"
#include "SharedGaussian.h"
#include "SharedDiagonal.h"
using namespace std;
using namespace gtsam;
@ -148,12 +150,12 @@ TEST(NoiseModel, ConstrainedAll )
//
// // Expected result
// 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
// sharedDiagonal diagonal = noiseModel::Diagonal::Sigmas(sigmas);
// sharedDiagonal actual1 = diagonal->QR(Ab1);
// sharedDiagonal expected = noiseModel::Unit::Create(4);
// SharedDiagonal diagonal = noiseModel::Diagonal::Sigmas(sigmas);
// SharedDiagonal actual1 = diagonal->QR(Ab1);
// SharedDiagonal expected = noiseModel::Unit::Create(4);
// CHECK(assert_equal(*expected,*actual1));
// Matrix expectedRd1 = Matrix_(4, 6+1,
// 11.1803, 0.0, -2.23607, 0.0, -8.94427, 0.0, 2.23607,
@ -163,8 +165,8 @@ TEST(NoiseModel, ConstrainedAll )
// CHECK(assert_equal(expectedRd1,Ab1,1e-4)); // Ab was modified in place !!!
//
// // Call Constrained version
// sharedDiagonal constrained = noiseModel::Constrained::MixedSigmas(sigmas);
// sharedDiagonal actual2 = constrained->QR(Ab2);
// SharedDiagonal constrained = noiseModel::Constrained::MixedSigmas(sigmas);
// SharedDiagonal actual2 = constrained->QR(Ab2);
// CHECK(assert_equal(*expectedModel,*actual2));
// Matrix expectedRd2 = Matrix_(4, 6+1,
// 1., 0., -0.2, 0., -0.8, 0., 0.2,
@ -177,13 +179,13 @@ TEST(NoiseModel, ConstrainedAll )
/* ************************************************************************* */
TEST(NoiseModel, QRNan )
{
sharedDiagonal constrained = noiseModel::Constrained::All(2);
SharedDiagonal constrained = noiseModel::Constrained::All(2);
Matrix Ab = Matrix_(2, 5, 1., 2., 1., 2., 3., 2., 1., 2., 4., 4.);
sharedDiagonal expected = noiseModel::Constrained::All(2);
SharedDiagonal expected = noiseModel::Constrained::All(2);
Matrix expectedAb = Matrix_(2, 5, 1., 2., 1., 2., 3., 0., 1., 0., 0., 2.0/3);
sharedDiagonal actual = constrained->QR(Ab);
SharedDiagonal actual = constrained->QR(Ab);
CHECK(assert_equal(*expected,*actual));
CHECK(assert_equal(expectedAb,Ab));
}
@ -192,8 +194,8 @@ TEST(NoiseModel, QRNan )
TEST(NoiseModel, SmartCovariance )
{
bool smart = true;
sharedGaussian expected = Unit::Create(3);
sharedGaussian actual = Gaussian::Covariance(eye(3), smart);
SharedGaussian expected = Unit::Create(3);
SharedGaussian actual = Gaussian::Covariance(eye(3), smart);
CHECK(assert_equal(*expected,*actual));
}
@ -201,8 +203,8 @@ TEST(NoiseModel, SmartCovariance )
TEST(NoiseModel, ScalarOrVector )
{
bool smart = true;
sharedGaussian expected = Unit::Create(3);
sharedGaussian actual = Gaussian::Covariance(eye(3), smart);
SharedGaussian expected = Unit::Create(3);
SharedGaussian actual = Gaussian::Covariance(eye(3), smart);
CHECK(assert_equal(*expected,*actual));
}

12
cpp/testNonlinearConstraint.cpp
View File

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

2
cpp/testNonlinearEquality.cpp
View File

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

2
cpp/testNonlinearFactor.cpp
View File

@ -29,7 +29,7 @@ typedef boost::shared_ptr<NonlinearFactor<VectorConfig> > shared_nlf;
/* ************************************************************************* */
TEST( NonlinearFactor, equals )
{
sharedGaussian sigma(noiseModel::Isotropic::Sigma(2,1.0));
SharedGaussian sigma(noiseModel::Isotropic::Sigma(2,1.0));
// create two nonlinear2 factors
Point2 z3(0.,-1.);

8
cpp/testNonlinearOptimizer.cpp
View File

@ -170,8 +170,8 @@ TEST( NonlinearOptimizer, Factorization )
config->insert(2, Pose2(1.5,0.,0.));
boost::shared_ptr<Pose2Graph> graph(new Pose2Graph);
graph->addPrior(1, Pose2(0.,0.,0.), Isotropic::Sigma(3, 1e-10));
graph->addConstraint(1,2, Pose2(1.,0.,0.), Isotropic::Sigma(3, 1));
graph->addPrior(1, Pose2(0.,0.,0.), sharedSigma(3, 1e-10));
graph->addConstraint(1,2, Pose2(1.,0.,0.), sharedSigma(3, 1));
boost::shared_ptr<Ordering> ordering(new Ordering);
ordering->push_back(Pose2Config::Key(1));
@ -197,8 +197,8 @@ TEST( NonlinearOptimizer, SubgraphPCG )
config->insert(2, Pose2(1.5,0.,0.));
boost::shared_ptr<Pose2Graph> graph(new Pose2Graph);
graph->addPrior(1, Pose2(0.,0.,0.), Isotropic::Sigma(3, 1e-10));
graph->addConstraint(1,2, Pose2(1.,0.,0.), Isotropic::Sigma(3, 1));
graph->addPrior(1, Pose2(0.,0.,0.), sharedSigma(3, 1e-10));
graph->addConstraint(1,2, Pose2(1.,0.,0.), sharedSigma(3, 1));
double relativeThreshold = 1e-5;
double absoluteThreshold = 1e-5;

2
cpp/testPlanarSLAM.cpp
View File

@ -15,7 +15,7 @@ using namespace gtsam;
static Pose2 x1, x2(1, 1, 0), x3(1, 1, M_PI_4);
static Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3);
sharedGaussian
SharedGaussian
sigma(noiseModel::Isotropic::Sigma(1,0.1)),
I3(noiseModel::Unit::Create(3));

41
cpp/testPose2.cpp
View File

@ -181,15 +181,15 @@ Matrix matrix(const Pose2& gTl) {
TEST( Pose2, matrix )
{
Point2 origin, t(1,2);
Pose2 gT1(M_PI_2, t); // robot at (1,2) looking towards y
Matrix gM1 = matrix(gT1);
Pose2 gTl(M_PI_2, t); // robot at (1,2) looking towards y
Matrix gMl = matrix(gTl);
CHECK(assert_equal(Matrix_(3,3,
0.0, -1.0, 1.0,
1.0, 0.0, 2.0,
0.0, 0.0, 1.0),
gM1));
Rot2 gR1 = gT1.r();
CHECK(assert_equal(homogeneous(t),gM1*homogeneous(origin)));
gMl));
Rot2 gR1 = gTl.r();
CHECK(assert_equal(homogeneous(t),gMl*homogeneous(origin)));
Point2 x_axis(1,0), y_axis(0,1);
CHECK(assert_equal(Matrix_(2,2,
0.0, -1.0,
@ -197,14 +197,26 @@ TEST( Pose2, matrix )
gR1.matrix()));
CHECK(assert_equal(Point2(0,1),gR1*x_axis));
CHECK(assert_equal(Point2(-1,0),gR1*y_axis));
CHECK(assert_equal(homogeneous(Point2(1+0,2+1)),gM1*homogeneous(x_axis)));
CHECK(assert_equal(homogeneous(Point2(1-1,2+0)),gM1*homogeneous(y_axis)));
CHECK(assert_equal(homogeneous(Point2(1+0,2+1)),gMl*homogeneous(x_axis)));
CHECK(assert_equal(homogeneous(Point2(1-1,2+0)),gMl*homogeneous(y_axis)));
// check inverse pose
Matrix _1Mg = matrix(inverse(gT1));
CHECK(assert_equal(inverse(gM1),_1Mg));
CHECK(assert_equal(eye(3),inverse(_1Mg)*_1Mg));
CHECK(assert_equal(eye(3),inverse(gM1)*gM1));
Matrix lMg = matrix(inverse(gTl));
CHECK(assert_equal(Matrix_(3,3,
0.0, 1.0,-2.0,
-1.0, 0.0, 1.0,
0.0, 0.0, 1.0),
lMg));
}
/* ************************************************************************* */
TEST( Pose2, compose_matrix )
{
Pose2 gT1(M_PI_2, Point2(1,2)); // robot at (1,2) looking towards y
Pose2 _1T2(M_PI, Point2(-1,4)); // local robot at (-1,4) loooking at negative x
Matrix gM1(matrix(gT1)),_1M2(matrix(_1T2));
CHECK(assert_equal(gM1*_1M2,matrix(compose(_1T2,gT1)))); // WRONG CHECKS OUT !
// CHECK(assert_equal(gM1*_1M2,matrix(compose(gT1,_1T2)))); RIGHT DOES NOT
}
/* ************************************************************************* */
@ -221,13 +233,6 @@ TEST( Pose2, between )
Matrix actualH1,actualH2;
Pose2 expected(M_PI_2, Point2(2,2));
Pose2 actual1 = between(gT1,gT2); // gT2 * 1Tg does not make sense !!!!!
GTSAM_PRINT(between(gT1,gT2));
// what we want is 1T2 = 1Tg * gT2 = between(2Tg,1Tg)
// print(matrix(gT1));
// print(matrix(gT2));
// print(inverse(matrix(gT1))*matrix(gT2)); // 1T2
// print(inverse(matrix(gT2))*matrix(gT1)); // 2T1
// GTSAM_PRINT(between(inverse(gT2),inverse(gT1)));
Pose2 actual2 = between(gT1,gT2,actualH1,actualH2);
CHECK(assert_equal(expected,actual1));
CHECK(assert_equal(expected,actual2));

2
cpp/testPose2Factor.cpp
View File

@ -114,7 +114,7 @@ TEST( Pose2Factor, linearize )
Vector expected_b = Vector_(3, 0.0, 0.0, 0.0);
// expected linear factor
sharedDiagonal probModel1 = noiseModel::Unit::Create(3);
SharedDiagonal probModel1 = noiseModel::Unit::Create(3);
GaussianFactor expected("x1", expectedH1, "x2", expectedH2, expected_b, probModel1);
// Actual linearization

2
cpp/testPose2Prior.cpp
View File

@ -16,7 +16,7 @@ using namespace gtsam;
// Common measurement covariance
static double sx=0.5, sy=0.5,st=0.1;
static sharedGaussian sigmas = Diagonal::Sigmas(Vector_(3,sx,sy,st));
static SharedGaussian sigmas = sharedSigmas(Vector_(3,sx,sy,st));
/* ************************************************************************* */
// Very simple test establishing Ax-b \approx z-h(x)

2
cpp/testPose2SLAM.cpp
View File

@ -79,7 +79,7 @@ TEST( Pose2Graph, linearization )
double sigma = 1;
Vector b = Vector_(3,-0.1/sx,0.1/sy,0.0);
sharedDiagonal probModel1 = noiseModel::Unit::Create(3);
SharedDiagonal probModel1 = noiseModel::Unit::Create(3);
lfg_expected.add("x1", A1, "x2", A2, b, probModel1);
CHECK(assert_equal(lfg_expected, lfg_linearized));

2
cpp/testPose3Factor.cpp
View File

@ -23,7 +23,7 @@ TEST( Pose3Factor, error )
Pose3 z(rodriguez(0.2,0.2,0.3),Point3(0,1.1,0));;
// Create factor
sharedGaussian I6(noiseModel::Unit::Create(6));
SharedGaussian I6(noiseModel::Unit::Create(6));
Pose3Factor factor(1,2, z, I6);
// Create config

12
cpp/testSQP.cpp
View File

@ -13,7 +13,7 @@
#include <CppUnitLite/TestHarness.h>
// TODO: DANGEROUS, create shared pointers
#define GTSAM_MAGIC_GAUSSIAN 2
#define GTSAM_DANGEROUS_GAUSSIAN 2
#define GTSAM_MAGIC_KEY
#include <Pose3.h>
@ -36,9 +36,9 @@ using namespace boost;
using namespace boost::assign;
// Models to use
sharedDiagonal probModel1 = sharedSigma(1,1.0);
sharedDiagonal probModel2 = sharedSigma(2,1.0);
sharedDiagonal constraintModel1 = noiseModel::Constrained::All(1);
SharedDiagonal probModel1 = sharedSigma(1,1.0);
SharedDiagonal probModel2 = sharedSigma(2,1.0);
SharedDiagonal constraintModel1 = noiseModel::Constrained::All(1);
// trick from some reading group
#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
@ -279,7 +279,7 @@ TEST (SQP, two_pose_truth ) {
// measurement from x1 to l1
Point2 z1(0.0, 5.0);
sharedGaussian sigma(noiseModel::Isotropic::Sigma(2, 0.1));
SharedGaussian sigma(noiseModel::Isotropic::Sigma(2, 0.1));
shared f1(new simulated2D::Measurement(z1, sigma, x1,l1));
graph->push_back(f1);
@ -387,7 +387,7 @@ TEST (SQP, two_pose ) {
// measurement from x1 to l1
Point2 z1(0.0, 5.0);
sharedGaussian sigma(noiseModel::Isotropic::Sigma(2, 0.1));
SharedGaussian sigma(noiseModel::Isotropic::Sigma(2, 0.1));
shared f1(new simulated2D::Measurement(z1, sigma, x1,l1));
graph->push_back(f1);

2
cpp/testSQPOptimizer.cpp
View File

@ -29,7 +29,7 @@ using namespace boost;
using namespace boost::assign;
using namespace simulated2D;
static sharedGaussian sigma(noiseModel::Isotropic::Sigma(1,0.1));
static SharedGaussian sigma(noiseModel::Isotropic::Sigma(1,0.1));
// typedefs
typedef simulated2D::Config Config2D;

4
cpp/testVSLAMFactor.cpp
View File

@ -24,7 +24,7 @@ static double fov = 60; // degrees
static size_t w=640,h=480;
static Cal3_S2 K(fov,w,h);
static sharedGaussian sigma(noiseModel::Unit::Create(1));
static SharedGaussian sigma(noiseModel::Unit::Create(1));
static shared_ptrK sK(new Cal3_S2(K));
// make cameras
@ -52,7 +52,7 @@ TEST( ProjectionFactor, error )
Matrix Al1 = Matrix_(2, 3, 61.584, 0., 0., 0., 61.584, 0.);
Matrix Ax1 = Matrix_(2, 6, 0., -369.504, 0., -61.584, 0., 0., 369.504, 0., 0., 0., -61.584, 0.);
Vector b = Vector_(2,3.,0.);
sharedDiagonal probModel1 = noiseModel::Unit::Create(2);
SharedDiagonal probModel1 = noiseModel::Unit::Create(2);
GaussianFactor expected("l1", Al1, "x1", Ax1, b, probModel1);
GaussianFactor::shared_ptr actual = factor->linearize(config);
CHECK(assert_equal(expected,*actual,1e-3));

2
cpp/testVSLAMGraph.cpp
View File

@ -23,7 +23,7 @@ using namespace gtsam;
using namespace gtsam::visualSLAM;
using namespace boost;
typedef NonlinearOptimizer<Graph,Config> Optimizer;
static sharedGaussian sigma(noiseModel::Unit::Create(1));
static SharedGaussian sigma(noiseModel::Unit::Create(1));
/* ************************************************************************* */
Point3 landmark1(-1.0,-1.0, 0.0);

2
cpp/visualSLAM.h
View File

@ -63,7 +63,7 @@ namespace gtsam { namespace visualSLAM {
* @param K the constant calibration
*/
ProjectionFactor(const Point2& z,
const sharedGaussian& model, PoseKey j_pose,
const SharedGaussian& model, PoseKey j_pose,
PointKey j_landmark, const shared_ptrK& K) :
z_(z), K_(K), Base(model, j_pose, j_landmark) {
}

26
matlab/createGaussianFactorGraph.m
View File

@ -6,46 +6,36 @@ c = createNoisyConfig();
% Create
fg = GaussianFactorGraph;
sigma1=.1;
% Create shared Noise models
sigma0_1 = SharedDiagonal([0.1;0.1]);
sigma0_2 = SharedDiagonal([0.2;0.2]);
% prior on x1
A11=eye(2);
b = - c.get('x1');
f1 = GaussianFactor('x1', A11, b, sigma1); % generate a Gaussian factor of odometry
f1 = GaussianFactor('x1', A11, b, sigma0_1); % generate a Gaussian factor of odometry
fg.push_back(f1);
% odometry between x1 and x2
sigma2=.1;
A21=-eye(2);
A22=eye(2);
b = [.2;-.1];
f2 = GaussianFactor('x1', A21, 'x2', A22, b,sigma2);
f2 = GaussianFactor('x1', A21, 'x2', A22, b,sigma0_1);
fg.push_back(f2);
% measurement between x1 and l1
sigma3=.2;
A31=-eye(2);
A33=eye(2);
b = [0;.2];
f3 = GaussianFactor('x1', A31, 'l1', A33, b,sigma3);
f3 = GaussianFactor('x1', A31, 'l1', A33, b,sigma0_2);
fg.push_back(f3);
% measurement between x2 and l1
sigma4=.2;
A42=-eye(2);
A43=eye(2);
b = [-.2;.3];
f4 = GaussianFactor('x2', A42, 'l1', A43, b,sigma4);
f4 = GaussianFactor('x2', A42, 'l1', A43, b,sigma0_2);
fg.push_back(f4);
end

15
matlab/testGaussianFactor.m
View File

@ -25,14 +25,13 @@ Ax1 = [
% the RHS
b2=[-1;1.5;2;-1];
combined = GaussianFactor('x2', Ax2, 'l1', Al1, 'x1', Ax1, b2, 1);
model = SharedDiagonal([1;1]);
combined = GaussianFactor('x2', Ax2, 'l1', Al1, 'x1', Ax1, b2, model);
% eliminate the combined factor
% NOT WORKING
% this is not working because there is no elimination function for a linear
% factor. Just for the MutableGaussianFactor
%[actualCG,actualLF] = combined.eliminate('x2');
% this is not working because the eliminate has to be added back to gtsam.h
[actualCG,actualLF] = combined.eliminate('x2');
% create expected Conditional Gaussian
R11 = [
@ -48,7 +47,7 @@ S13 = [
+0.00,-8.94427
];
d=[2.23607;-1.56525];
expectedCG = GaussianConditional('x2',d,R11,'l1',S12,'x1',S13,[1 1]');
expectedCG = GaussianConditional('x2',d,R11,'l1',S12,'x1',S13,model);
% the expected linear factor
Bl1 = [
@ -70,5 +69,5 @@ expectedLF = GaussianFactor('l1', Bl1, 'x1', Bx1, b1, 1);
% check if the result matches
% NOT WORKING
% because can not be computed with GaussianFactor.eliminate
%if(~actualCG.equals(expectedCG)), warning('GTSAM:unit','~actualCG.equals(expectedCG)'); end
%if(~actualLF.equals(expectedLF,1e-5)), warning('GTSAM:unit','~actualLF.equals(expectedLF,1e-5)');end
if(~actualCG.equals(expectedCG)), warning('GTSAM:unit','~actualCG.equals(expectedCG)'); end
if(~actualLF.equals(expectedLF,1e-5)), warning('GTSAM:unit','~actualLF.equals(expectedLF,1e-5)');end

2
matlab/testLinearFactorGraph.m
View File

@ -2,7 +2,7 @@
% equals
fg = createGaussianFactorGraph();
fg2 = createGaussianFactorGraph();
CHECK('equals',fg.equals(fg2));
CHECK('equals',fg.equals(fg2,1e-9));
%-----------------------------------------------------------------------
% error

8
wrap/wrap-matlab.h
View File

@ -15,10 +15,16 @@ extern "C" {
#include <boost/shared_ptr.hpp>
using namespace std;
using namespace boost; // not usual, but for consiseness of generated code
using namespace boost; // not usual, but for conciseness of generated code
// start GTSAM Specifics /////////////////////////////////////////////////
typedef numeric::ublas::vector<double> Vector;
typedef numeric::ublas::matrix<double> Matrix;
// to make keys be constructed from strings:
#define GTSAM_MAGIC_KEY
// to enable Matrix and Vector constructor for SharedGaussian:
#define GTSAM_MAGIC_GAUSSIAN
// end GTSAM Specifics /////////////////////////////////////////////////
#ifdef __LP64__
// 64-bit Mac