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

Large gtsam refactoring 

To support faster development *and* better performance Richard and I pushed through a large refactoring of NonlinearFactors.

The following are the biggest changes:

1) NonLinearFactor1 and NonLinearFactor2 are now templated on Config, Key type, and X type, where X is the argument to the measurement function.

2) The measurement itself is no longer kept in the nonlinear factor. Instead, a derived class (see testVSLAMFactor, testNonlinearEquality, testPose3Factor etc...) has to implement a function to compute the errors, "evaluateErrors". Instead of (h(x)-z), it needs to return (z-h(x)), so Ax-b is an approximation of the error. IMPORTANT: evaluateErrors needs - if asked - *combine* the calculation of the function value h(x) and the derivatives dh(x)/dx. This was a major performance issue. To do this, boost::optional<Matrix&> arguments are provided, and tin EvaluateErrors you just  says something like

	if (H) *H = Matrix_(3,6,....);

3) We are no longer using int or strings for nonlinear factors. Instead, the preferred key type is now Symbol, defined in Key.h. This is both fast and cool: you can construct it from an int, and cast it to a strong. It also does type checking: a Symbol<Pose3,'x'> will not match a Symbol<Pose2,'x'>

4) minor: take a look at LieConfig.h: it help you avoid writing a lot of code bu automatically creating configs for a certain type. See e.g. Pose3Config.h. A "double" LieConfig is on the way - Thanks Richard and Manohar !
release/4.3a0
Frank Dellaert 2010-01-13 22:25:03 +00:00
parent 22b4912d5e
commit 93465945e9
59 changed files with 1182 additions and 1179 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

68
cpp/BetweenFactor.h
View File

@ -17,14 +17,14 @@ namespace gtsam {
* A class for a measurement predicted by "between(config[key1],config[key2])"
* T is the Lie group type, Config where the T's are gotten from
*/
template<class T, class Config>
class BetweenFactor: public NonlinearFactor<Config> {
template<class Config, class Key, class T>
class BetweenFactor: public NonlinearFactor2<Config, Key, T, Key, T> {
private:
typedef NonlinearFactor2<Config, Key, T, Key, T> Base;
T measured_; /** The measurement */
std::string key1_, key2_; /** The keys of the two poses, order matters */
std::list<std::string> keys_; /** The keys as a list */
Matrix square_root_inverse_covariance_; /** sqrt(inv(measurement_covariance)) */
public:
@ -33,72 +33,50 @@ namespace gtsam {
typedef typename boost::shared_ptr<BetweenFactor> shared_ptr;
/** Constructor */
BetweenFactor(const std::string& key1, const std::string& key2,
const T& measured, const Matrix& measurement_covariance) :
key1_(key1), key2_(key2), measured_(measured) {
BetweenFactor(const Key& key1, const Key& key2, const T& measured,
const Matrix& measurement_covariance) :
Base(1, key1, key2), measured_(measured) {
square_root_inverse_covariance_ = inverse_square_root(
measurement_covariance);
keys_.push_back(key1);
keys_.push_back(key2);
}
/** implement functions needed for Testable */
/** print */
void print(const std::string& name) const {
std::cout << name << std::endl;
std::cout << "Factor " << std::endl;
std::cout << "key1 " << key1_ << std::endl;
std::cout << "key2 " << key2_ << std::endl;
void print(const std::string& s) const {
Base::print(s);
measured_.print("measured ");
gtsam::print(square_root_inverse_covariance_, "MeasurementCovariance");
}
/** equals */
bool equals(const NonlinearFactor<Config>& expected, double tol) const {
return key1_ == expected.key1_ && key2_ == expected.key2_
&& measured_.equals(expected.measured_, tol);
const BetweenFactor<Config, Key, T> *e =
dynamic_cast<const BetweenFactor<Config, Key, T>*> (&expected);
return e != NULL && Base::equals(expected)
&& this->measured_.equals(e->measured_, tol);
}
/** implement functions needed to derive from Factor */
/** vector of errors */
Vector error_vector(const T& p1, const T& p2) const {
//z-h
T hx = between(p1,p2);
// manifold equivalent of z-h(x) -> log(h(x),z)
return square_root_inverse_covariance_ * logmap(hx,measured_);
Vector evaluateError(const T& p1, const T& p2, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none) const {
// h - z
T hx = between(p1, p2);
// TODO should be done by noise model
if (H1) *H1 = square_root_inverse_covariance_ * Dbetween1(p1, p2);
if (H2) *H2 = square_root_inverse_covariance_ * Dbetween2(p1, p2);
// manifold equivalent of h(x)-z -> log(z,h(x))
// TODO use noise model, error vector is not whitened yet
return square_root_inverse_covariance_ * logmap(measured_, hx);
}
/** vector of errors */
Vector error_vector(const Config& c) const {
return error_vector(c.get(key1_), c.get(key2_));
}
/** methods to retrieve both keys */
inline const std::string& key1() const { return key1_;}
inline const std::string& key2() const { return key2_;}
/** return the measured */
inline const T measured() const {return measured_;}
/** keys as a list */
inline std::list<std::string> keys() const { return keys_;}
/** number of variables attached to this factor */
inline std::size_t size() const { return 2;}
/** linearize */
boost::shared_ptr<GaussianFactor> linearize(const Config& x0) const {
const T& p1 = x0.get(key1_), p2 = x0.get(key2_);
Matrix A1 = Dbetween1(p1, p2);
Matrix A2 = Dbetween2(p1, p2);
Vector b = error_vector(p1, p2); // already has sigmas in !
boost::shared_ptr<GaussianFactor> linearized(new GaussianFactor(
key1_, square_root_inverse_covariance_ * A1,
key2_, square_root_inverse_covariance_ * A2, b, 1.0));
return linearized;
}
};
} /// namespace gtsam

2
cpp/FactorGraph-inl.h
View File

@ -269,7 +269,7 @@ void FactorGraph<Factor>::associateFactor(int index, sharedFactor factor) {
}
}
/* ************************************************************************* */
/* ************************************************************************* *
template<class Factor>
map<string, string> FactorGraph<Factor>::findMinimumSpanningTree() const {

6
cpp/FactorGraph.h
View File

@ -120,14 +120,14 @@ namespace gtsam {
/**
* find the minimum spanning tree.
*/
std::map<std::string, std::string> findMinimumSpanningTree() const;
// std::map<std::string, std::string> findMinimumSpanningTree() const;
/**
* Split the graph into two parts: one corresponds to the given spanning tre,
* and the other corresponds to the rest of the factors
*/
void split(std::map<std::string, std::string> tree,
FactorGraph<Factor>& Ab1, FactorGraph<Factor>& Ab2) const;
// void split(std::map<std::string, std::string> tree,
// FactorGraph<Factor>& Ab1, FactorGraph<Factor>& Ab2) const;
private:
/** Associate factor index with the variables connected to the factor */

58
cpp/Key.h Normal file
View File

@ -0,0 +1,58 @@
/*
* Key.h
*
* Created on: Jan 12, 2010
* @author: Frank Dellaert
* @author: Richard Roberts
*/
#pragma once
#include <boost/format.hpp>
#include <boost/serialization/serialization.hpp>
namespace gtsam {
/**
* Symbol key class is templated on
* 1) the type T it is supposed to retrieve, for extra type checking
* 2) the character constant used for its string representation
* TODO: make Testable
*/
template <class T, char C>
class Symbol {
private:
size_t j_;
public:
// Constructors:
Symbol():j_(999999) {}
Symbol(size_t j):j_(j) {}
// Get stuff:
size_t index() const { return j_;}
operator std::string() const { return (boost::format("%c%d") % C % j_).str(); }
std::string latex() const { return (boost::format("%c_{%d}") % C % j_).str(); }
// logic:
bool operator< (const Symbol& compare) const { return j_<compare.j_;}
bool operator== (const Symbol& compare) const { return j_==compare.j_;}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(j_);
}
};
} // namespace gtsam

84
cpp/LieConfig-inl.h
View File

@ -17,58 +17,57 @@
using namespace std;
#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
namespace gtsam {
template<class T>
void LieConfig<T>::print(const string &s) const {
template<class J, class T>
void LieConfig<J,T>::print(const string &s) const {
cout << "LieConfig " << s << ", size " << values_.size() << "\n";
pair<string, T> v;
BOOST_FOREACH(v, values_)
gtsam::print(v.second, v.first + ": ");
BOOST_FOREACH(const typename Values::value_type& v, values_)
gtsam::print(v.second, (string)(v.first));
}
template<class T>
bool LieConfig<T>::equals(const LieConfig<T>& expected, double tol) const {
template<class J, class T>
bool LieConfig<J,T>::equals(const LieConfig<J,T>& expected, double tol) const {
if (values_.size() != expected.values_.size()) return false;
pair<string, T> v;
BOOST_FOREACH(v, values_) {
boost::optional<const T&> expval = expected.gettry(v.first);
if(!expval || !gtsam::equal(v.second, *expval, tol))
BOOST_FOREACH(const typename Values::value_type& v, values_) {
if (!exists(v.first)) return false;
if(!gtsam::equal(v.second, expected[v.first], tol))
return false;
}
return true;
}
template<class T>
const T& LieConfig<T>::get(const std::string& key) const {
iterator it = values_.find(key);
if (it == values_.end()) throw std::invalid_argument("invalid key");
else return it->second;
}
template<class T>
boost::optional<const T&> LieConfig<T>::gettry(const std::string& key) const {
template<class J, class T>
const T& LieConfig<J,T>::at(const Key& key) const {
const_iterator it = values_.find(key);
if (it == values_.end()) return boost::optional<const T&>();
if (it == values_.end()) throw std::invalid_argument("invalid key: " + (string)key);
else return it->second;
}
template<class T>
void LieConfig<T>::insert(const std::string& name, const T& val) {
template<class J, class T>
void LieConfig<J,T>::insert(const Key& name, const T& val) {
values_.insert(make_pair(name, val));
dim_ += dim(val);
}
template<class J, class T>
void LieConfig<J,T>::erase(const Key& key) {
iterator it = values_.find(key);
if (it == values_.end()) throw std::invalid_argument("invalid key: " + (string)key);
values_.erase(it);
}
// todo: insert for every element is inefficient
template<class T>
LieConfig<T> expmap(const LieConfig<T>& c, const VectorConfig& delta) {
LieConfig<T> newConfig;
string j; T pj;
FOREACH_PAIR(j, pj, c) {
if (delta.contains(j)) {
const Vector& dj = delta[j];
template<class J, class T>
LieConfig<J,T> expmap(const LieConfig<J,T>& c, const VectorConfig& delta) {
LieConfig<J,T> newConfig;
typedef pair<typename LieConfig<J,T>::Key,T> Value;
BOOST_FOREACH(const Value& value, c) {
const typename LieConfig<J,T>::Key& j = value.first;
const T& pj = value.second;
string key = (string)j;
if (delta.contains(key)) {
const Vector& dj = delta[key];
newConfig.insert(j, expmap(pj,dj));
} else
newConfig.insert(j, pj);
@ -77,21 +76,22 @@ namespace gtsam {
}
// todo: insert for every element is inefficient
template<class T>
LieConfig<T> expmap(const LieConfig<T>& c, const Vector& delta) {
template<class J, class T>
LieConfig<J,T> expmap(const LieConfig<J,T>& c, const Vector& delta) {
if(delta.size() != dim(c))
throw invalid_argument("Delta vector length does not match config dimensionality.");
LieConfig<T> newConfig;
pair<string, T> value;
LieConfig<J,T> newConfig;
int delta_offset = 0;
BOOST_FOREACH(value, c) {
int cur_dim = dim(value.second);
newConfig.insert(value.first,
expmap(value.second,
sub(delta, delta_offset, delta_offset+cur_dim)));
typedef pair<typename LieConfig<J,T>::Key,T> Value;
BOOST_FOREACH(const Value& value, c) {
const typename LieConfig<J,T>::Key& j = value.first;
const T& pj = value.second;
int cur_dim = dim(pj);
newConfig.insert(j,expmap(pj,sub(delta, delta_offset, delta_offset+cur_dim)));
delta_offset += cur_dim;
}
return newConfig;
}
}

96
cpp/LieConfig.h
View File

@ -9,61 +9,92 @@
#pragma once
#include <map>
#include "Vector.h"
#include "Testable.h"
#include "Lie.h"
#include "Key.h"
namespace boost { template<class T> class optional; }
namespace gtsam { class VectorConfig; }
namespace gtsam {
template<class T> class LieConfig;
class VectorConfig;
// TODO: why are these defined *before* the class ?
template<class J, class T> class LieConfig;
/** Dimensionality of the tangent space */
template<class T>
size_t dim(const LieConfig<T>& c);
template<class J, class T>
size_t dim(const LieConfig<J,T>& c);
/** Add a delta config */
template<class T>
LieConfig<T> expmap(const LieConfig<T>& c, const VectorConfig& delta);
template<class J, class T>
LieConfig<J,T> expmap(const LieConfig<J,T>& c, const VectorConfig& delta);
/** Add a delta vector, uses iterator order */
template<class T>
LieConfig<T> expmap(const LieConfig<T>& c, const Vector& delta);
template<class J, class T>
LieConfig<J,T> expmap(const LieConfig<J,T>& c, const Vector& delta);
/**
* Lie type configuration
*/
template<class J, class T>
class LieConfig : public Testable<LieConfig<J,T> > {
public:
/**
* Typedefs
*/
typedef J Key;
typedef std::map<Key, T> Values;
typedef typename Values::iterator iterator;
typedef typename Values::const_iterator const_iterator;
template<class T>
class LieConfig : public Testable<LieConfig<T> > {
private:
std::map<std::string, T> values_;
Values values_;
size_t dim_;
public:
typedef typename std::map<std::string, T>::const_iterator iterator;
typedef iterator const_iterator;
LieConfig() : dim_(0) {}
LieConfig(const LieConfig& config) :
values_(config.values_), dim_(dim(config)) {}
virtual ~LieConfig() {}
/** print */
void print(const std::string &s) const;
/** Test whether configs are identical in keys and values */
bool equals(const LieConfig& expected, double tol=1e-9) const;
/** Retrieve a variable by key, throws std::invalid_argument if not found */
const T& get(const std::string& key) const;
const T& at(const Key& key) const;
/** operator[] syntax for get */
inline const T& operator[](const std::string& name) const {
return get(name);
}
inline const T& operator[](const Key& key) const { return at(key);}
/** Retrieve a variable by key, returns nothing if not found */
boost::optional<const T&> gettry(const std::string& key) const;
/** Check if a variable exists */
bool exists(const Key& i) const {return values_.find(i)!=values_.end();}
/** The number of variables in this config */
int size() const { return values_.size(); }
const_iterator begin() const { return values_.begin(); }
const_iterator end() const { return values_.end(); }
iterator begin() { return values_.begin(); }
iterator end() { return values_.end(); }
// imperative methods:
/** Add a variable with the given key */
void insert(const std::string& name, const T& val);
void insert(const Key& key, const T& val);
/** Remove a variable from the config */
void erase(const Key& key) ;
/** Replace all keys and variables */
LieConfig& operator=(const LieConfig& rhs) {
@ -78,27 +109,14 @@ namespace gtsam {
dim_ = 0;
}
/** The number of variables in this config */
int size() { return values_.size(); }
/** Test whether configs are identical in keys and values */
bool equals(const LieConfig& expected, double tol=1e-9) const;
void print(const std::string &s) const;
const_iterator begin() const { return values_.begin(); }
const_iterator end() const { return values_.end(); }
iterator begin() { return values_.begin(); }
iterator end() { return values_.end(); }
friend LieConfig<T> expmap<T>(const LieConfig<T>& c, const VectorConfig& delta);
friend LieConfig<T> expmap<T>(const LieConfig<T>& c, const Vector& delta);
friend size_t dim<T>(const LieConfig<T>& c);
// friend LieConfig<J,T> expmap<T>(const LieConfig<J,T>& c, const VectorConfig& delta);
// friend LieConfig<J,T> expmap<T>(const LieConfig<J,T>& c, const Vector& delta);
friend size_t dim<J,T>(const LieConfig<J,T>& c);
};
/** Dimensionality of the tangent space */
template<class T>
size_t dim(const LieConfig<T>& c) { return c.dim_; }
template<class J, class T>
size_t dim(const LieConfig<J,T>& c) { return c.dim_; }
}

4
cpp/Makefile.am
View File

@ -66,7 +66,7 @@ testSymbolicBayesNet_LDADD = libgtsam.la
# Inference
headers += inference.h inference-inl.h
headers += graph-inl.h
headers += graph.h graph-inl.h
headers += FactorGraph.h FactorGraph-inl.h
headers += BayesNet.h BayesNet-inl.h
headers += BayesTree.h BayesTree-inl.h
@ -134,7 +134,7 @@ testSubgraphPreconditioner_LDADD = libgtsam.la
#timeGaussianFactorGraph_LDADD = libgtsam.la
# Nonlinear inference
headers += NonlinearFactorGraph.h NonlinearFactorGraph-inl.h
headers += Key.h NonlinearFactorGraph.h NonlinearFactorGraph-inl.h
headers += NonlinearOptimizer.h NonlinearOptimizer-inl.h
sources += NonlinearFactor.cpp
check_PROGRAMS += testNonlinearFactor testNonlinearFactorGraph testNonlinearOptimizer

6
cpp/NonlinearConstraint-inl.h
View File

@ -22,7 +22,7 @@ NonlinearConstraint<Config>::NonlinearConstraint(const std::string& lagrange_key
size_t dim_lagrange,
Vector (*g)(const Config& config),
bool isEquality)
: NonlinearFactor<Config>(zero(dim_lagrange), 1.0),
: NonlinearFactor<Config>(1.0),z_(zero(dim_lagrange)),
lagrange_key_(lagrange_key), p_(dim_lagrange),
isEquality_(isEquality), g_(boost::bind(g, _1)) {}
@ -32,8 +32,8 @@ NonlinearConstraint<Config>::NonlinearConstraint(const std::string& lagrange_key
size_t dim_lagrange,
boost::function<Vector(const Config& config)> g,
bool isEquality)
: NonlinearFactor<Config>(zero(dim_lagrange), 1.0),
lagrange_key_(lagrange_key), p_(dim_lagrange),
: NonlinearFactor<Config>(1.0),
lagrange_key_(lagrange_key), p_(dim_lagrange),z_(zero(dim_lagrange)),
g_(g), isEquality_(isEquality) {}
/* ************************************************************************* */

4
cpp/NonlinearConstraint.h
View File

@ -27,6 +27,10 @@ template <class Config>
class NonlinearConstraint : public NonlinearFactor<Config> {
protected:
/** Lagrange multipliers? */
Vector z_;
/** key for the lagrange multipliers */
std::string lagrange_key_;

93
cpp/NonlinearEquality.h
View File

@ -8,19 +8,17 @@
#include <limits>
#include <iostream>
#include "Key.h"
#include "NonlinearFactor.h"
namespace gtsam {
/**
* Template default compare function that assumes Congig.get yields a testable T
* Template default compare function that assumes a testable T
*/
template<class Config, class T>
bool compare(const std::string& key, const Config& feasible, const Config& input) {
const T& t1 = feasible.get(key);
const T& t2 = input.get(key);
return t1.equals(t2);
}
template<class T>
bool compare(const T& a, const T& b) {return a.equals(b); }
/**
@ -28,85 +26,54 @@ namespace gtsam {
* or a set of variables to be equal to each other.
* Throws an error at linearization if the constraints are not met.
*/
template<class Config>
class NonlinearEquality: public NonlinearFactor<Config> {
template<class Config, class Key, class T>
class NonlinearEquality: public NonlinearFactor1<Config, Key, T> {
private:
// node to constrain
std::string key_;
// config containing the necessary feasible point
Config feasible_;
// dimension of the variable
size_t dim_;
// feasible value
T feasible_;
public:
/**
* Function that compares a value from a config with
* another to determine whether a linearization point is
* a feasible point.
* @param key is the identifier for the key
* @param feasible is the value which is constrained
* @param input is the config to be tested for feasibility
* @return true if the linearization point is feasible
* Function that compares two values
*/
bool (*compare_)(const std::string& key, const Config& feasible,
const Config& input);
bool (*compare_)(const T& a, const T& b);
/** Constructor */
NonlinearEquality(const std::string& key, const Config& feasible,
size_t dim, bool(*compare)(const std::string& key,
const Config& feasible, const Config& input)) :
key_(key), dim_(dim), feasible_(feasible), compare_(compare) {
}
typedef NonlinearFactor1<Config, Key, T> Base;
/**
* Constructor with default compare
* Needs class T to have dim()
* and Config to have insert and get
* Constructor
*/
template <class T>
NonlinearEquality(const std::string& key, const T& feasible) :
key_(key), dim_(dim(feasible)), compare_(compare<Config,T>) {
feasible_.insert(key,feasible);
NonlinearEquality(const Key& j, const T& feasible, bool (*compare)(const T&, const T&) = compare<T>) :
Base(0, j), feasible_(feasible), compare_(compare) {
}
void print(const std::string& s = "") const {
std::cout << "Constraint: " << s << " on [" << key_ << "]\n";
feasible_.print("Feasible Point");
std::cout << "Variable Dimension: " << dim_ << std::endl;
std::cout << "Constraint: " << s << " on [" << (std::string)(this->key_) << "]\n";
gtsam::print(feasible_,"Feasible Point");
std::cout << "Variable Dimension: " << dim(feasible_) << std::endl;
}
/** Check if two factors are equal */
bool equals(const Factor<Config>& f, double tol = 1e-9) const {
const NonlinearEquality<Config>* p =
dynamic_cast<const NonlinearEquality<Config>*> (&f);
const NonlinearEquality<Config,Key,T>* p =
dynamic_cast<const NonlinearEquality<Config,Key,T>*> (&f);
if (p == NULL) return false;
if (key_ != p->key_) return false;
if (!compare_(key_, feasible_, p->feasible_)) return false; // only check the relevant value
return dim_ == p->dim_;
if (!Base::equals(*p)) return false;
return compare_(feasible_, p->feasible_);
}
/** error function */
inline Vector error_vector(const Config& c) const {
if (!compare_(key_, feasible_, c))
return repeat(dim_, std::numeric_limits<double>::infinity()); // set error to infinity if not equal
else
return zero(dim_); // set error to zero if equal
}
/** linearize a nonlinear constraint into a linear constraint */
boost::shared_ptr<GaussianFactor> linearize(const Config& c) const {
if (!compare_(key_, feasible_, c)) {
throw std::invalid_argument("Linearization point not feasible for "
+ key_ + "!");
inline Vector evaluateError(const T& xj, boost::optional<Matrix&> H) const {
size_t nj = dim(feasible_);
if (compare_(feasible_,xj)) {
if (H) *H = eye(nj);
return zero(nj); // set error to zero if equal
} else {
GaussianFactor::shared_ptr ret(new GaussianFactor(key_, eye(dim_),
zero(dim_), 0.0));
return ret;
if (H) throw std::invalid_argument(
"Linearization point not feasible for " + (std::string)(this->key_) + "!");
return repeat(nj, std::numeric_limits<double>::infinity()); // set error to infinity if not equal
}
}
}; // NonlinearEquality

108
cpp/NonlinearFactor.cpp
View File

@ -2,10 +2,10 @@
* @file NonlinearFactor.cpp
* @brief nonlinear factor versions which assume a Gaussian noise on a measurement
* @brief predicted by a non-linear function h nonlinearFactor
* @author Kai Ni
* @author Carlos Nieto
* @author Christian Potthast
* @author Frank Dellaert
* @author Richard Roberts
*
* Earlier prototype contributors: Kai Ni, Carlos Nieto, Christian Potthast
*/
#include "NonlinearFactor.h"
@ -14,105 +14,3 @@ using namespace std;
using namespace gtsam;
/* ************************************************************************* */
NonlinearFactor1::NonlinearFactor1(const Vector& z,
const double sigma,
Vector (*h)(const Vector&),
const string& key1,
Matrix (*H)(const Vector&))
: NonlinearFactor<VectorConfig>(z, sigma), h_(h), key_(key1), H_(H)
{
keys_.push_front(key1);
}
/* ************************************************************************* */
void NonlinearFactor1::print(const string& s) const {
cout << "NonlinearFactor1 " << s << endl;
cout << "h : " << (void*)h_ << endl;
cout << "key: " << key_ << endl;
cout << "H : " << (void*)H_ << endl;
NonlinearFactor<VectorConfig>::print("parent");
}
/* ************************************************************************* */
GaussianFactor::shared_ptr NonlinearFactor1::linearize(const VectorConfig& c) const {
// get argument 1 from config
Vector x1 = c[key_];
// Jacobian A = H(x1)/sigma
Matrix A = H_(x1);
// Right-hand-side b = error(c) = (z - h(x1))/sigma
Vector b = (z_ - h_(x1));
GaussianFactor::shared_ptr p(new GaussianFactor(key_, A, b, sigma_));
return p;
}
/* ************************************************************************* */
/** http://artis.imag.fr/~Xavier.Decoret/resources/C++/operator==.html */
/* ************************************************************************* */
bool NonlinearFactor1::equals(const Factor<VectorConfig>& f, double tol) const {
const NonlinearFactor1* p = dynamic_cast<const NonlinearFactor1*> (&f);
if (p == NULL) return false;
return NonlinearFactor<VectorConfig>::equals(*p, tol)
&& (h_ == p->h_)
&& (key_ == p->key_)
&& (H_ == p->H_);
}
/* ************************************************************************* */
NonlinearFactor2::NonlinearFactor2(const Vector& z,
const double sigma,
Vector (*h)(const Vector&, const Vector&),
const string& key1,
Matrix (*H1)(const Vector&, const Vector&),
const string& key2,
Matrix (*H2)(const Vector&, const Vector&)
)
: NonlinearFactor<VectorConfig>(z, sigma), h_(h), key1_(key1), H1_(H1), key2_(key2), H2_(H2)
{
keys_.push_front(key1);
keys_.push_front(key2);
}
/* ************************************************************************* */
void NonlinearFactor2::print(const string& s) const {
cout << "NonlinearFactor2 " << s << endl;
cout << "h : " << (void*)h_ << endl;
cout << "key1: " << key1_ << endl;
cout << "H2 : " << (void*)H2_ << endl;
cout << "key2: " << key2_ << endl;
cout << "H1 : " << (void*)H1_ << endl;
NonlinearFactor<VectorConfig>::print("parent");
}
/* ************************************************************************* */
GaussianFactor::shared_ptr NonlinearFactor2::linearize(const VectorConfig& c) const {
// get arguments from config
Vector x1 = c[key1_];
Vector x2 = c[key2_];
// Jacobian A = H(x)/sigma
Matrix A1 = H1_(x1, x2);
Matrix A2 = H2_(x1, x2);
// Right-hand-side b = (z - h(x))/sigma
Vector b = (z_ - h_(x1, x2));
GaussianFactor::shared_ptr p(new GaussianFactor(key1_, A1, key2_, A2, b, sigma_));
return p;
}
/* ************************************************************************* */
bool NonlinearFactor2::equals(const Factor<VectorConfig>& f, double tol) const {
const NonlinearFactor2* p = dynamic_cast<const NonlinearFactor2*> (&f);
if (p == NULL) return false;
return NonlinearFactor<VectorConfig>::equals(*p, tol)
&& (h_ == p->h_)
&& (key1_ == p->key1_)
&& (H1_ == p->H1_)
&& (key2_ == p->key2_)
&& (H2_ == p->H2_);
}
/* ************************************************************************* */

399
cpp/NonlinearFactor.h
View File

@ -1,13 +1,10 @@
/**
* @file NonlinearFactor.h
* @brief Non-linear factor class
* @author Kai Ni
* @author Carlos Nieto
* @author Christian Potthast
* @author Frank Dellaert
* @author Richard Roberts
*/
// \callgraph
#pragma once
@ -16,184 +13,304 @@
#include <limits>
#include <boost/shared_ptr.hpp>
#include <boost/serialization/list.hpp>
#include <boost/serialization/base_object.hpp>
#include "Factor.h"
#include "Vector.h"
#include "Matrix.h"
#include "GaussianFactor.h"
/**
* Base Class
* Just has the measurement and noise model
*/
namespace gtsam {
// forward declaration of GaussianFactor
//class GaussianFactor;
//typedef boost::shared_ptr<GaussianFactor> shared_ptr;
// TODO class NoiseModel {};
// TODO class Isotropic : public NoiseModel {};
// TODO class Diagonal : public NoiseModel {};
// TODO class Full : public NoiseModel {};
// TODO class Robust : public NoiseModel {};
/**
* Nonlinear factor which assumes Gaussian noise on a measurement
* predicted by a non-linear function h.
*
* Templated on a configuration type. The configurations are typically more general
* than just vectors, e.g., Rot3 or Pose3, which are objects in non-linear manifolds.
*/
template <class Config>
class NonlinearFactor : public Factor<Config>
{
protected:
/**
* Nonlinear factor which assumes zero-mean Gaussian noise on the
* on a measurement predicted by a non-linear function h.
*
* Templated on a configuration type. The configurations are typically
* more general than just vectors, e.g., Rot3 or Pose3,
* which are objects in non-linear manifolds (Lie groups).
*/
template<class Config>
class NonlinearFactor: public Factor<Config> {
Vector z_; // measurement
double sigma_; // noise standard deviation
std::list<std::string> keys_; // keys
protected:
public:
double sigma_; // noise standard deviation
std::list<std::string> keys_; // keys
/** Default constructor, with easily identifiable bogus values */
NonlinearFactor():z_(Vector_(2,888.0,999.0)),sigma_(0.1234567) {}
typedef NonlinearFactor<Config> This;
/**
* Constructor
* @param z the measurement
* @param sigma the standard deviation
*/
NonlinearFactor(const Vector& z, const double sigma) {
z_ = z;
sigma_ = sigma;
}
public:
/** print */
void print(const std::string& s = "") const {
std::cout << "NonlinearFactor " << s << std::endl;
gtsam::print(z_, " z = ");
std::cout << " sigma = " << sigma_ << std::endl;
}
/** Default constructor for I/O only */
NonlinearFactor() {
}
/** Check if two NonlinearFactor objects are equal */
bool equals(const Factor<Config>& f, double tol=1e-9) const {
const NonlinearFactor<Config>* p = dynamic_cast<const NonlinearFactor<Config>*> (&f);
if (p == NULL) return false;
return equal_with_abs_tol(z_,p->z_,tol) && fabs(sigma_ - p->sigma_)<=tol;
}
/**
* Constructor
* @param sigma the standard deviation
* // TODO: take a NoiseModel shared pointer
*/
NonlinearFactor(const double sigma) :
sigma_(sigma) {
}
/** Vector of errors */
virtual Vector error_vector(const Config& c) const = 0;
/** print */
void print(const std::string& s = "") const {
std::cout << "NonlinearFactor " << s << std::endl;
std::cout << " sigma = " << sigma_ << std::endl;
}
/** linearize to a GaussianFactor */
virtual boost::shared_ptr<GaussianFactor> linearize(const Config& c) const = 0;
/** Check if two NonlinearFactor objects are equal */
bool equals(const Factor<Config>& f, double tol = 1e-9) const {
const This* p = dynamic_cast<const NonlinearFactor<Config>*> (&f);
if (p == NULL) return false;
return fabs(sigma_ - p->sigma_) <= tol;
}
/** get functions */
double sigma() const {return sigma_;}
Vector measurement() const {return z_;}
std::list<std::string> keys() const {return keys_;}
/**
* calculate the error of the factor
* TODO: use NoiseModel
*/
double error(const Config& c) const {
// return NoiseModel.mahalanobis(error_vector(c)); // e'*inv(C)*e
if (sigma_ == 0.0) {
Vector e = error_vector(c);
return (inner_prod(e, e) > 0) ? std::numeric_limits<double>::infinity()
: 0.0;
}
Vector e = error_vector(c) / sigma_;
return 0.5 * inner_prod(e, e);
}
;
/** calculate the error of the factor */
double error(const Config& c) const {
if (sigma_==0.0) {
Vector e = error_vector(c);
return (inner_prod(e,e)>0) ? std::numeric_limits<double>::infinity() : 0.0;
}
Vector e = error_vector(c) / sigma_;
return 0.5 * inner_prod(e,e);
};
/** return keys */
std::list<std::string> keys() const {
return keys_;
}
/** get the size of the factor */
std::size_t size() const{return keys_.size();}
/** get the size of the factor */
std::size_t size() const {
return keys_.size();
}
private:
/** get functions */
double sigma() const {
return sigma_;
} // TODO obsolete when using NoiseModel
/** Vector of errors */
virtual Vector error_vector(const Config& c) const = 0;
/** linearize to a GaussianFactor */
virtual boost::shared_ptr<GaussianFactor>
linearize(const Config& c) const = 0;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(z_);
ar & BOOST_SERIALIZATION_NVP(sigma_);
ar & BOOST_SERIALIZATION_NVP(keys_);
ar & BOOST_SERIALIZATION_NVP(sigma_); // TODO NoiseModel
}
}; // NonlinearFactor
}; // NonlinearFactor
/**
* a Gaussian nonlinear factor that takes 1 parameter
* Note: cannot be serialized as contains function pointers
* Specialized derived classes could do this
*/
class NonlinearFactor1 : public NonlinearFactor<VectorConfig> {
private:
std::string key_;
public:
Vector (*h_)(const Vector&);
Matrix (*H_)(const Vector&);
/** Constructor */
NonlinearFactor1(const Vector& z, // measurement
const double sigma, // variance
Vector (*h)(const Vector&), // measurement function
const std::string& key1, // key of the variable
Matrix (*H)(const Vector&)); // derivative of the measurement function
void print(const std::string& s = "") const;
/** Check if two factors are equal */
bool equals(const Factor<VectorConfig>& f, double tol=1e-9) const;
/** error function */
inline Vector error_vector(const VectorConfig& c) const {
return z_ - h_(c[key_]);
}
/** linearize a non-linearFactor1 to get a linearFactor1 */
boost::shared_ptr<GaussianFactor> linearize(const VectorConfig& c) const;
};
/**
* a Gaussian nonlinear factor that takes 2 parameters
* A Gaussian nonlinear factor that takes 1 parameter
* implementing the density P(z|x) \propto exp -0.5*|z-h(x)|^2_C
* Templated on the parameter type X and the configuration Config
* There is no return type specified for h(x). Instead, we require
* the derived class implements error_vector(c) = h(x)-z \approx Ax-b
* This allows a graph to have factors with measurements of mixed type.
*/
template<class Config, class Key, class X>
class NonlinearFactor1: public NonlinearFactor<Config> {
protected:
// The value of the key. Not const to allow serialization
Key key_;
typedef NonlinearFactor<Config> Base;
typedef NonlinearFactor1<Config, Key, X> This;
public:
/**
* Constructor
* @param z measurement
* @param key by which to look up X value in Config
*/
NonlinearFactor1(double sigma, const Key& key1) :
Base(sigma), key_(key1) {
this->keys_.push_back(key_);
}
/* print */
void print(const std::string& s = "") const {
std::cout << "NonlinearFactor1 " << s << std::endl;
std::cout << "key: " << (std::string) key_ << std::endl;
Base::print("parent");
}
/** Check if two factors are equal. Note type is Factor and needs cast. */
bool equals(const Factor<Config>& f, double tol = 1e-9) const {
const This* p = dynamic_cast<const This*> (&f);
if (p == NULL) return false;
return Base::equals(*p, tol) && (key_ == p->key_);
}
/** error function z-h(x) */
inline Vector error_vector(const Config& x) const {
Key j = key_;
const X& xj = x[j];
return evaluateError(xj);
}
/**
* Linearize a non-linearFactor1 to get a GaussianFactor
* Ax-b \approx h(x0+dx)-z = h(x0) + A*dx - z
* Hence b = z - h(x0) = - error_vector(x)
*/
boost::shared_ptr<GaussianFactor> linearize(const Config& x) const {
const X& xj = x[key_];
Matrix A;
Vector b = -evaluateError(xj, A);
return GaussianFactor::shared_ptr(new GaussianFactor(key_, A, b,
this->sigma()));
}
/*
* Override this method to finish implementing a unary factor.
* If the optional Matrix reference argument is specified, it should compute
* both the function evaluation and its derivative in X.
*/
virtual Vector evaluateError(const X& x, boost::optional<Matrix&> H =
boost::none) const = 0;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NonlinearFactor", boost::serialization::base_object<NonlinearFactor>(*this));
ar & BOOST_SERIALIZATION_NVP(key_);
}
};
/**
* A Gaussian nonlinear factor that takes 2 parameters
* Note: cannot be serialized as contains function pointers
* Specialized derived classes could do this
*/
class NonlinearFactor2 : public NonlinearFactor<VectorConfig> {
*/
template<class Config, class Key1, class X1, class Key2, class X2>
class NonlinearFactor2: public NonlinearFactor<Config> {
private:
protected:
std::string key1_;
std::string key2_;
// The values of the keys. Not const to allow serialization
Key1 key1_;
Key2 key2_;
public:
typedef NonlinearFactor<Config> Base;
typedef NonlinearFactor2<Config, Key1, X1, Key2, X2> This;
Vector (*h_)(const Vector&, const Vector&);
Matrix (*H1_)(const Vector&, const Vector&);
Matrix (*H2_)(const Vector&, const Vector&);
public:
/** Constructor */
NonlinearFactor2(const Vector& z, // the measurement
const double sigma, // the variance
Vector (*h)(const Vector&, const Vector&), // the measurement function
const std::string& key1, // key of the first variable
Matrix (*H1)(const Vector&, const Vector&), // derivative of h in first variable
const std::string& key2, // key of the second variable
Matrix (*H2)(const Vector&, const Vector&));// derivative of h in second variable
/**
* Default Constructor for I/O
*/
NonlinearFactor2();
/** Print */
void print(const std::string& s = "") const;
/**
* Constructor
* @param j1 key of the first variable
* @param j2 key of the second variable
*/
NonlinearFactor2(double sigma, Key1 j1, Key2 j2) :
Base(sigma), key1_(j1), key2_(j2) {
this->keys_.push_back(key1_);
this->keys_.push_back(key2_);
}
/** Check if two factors are equal */
bool equals(const Factor<VectorConfig>& f, double tol=1e-9) const;
/** Print */
void print(const std::string& s = "") const {
std::cout << "NonlinearFactor2 " << s << std::endl;
std::cout << "key1: " << (std::string) key1_ << std::endl;
std::cout << "key2: " << (std::string) key2_ << std::endl;
Base::print("parent");
}
/** error function */
inline Vector error_vector(const VectorConfig& c) const {
return z_ - h_(c[key1_], c[key2_]);
}
/** Check if two factors are equal */
bool equals(const Factor<Config>& f, double tol = 1e-9) const {
const This* p = dynamic_cast<const This*> (&f);
if (p == NULL) return false;
return Base::equals(*p, tol) && (key1_ == p->key1_)
&& (key2_ == p->key2_);
}
/** Linearize a non-linearFactor2 to get a linearFactor2 */
boost::shared_ptr<GaussianFactor> linearize(const VectorConfig& c) const;
};
/** error function z-h(x1,x2) */
inline Vector error_vector(const Config& x) const {
const X1& x1 = x[key1_];
const X2& x2 = x[key2_];
return evaluateError(x1, x2);
}
/* ************************************************************************* */
/**
* Linearize a non-linearFactor1 to get a GaussianFactor
* Ax-b \approx h(x1+dx1,x2+dx2)-z = h(x1,x2) + A2*dx1 + A2*dx2 - z
* Hence b = z - h(x1,x2) = - error_vector(x)
*/
boost::shared_ptr<GaussianFactor> linearize(const Config& c) const {
const X1& x1 = c[key1_];
const X2& x2 = c[key2_];
Matrix A1, A2;
Vector b = -evaluateError(x1, x2, A1, A2);
return GaussianFactor::shared_ptr(new GaussianFactor(key1_, A1, key2_,
A2, b, this->sigma()));
}
/** methods to retrieve both keys */
inline const Key1& key1() const {
return key1_;
}
inline const Key2& key2() const {
return key2_;
}
/*
* Override this method to finish implementing a binary factor.
* If any of the optional Matrix reference arguments are specified, it should compute
* both the function evaluation and its derivative(s) in X1 (and/or X2).
*/
virtual Vector evaluateError(const X1&, const X2&,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
boost::none) const = 0;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NonlinearFactor", boost::serialization::base_object<NonlinearFactor>(*this));
ar & BOOST_SERIALIZATION_NVP(key1_);
ar & BOOST_SERIALIZATION_NVP(key2_);
}
};
/* ************************************************************************* */
}

5
cpp/Ordering.cpp
View File

@ -20,6 +20,7 @@ using namespace boost::assign;
#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
/*
class ordering_key_visitor : public boost::default_bfs_visitor {
public:
ordering_key_visitor(Ordering& ordering_in) : ordering_(ordering_in) {}
@ -36,8 +37,8 @@ public:
//boost::add_edge(v1, v2, g);
}
};
/* ************************************************************************* */
*/
/* ************************************************************************* *
Ordering::Ordering(const map<string, string>& p_map) {
SGraph g;

2
cpp/Ordering.h
View File

@ -42,7 +42,7 @@ namespace gtsam {
/**
* Generate the ordering from a spanning tree represented by its parent map
*/
Ordering(const std::map<std::string, std::string>& p_map);
//Ordering(const std::map<std::string, std::string>& p_map);
/**
* Remove a set of keys from an ordering

2
cpp/Point2.h
View File

@ -70,7 +70,7 @@ namespace gtsam {
/** Lie group functions */
/** Global print calls member function */
inline void print(const Point2& p, std::string& s) { p.print(s); }
inline void print(const Point2& p, const std::string& s) { p.print(s); }
inline void print(const Point2& p) { p.print(); }
/** Dimensionality of the tangent space */

20
cpp/Point2Prior.h
View File

@ -4,13 +4,21 @@
#include "NonlinearFactor.h"
#include "simulated2D.h"
namespace gtsam {
namespace simulated2D {
class Point2Prior: public NonlinearFactor1 {
public:
Point2Prior(const Vector& mu, double sigma, const std::string& key) :
NonlinearFactor1(mu, sigma, prior, key, Dprior) {
struct Point2Prior: public gtsam::NonlinearFactor1<VectorConfig, std::string, Vector> {
Vector z_;
Point2Prior(const Vector& z, double sigma, const std::string& key) :
gtsam::NonlinearFactor1<VectorConfig, std::string, Vector>(sigma, key), z_(z) {
}
Vector evaluateError(const Vector& x, boost::optional<Matrix&> H = boost::none) const {
if (H) *H = Dprior(x);
return prior(x) - z_;
}
};
} // namespace gtsam
} // namespace simulated2D

2
cpp/Point3.h
View File

@ -79,7 +79,7 @@ namespace gtsam {
/** Global print calls member function */
inline void print(const Point3& p, std::string& s) { p.print(s); }
inline void print(const Point3& p, const std::string& s) { p.print(s); }
inline void print(const Point3& p) { p.print(); }
/** return DOF, dimensionality of tangent space */

20
cpp/Pose2Config.cpp
View File

@ -12,25 +12,17 @@ using namespace std;
namespace gtsam {
/** Explicit instantiation of templated methods and functions */
template class LieConfig<Pose2>;
template size_t dim(const LieConfig<Pose2>& c);
template LieConfig<Pose2> expmap(const LieConfig<Pose2>& c, const Vector& delta);
template LieConfig<Pose2> expmap(const LieConfig<Pose2>& c, const VectorConfig& delta);
template class LieConfig<Symbol<Pose2,'x'>,Pose2>;
template size_t dim(const Pose2Config& c);
template Pose2Config expmap(const Pose2Config& c, const Vector& delta);
template Pose2Config expmap(const Pose2Config& c, const VectorConfig& delta);
/* ************************************************************************* */
// TODO: local version, should probably defined in LieConfig
static string symbol(char c, int index) {
stringstream ss;
ss << c << index;
return ss.str();
}
/* ************************************************************************* */
Pose2Config pose2Circle(size_t n, double R, char c) {
Pose2Config pose2Circle(size_t n, double R) {
Pose2Config x;
double theta = 0, dtheta = 2*M_PI/n;
for(size_t i=0;i<n;i++, theta+=dtheta)
x.insert(symbol(c,i), Pose2(cos(theta), sin(theta), M_PI_2 + theta));
x.insert(i, Pose2(cos(theta), sin(theta), M_PI_2 + theta));
return x;
}

4
cpp/Pose2Config.h
View File

@ -14,7 +14,7 @@ namespace gtsam {
/**
* Pose2Config is now simply a typedef
*/
typedef LieConfig<Pose2> Pose2Config;
typedef LieConfig<Symbol<Pose2,'x'>,Pose2> Pose2Config;
/**
* Create a circle of n 2D poses tangent to circle of radius R, first pose at (R,0)
@ -23,6 +23,6 @@ namespace gtsam {
* @param c character to use for keys
* @return circle of n 2D poses
*/
Pose2Config pose2Circle(size_t n, double R, char c = 'p');
Pose2Config pose2Circle(size_t n, double R);
} // namespace

2
cpp/Pose2Factor.h
View File

@ -12,6 +12,6 @@
namespace gtsam {
/** This is just a typedef now */
typedef BetweenFactor<Pose2, Pose2Config> Pose2Factor;
typedef BetweenFactor<Pose2Config, Pose2Config::Key, Pose2> Pose2Factor;
} /// namespace gtsam

5
cpp/Pose2Graph.cpp
View File

@ -3,10 +3,11 @@
* @brief A factor graph for the 2D PoseSLAM problem
* @authors Frank Dellaert, Viorela Ila
*/
#include "Pose2Graph.h"
#include "FactorGraph-inl.h"
#include "NonlinearFactorGraph-inl.h"
#include "graph-inl.h"
#include "Pose2Graph.h"
using namespace std;
using namespace gtsam;
@ -14,7 +15,7 @@ using namespace gtsam;
namespace gtsam {
// explicit instantiation so all the code is there and we can link with it
template class FactorGraph<NonlinearFactor<gtsam::Pose2Config> > ;
template class FactorGraph<NonlinearFactor<Pose2Config> > ;
template class NonlinearFactorGraph<Pose2Config> ;
//template class NonlinearOptimizer<Pose2Graph, Pose2Config> ;

10
cpp/Pose2Graph.h
View File

@ -7,10 +7,10 @@
#pragma once
#include "NonlinearFactorGraph.h"
#include "NonlinearEquality.h"
#include "Pose2Factor.h"
#include "Pose2Config.h"
#include "NonlinearFactorGraph.h"
#include "NonlinearEquality.h"
namespace gtsam {
@ -33,7 +33,7 @@ namespace gtsam {
/**
* Add a factor without having to do shared factor dance
*/
inline void add(const std::string& key1, const std::string& key2,
inline void add(const Pose2Config::Key& key1, const Pose2Config::Key& key2,
const Pose2& measured, const Matrix& covariance) {
push_back(sharedFactor(new Pose2Factor(key1, key2, measured, covariance)));
}
@ -43,8 +43,8 @@ namespace gtsam {
* @param key of pose
* @param pose which pose to constrain it to
*/
inline void addConstraint(const std::string& key, const Pose2& pose = Pose2()) {
push_back(sharedFactor(new NonlinearEquality<Pose2Config>(key, pose)));
inline void addConstraint(const Pose2Config::Key& key, const Pose2& pose = Pose2()) {
push_back(sharedFactor(new NonlinearEquality<Pose2Config,Pose2Config::Key,Pose2>(key, pose)));
}
private:

12
cpp/Pose3Config.cpp
View File

@ -12,10 +12,10 @@ using namespace std;
namespace gtsam {
/** Explicit instantiation of templated methods and functions */
template class LieConfig<Pose3>;
template size_t dim(const LieConfig<Pose3>& c);
template LieConfig<Pose3> expmap(const LieConfig<Pose3>& c, const Vector& delta);
template LieConfig<Pose3> expmap(const LieConfig<Pose3>& c, const VectorConfig& delta);
template class LieConfig<Symbol<Pose3,'x'>,Pose3>;
template size_t dim(const Pose3Config& c);
template Pose3Config expmap(const Pose3Config& c, const Vector& delta);
template Pose3Config expmap(const Pose3Config& c, const VectorConfig& delta);
/* ************************************************************************* */
// TODO: local version, should probably defined in LieConfig
@ -26,13 +26,13 @@ namespace gtsam {
}
/* ************************************************************************* */
Pose3Config pose3Circle(size_t n, double R, char c) {
Pose3Config pose3Circle(size_t n, double R) {
Pose3Config x;
double theta = 0, dtheta = 2*M_PI/n;
// Vehicle at p0 is looking towards y axis
Rot3 R0(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1));
for (size_t i = 0; i < n; i++, theta += dtheta)
x.insert(symbol(c,i), Pose3(R0 * Rot3::yaw(-theta), Point3(cos(theta),sin(theta),0)));
x.insert(i, Pose3(R0 * Rot3::yaw(-theta), Point3(cos(theta),sin(theta),0)));
return x;
}

4
cpp/Pose3Config.h
View File

@ -14,7 +14,7 @@ namespace gtsam {
/**
* Pose3Config is now simply a typedef
*/
typedef LieConfig<Pose3> Pose3Config;
typedef LieConfig<Symbol<Pose3,'x'>,Pose3> Pose3Config;
/**
* Create a circle of n 3D poses tangent to circle of radius R, first pose at (R,0,0)
@ -24,6 +24,6 @@ namespace gtsam {
* @param c character to use for keys
* @return circle of n 2D poses
*/
Pose3Config pose3Circle(size_t n, double R, char c = 'p');
Pose3Config pose3Circle(size_t n, double R);
} // namespace

2
cpp/Pose3Factor.h
View File

@ -14,6 +14,6 @@ namespace gtsam {
* A Factor for 3D pose measurements
* This is just a typedef now
*/
typedef BetweenFactor<Pose3, Pose3Config> Pose3Factor;
typedef BetweenFactor<Pose3Config,Pose3Config::Key,Pose3> Pose3Factor;
} /// namespace gtsam

10
cpp/Pose3Graph.h
View File

@ -7,10 +7,10 @@
#pragma once
#include "NonlinearFactorGraph.h"
#include "NonlinearEquality.h"
#include "Pose3Factor.h"
#include "Pose3Config.h"
#include "NonlinearFactorGraph.h"
#include "NonlinearEquality.h"
namespace gtsam {
@ -33,7 +33,7 @@ namespace gtsam {
/**
* Add a factor without having to do shared factor dance
*/
inline void add(const std::string& key1, const std::string& key2,
inline void add(const Pose3Config::Key& key1, const Pose3Config::Key& key2,
const Pose3& measured, const Matrix& covariance) {
push_back(sharedFactor(new Pose3Factor(key1, key2, measured, covariance)));
}
@ -43,8 +43,8 @@ namespace gtsam {
* @param key of pose
* @param pose which pose to constrain it to
*/
inline void addConstraint(const std::string& key, const Pose3& pose = Pose3()) {
push_back(sharedFactor(new NonlinearEquality<Pose3Config> (key, pose)));
inline void addConstraint(const Pose3Config::Key& key, const Pose3& pose =Pose3()) {
push_back(sharedFactor(new NonlinearEquality<Pose3Config,Pose3Config::Key,Pose3> (key, pose)));
}
private:

27
cpp/Simulated2DMeasurement.h
View File

@ -4,11 +4,26 @@
#include "NonlinearFactor.h"
#include "simulated2D.h"
namespace gtsam {
namespace simulated2D {
struct Simulated2DMeasurement : public NonlinearFactor2 {
Simulated2DMeasurement(const Vector& z, double sigma, const std::string& key1, const std::string& key2):
NonlinearFactor2(z, sigma, mea, key1, Dmea1, key2, Dmea2) {}
};
struct Simulated2DMeasurement: public gtsam::NonlinearFactor2<VectorConfig,
PoseKey, Vector, PointKey, Vector> {
} // namespace gtsam
Vector z_;
Simulated2DMeasurement(const Vector& z, double sigma, const std::string& j1,
const std::string& j2) :
z_(z), gtsam::NonlinearFactor2<VectorConfig, PoseKey, Vector, PointKey,
Vector>(sigma, j1, j2) {
}
Vector evaluateError(const Vector& x1, const Vector& x2, boost::optional<
Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const {
if (H1) *H1 = Dmea1(x1, x2);
if (H2) *H2 = Dmea2(x1, x2);
return mea(x1, x2) - z_;
}
};
} // namespace simulated2D

24
cpp/Simulated2DOdometry.h
View File

@ -4,11 +4,25 @@
#include "NonlinearFactor.h"
#include "simulated2D.h"
namespace gtsam {
namespace simulated2D {
struct Simulated2DOdometry : public NonlinearFactor2 {
Simulated2DOdometry(const Vector& z, double sigma, const std::string& key1, const std::string& key2):
NonlinearFactor2(z, sigma, odo, key1, Dodo1, key2, Dodo2) {}
};
struct Simulated2DOdometry: public gtsam::NonlinearFactor2<VectorConfig,
PoseKey, Vector, PointKey, Vector> {
Vector z_;
Simulated2DOdometry(const Vector& z, double sigma, const std::string& j1,
const std::string& j2) :
z_(z), gtsam::NonlinearFactor2<VectorConfig, PoseKey, Vector, PointKey,
Vector>(sigma, j1, j2) {
}
Vector evaluateError(const Vector& x1, const Vector& x2, boost::optional<
Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const {
if (H1) *H1 = Dodo1(x1, x2);
if (H2) *H2 = Dodo2(x1, x2);
return odo(x1, x2) - z_;
}
};
} // namespace gtsam

20
cpp/Simulated3D.cpp
View File

@ -6,49 +6,51 @@
#include "Simulated3D.h"
namespace gtsam {
namespace simulated3D {
Vector prior_3D (const Vector& x)
using namespace gtsam;
Vector prior (const Vector& x)
{
return x;
}
Matrix Dprior_3D(const Vector& x)
Matrix Dprior(const Vector& x)
{
Matrix H = eye((int) x.size());
return H;
}
Vector odo_3D(const Vector& x1, const Vector& x2)
Vector odo(const Vector& x1, const Vector& x2)
{
return x2 - x1;
}
Matrix Dodo1_3D(const Vector& x1, const Vector& x2)
Matrix Dodo1(const Vector& x1, const Vector& x2)
{
Matrix H = -1 * eye((int) x1.size());
return H;
}
Matrix Dodo2_3D(const Vector& x1, const Vector& x2)
Matrix Dodo2(const Vector& x1, const Vector& x2)
{
Matrix H = eye((int) x1.size());
return H;
}
Vector mea_3D(const Vector& x, const Vector& l)
Vector mea(const Vector& x, const Vector& l)
{
return l - x;
}
Matrix Dmea1_3D(const Vector& x, const Vector& l)
Matrix Dmea1(const Vector& x, const Vector& l)
{
Matrix H = -1 * eye((int) x.size());
return H;
}
Matrix Dmea2_3D(const Vector& x, const Vector& l)
Matrix Dmea2(const Vector& x, const Vector& l)
{
Matrix H = eye((int) x.size());
return H;

96
cpp/Simulated3D.h
View File

@ -1,47 +1,81 @@
/**
* @file Simulated3D.h
* @brief measurement functions and derivatives for simulated 3D robot
* @author Alex Cunningham
**/
* @file Simulated3D.h
* @brief measurement functions and derivatives for simulated 3D robot
* @author Alex Cunningham
**/
// \callgraph
#pragma once
#include "Matrix.h"
#include "VectorConfig.h"
#include "NonlinearFactor.h"
// \namespace
namespace gtsam {
namespace simulated3D {
/**
* Prior on a single pose
*/
Vector prior_3D (const Vector& x);
Matrix Dprior_3D(const Vector& x);
typedef gtsam::VectorConfig VectorConfig;
/**
* odometry between two poses
*/
Vector odo_3D(const Vector& x1, const Vector& x2);
Matrix Dodo1_3D(const Vector& x1, const Vector& x2);
Matrix Dodo2_3D(const Vector& x1, const Vector& x2);
/**
* measurement between landmark and pose
*/
Vector mea_3D(const Vector& x, const Vector& l);
Matrix Dmea1_3D(const Vector& x, const Vector& l);
Matrix Dmea2_3D(const Vector& x, const Vector& l);
struct Point2Prior3D : public NonlinearFactor1 {
Point2Prior3D(const Vector& mu, double sigma, const std::string& key):
NonlinearFactor1(mu, sigma, prior_3D, key, Dprior_3D) {}
struct PoseKey: public std::string {
};
struct PointKey: public std::string {
};
struct Simulated3DMeasurement : public NonlinearFactor2 {
Simulated3DMeasurement(const Vector& z, double sigma, const std::string& key1, const std::string& key2):
NonlinearFactor2(z, sigma, mea_3D, key1, Dmea1_3D, key2, Dmea2_3D) {}
/**
* Prior on a single pose
*/
Vector prior(const Vector& x);
Matrix Dprior(const Vector& x);
/**
* odometry between two poses
*/
Vector odo(const Vector& x1, const Vector& x2);
Matrix Dodo1(const Vector& x1, const Vector& x2);
Matrix Dodo2(const Vector& x1, const Vector& x2);
/**
* measurement between landmark and pose
*/
Vector mea(const Vector& x, const Vector& l);
Matrix Dmea1(const Vector& x, const Vector& l);
Matrix Dmea2(const Vector& x, const Vector& l);
struct Point2Prior3D: public gtsam::NonlinearFactor1<VectorConfig, PoseKey,
Vector> {
Vector z_;
Point2Prior3D(const Vector& z, double sigma, const PoseKey& j) :
gtsam::NonlinearFactor1<VectorConfig, PoseKey, Vector>(sigma, j), z_(z) {
}
Vector evaluateError(const Vector& x, boost::optional<Matrix&> H =
boost::none) {
if (H) *H = Dprior(x);
return prior(x) - z_;
}
};
} // namespace gtsam
struct Simulated3DMeasurement: public gtsam::NonlinearFactor2<VectorConfig,
PoseKey, Vector, PointKey, Vector> {
Vector z_;
Simulated3DMeasurement(const Vector& z, double sigma, PoseKey& j1,
PointKey j2) :
z_(z), gtsam::NonlinearFactor2<VectorConfig, PoseKey, Vector, PointKey,
Vector>(sigma, j1, j2) {
}
Vector evaluateError(const Vector& x1, const Vector& x2, boost::optional<
Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) {
if (H1) *H1 = Dmea1(x1, x2);
if (H2) *H2 = Dmea2(x1, x2);
return mea(x1, x2) - z_;
}
};
} // namespace simulated3D

109
cpp/VSLAMConfig.cpp
View File

@ -11,109 +11,32 @@
#include <boost/tuple/tuple.hpp>
#include "VSLAMConfig.h"
#include "LieConfig-inl.h"
using namespace std;
// trick from some reading group
#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
#define SIGMA 1.0
namespace gtsam {
/* ************************************************************************* */
// Exponential map
VSLAMConfig expmap(const VSLAMConfig& c, const VectorConfig & delta) {
VSLAMConfig newConfig;
for (map<string, Vector>::const_iterator it = delta.begin(); it
!= delta.end(); it++) {
string key = it->first;
if (key[0] == 'x') {
int cameraNumber = atoi(key.substr(1, key.size() - 1).c_str());
if (c.cameraPoseExists(cameraNumber)) {
Pose3 basePose = c.cameraPose(cameraNumber);
newConfig.addCameraPose(cameraNumber, expmap(basePose, it->second));
}
}
if (key[0] == 'l') {
int landmarkNumber = atoi(key.substr(1, key.size() - 1).c_str());
if (c.landmarkPointExists(landmarkNumber)) {
Point3 basePoint = c.landmarkPoint(landmarkNumber);
newConfig.addLandmarkPoint(landmarkNumber, expmap(basePoint, it->second));
}
}
/* ************************************************************************* */
// Exponential map
VSLAMConfig expmap(const VSLAMConfig& x0, const VectorConfig & delta) {
VSLAMConfig x;
x.poses_ = expmap(x0.poses_, delta);
x.points_ = expmap(x0.points_, delta);
return x;
}
return newConfig;
}
/* ************************************************************************* */
void VSLAMConfig::print(const std::string& s) const
{
printf("%s:\n", s.c_str());
printf("Camera Poses:\n");
for(PoseMap::const_iterator it = cameraIteratorBegin(); it != cameraIteratorEnd(); it++)
{
printf("x%d:\n", it->first);
it->second.print();
}
printf("Landmark Points:\n");
for(PointMap::const_iterator it = landmarkIteratorBegin(); it != landmarkIteratorEnd(); it++)
{
printf("l%d:\n", (*it).first);
(*it).second.print();
}
}
/* ************************************************************************* */
bool VSLAMConfig::equals(const VSLAMConfig& c, double tol) const {
for (PoseMap::const_iterator it = cameraIteratorBegin(); it
!= cameraIteratorEnd(); it++) {
if (!c.cameraPoseExists(it->first)) return false;
if (!it->second.equals(c.cameraPose(it->first), tol)) return false;
/* ************************************************************************* */
void VSLAMConfig::print(const std::string& s) const {
printf("%s:\n", s.c_str());
poses_.print("Poses");
points_.print("Points");
}
for (PointMap::const_iterator it = landmarkIteratorBegin(); it
!= landmarkIteratorEnd(); it++) {
if (!c.landmarkPointExists(it->first)) return false;
if (!it->second.equals(c.landmarkPoint(it->first), tol)) return false;
/* ************************************************************************* */
bool VSLAMConfig::equals(const VSLAMConfig& c, double tol) const {
return poses_.equals(c.poses_, tol) && points_.equals(c.points_, tol);
}
return true;
}
/* ************************************************************************* */
void VSLAMConfig::addCameraPose(const int i, Pose3 cp)
{
pair<int, Pose3> camera;
camera.first = i;
camera.second = cp;
cameraPoses_.insert(camera);
}
/* ************************************************************************* */
void VSLAMConfig::addLandmarkPoint(const int i, Point3 lp)
{
pair<int, Point3> landmark;
landmark.first = i;
landmark.second = lp;
landmarkPoints_.insert(landmark);
}
/* ************************************************************************* */
void VSLAMConfig::removeCameraPose(const int i)
{
if(cameraPoseExists(i))
cameraPoses_.erase(i);
}
/* ************************************************************************* */
void VSLAMConfig::removeLandmarkPose(const int i)
{
if(landmarkPointExists(i))
landmarkPoints_.erase(i);
}
/* ************************************************************************* */

110
cpp/VSLAMConfig.h
View File

@ -9,10 +9,8 @@
#include <map>
#include <vector>
#include <fstream>
#include "VectorConfig.h"
#include "Pose3.h"
#include "Cal3_S2.h"
#include "Testable.h"
#include "Pose3Config.h"
#pragma once
@ -23,91 +21,59 @@ namespace gtsam{
*/
class VSLAMConfig : Testable<VSLAMConfig> {
private:
typedef std::map<int, Pose3> PoseMap;
typedef std::map<int, Point3> PointMap;
PointMap landmarkPoints_;
PoseMap cameraPoses_;
public:
typedef Symbol<Pose3,'x'> PoseKey;
typedef Symbol<Point3,'l'> PointKey;
private:
LieConfig<PoseKey, Pose3> poses_;
LieConfig<PointKey, Point3> points_;
public:
public:
typedef std::map<std::string, Vector>::const_iterator const_iterator;
typedef PoseMap::const_iterator const_Pose_iterator;
typedef PointMap::const_iterator const_Point_iterator;
/**
* default constructor
*/
VSLAMConfig() {}
/*
* copy constructor
*/
VSLAMConfig(const VSLAMConfig& original):
cameraPoses_(original.cameraPoses_), landmarkPoints_(original.landmarkPoints_){}
PoseMap::const_iterator cameraIteratorBegin() const { return cameraPoses_.begin();}
PoseMap::const_iterator cameraIteratorEnd() const { return cameraPoses_.end();}
PointMap::const_iterator landmarkIteratorBegin() const { return landmarkPoints_.begin();}
PointMap::const_iterator landmarkIteratorEnd() const { return landmarkPoints_.end();}
/**
* print
*/
void print(const std::string& s = "") const;
/**
* Retrieve robot pose
*/
bool cameraPoseExists(int i) const
{
PoseMap::const_iterator it = cameraPoses_.find(i);
if (it==cameraPoses_.end())
return false;
return true;
}
Pose3 cameraPose(int i) const {
PoseMap::const_iterator it = cameraPoses_.find(i);
if (it==cameraPoses_.end())
throw(std::invalid_argument("robotPose: invalid key"));
return it->second;
}
/**
* Check whether a landmark point exists
*/
bool landmarkPointExists(int i) const
{
PointMap::const_iterator it = landmarkPoints_.find(i);
if (it==landmarkPoints_.end())
return false;
return true;
}
/**
* Retrieve landmark point
*/
Point3 landmarkPoint(int i) const {
PointMap::const_iterator it = landmarkPoints_.find(i);
if (it==landmarkPoints_.end())
throw(std::invalid_argument("markerPose: invalid key"));
return it->second;
}
/**
* check whether two configs are equal
*/
bool equals(const VSLAMConfig& c, double tol=1e-6) const;
void addCameraPose(const int i, Pose3 cp);
void addLandmarkPoint(const int i, Point3 lp);
void removeCameraPose(const int i);
void removeLandmarkPose(const int i);
/**
* Get Poses or Points
*/
inline const Pose3& operator[](const PoseKey& key) const {return poses_[key];}
inline const Point3& operator[](const PointKey& key) const {return points_[key];}
void clear() {landmarkPoints_.clear(); cameraPoses_.clear();}
// (Awkwardly named) backwards compatibility:
inline size_t size(){
return landmarkPoints_.size() + cameraPoses_.size();
}
inline bool cameraPoseExists (const PoseKey& key) const {return poses_.exists(key);}
inline bool landmarkPointExists(const PointKey& key) const {return points_.exists(key);}
inline Pose3 cameraPose (const PoseKey& key) const {return poses_[key];}
inline Point3 landmarkPoint(const PointKey& key) const {return points_[key];}
inline size_t size() const {return points_.size() + poses_.size();}
inline size_t dim() const {return gtsam::dim(points_) + gtsam::dim(poses_);}
// Imperative functions:
inline void addCameraPose(const PoseKey& key, Pose3 cp) {poses_.insert(key,cp);}
inline void addLandmarkPoint(const PointKey& key, Point3 lp) {points_.insert(key,lp);}
inline void removeCameraPose(const PoseKey& key) { poses_.erase(key);}
inline void removeLandmarkPose(const PointKey& key) { points_.erase(key);}
inline void clear() {points_.clear(); poses_.clear();}
friend VSLAMConfig expmap(const VSLAMConfig& c, const VectorConfig & delta);
};
@ -119,7 +85,5 @@ class VSLAMConfig : Testable<VSLAMConfig> {
* Needed for use in nonlinear optimization
*/
VSLAMConfig expmap(const VSLAMConfig& c, const VectorConfig & delta);
} // namespace gtsam

99
cpp/VSLAMFactor.cpp
View File

@ -4,88 +4,37 @@
* @author Alireza Fathi
*/
#include "VSLAMConfig.h"
#include <boost/bind.hpp>
#include <boost/bind/placeholders.hpp>
#include "VSLAMFactor.h"
#include "Pose3.h"
#include "SimpleCamera.h"
using namespace std;
namespace gtsam{
namespace gtsam {
/* ************************************************************************* */
VSLAMFactor::VSLAMFactor() {
/// Arbitrary values
cameraFrameNumber_ = 111;
landmarkNumber_ = 222;
cameraFrameName_ = symbol('x',cameraFrameNumber_);
landmarkName_ = symbol('l',landmarkNumber_);
keys_.push_back(cameraFrameName_);
keys_.push_back(landmarkName_);
K_ = shared_ptrK(new Cal3_S2(444,555,666,777,888));
}
/* ************************************************************************* */
VSLAMFactor::VSLAMFactor(const Point2& z, double sigma, int cn, int ln, const shared_ptrK &K)
: NonlinearFactor<VSLAMConfig>(z.vector(), sigma)
{
cameraFrameNumber_ = cn;
landmarkNumber_ = ln;
cameraFrameName_ = symbol('x',cameraFrameNumber_);
landmarkName_ = symbol('l',landmarkNumber_);
keys_.push_back(cameraFrameName_);
keys_.push_back(landmarkName_);
K_ = K;
}
/* ************************************************************************* */
VSLAMFactor::VSLAMFactor() {
/// Arbitrary values
K_ = shared_ptrK(new Cal3_S2(444, 555, 666, 777, 888));
}
/* ************************************************************************* */
VSLAMFactor::VSLAMFactor(const Point2& z, double sigma, int cn, int ln,
const shared_ptrK &K) :
VSLAMFactorBase(sigma, cn, ln), z_(z), K_(K) {
}
/* ************************************************************************* */
void VSLAMFactor::print(const std::string& s) const {
printf("%s %s %s\n", s.c_str(), cameraFrameName_.c_str(), landmarkName_.c_str());
gtsam::print(this->z_, s+".z");
}
/* ************************************************************************* */
void VSLAMFactor::print(const std::string& s) const {
VSLAMFactorBase::print(s);
z_.print(s + ".z");
}
/* ************************************************************************* */
bool VSLAMFactor::equals(const VSLAMFactor& p, double tol) const {
if (&p == NULL) return false;
if (cameraFrameNumber_ != p.cameraFrameNumber_ || landmarkNumber_ != p.landmarkNumber_) return false;
if (!equal_with_abs_tol(this->z_,p.z_,tol)) return false;
return true;
}
/* ************************************************************************* */
Vector VSLAMFactor::predict(const VSLAMConfig& c) const {
Pose3 pose = c.cameraPose(cameraFrameNumber_);
Point3 landmark = c.landmarkPoint(landmarkNumber_);
return project(SimpleCamera(*K_,pose), landmark).vector();
}
/* ************************************************************************* */
Vector VSLAMFactor::error_vector(const VSLAMConfig& c) const {
// Right-hand-side b = (z - h(x))/sigma
Vector h = predict(c);
return (this->z_ - h);
}
/* ************************************************************************* */
GaussianFactor::shared_ptr VSLAMFactor::linearize(const VSLAMConfig& c) const
{
// get arguments from config
Pose3 pose = c.cameraPose(cameraFrameNumber_);
Point3 landmark = c.landmarkPoint(landmarkNumber_);
SimpleCamera camera(*K_,pose);
// Jacobians
Matrix Dh1 = Dproject_pose(camera, landmark);
Matrix Dh2 = Dproject_point(camera, landmark);
// Right-hand-side b = (z - h(x))
Vector h = project(camera, landmark).vector();
Vector b = this->z_ - h;
// Make new linearfactor, divide by sigma
GaussianFactor::shared_ptr
p(new GaussianFactor(cameraFrameName_, Dh1, landmarkName_, Dh2, b, this->sigma_));
return p;
}
/* ************************************************************************* */
bool VSLAMFactor::equals(const VSLAMFactor& p, double tol) const {
return VSLAMFactorBase::equals(p, tol) && z_.equals(p.z_, tol)
&& K_->equals(*p.K_, tol);
}
/* ************************************************************************* */
} // namespace gtsam

130
cpp/VSLAMFactor.h
View File

@ -6,88 +6,86 @@
#pragma once
#include <boost/optional.hpp>
#include "NonlinearFactor.h"
#include "GaussianFactor.h"
#include "SimpleCamera.h"
#include "VSLAMConfig.h"
#include "Cal3_S2.h"
#include "Testable.h"
namespace gtsam {
class VSLAMConfig;
/**
* Non-linear factor for a constraint derived from a 2D measurement,
* i.e. the main building block for visual SLAM.
*/
class VSLAMFactor : public NonlinearFactor<VSLAMConfig>, Testable<VSLAMFactor>
{
private:
int cameraFrameNumber_, landmarkNumber_;
std::string cameraFrameName_, landmarkName_;
boost::shared_ptr<Cal3_S2> K_; // Calibration stored in each factor. FD: need to think about this.
typedef NonlinearFactor<VSLAMConfig> ConvenientFactor;
public:
typedef boost::shared_ptr<VSLAMFactor> shared_ptr; // shorthand for a smart pointer to a factor
typedef NonlinearFactor2<VSLAMConfig, VSLAMConfig::PoseKey,
Pose3, VSLAMConfig::PointKey, Point3> VSLAMFactorBase;
/**
* Default constructor
* Non-linear factor for a constraint derived from a 2D measurement,
* i.e. the main building block for visual SLAM.
*/
VSLAMFactor();
class VSLAMFactor: public VSLAMFactorBase , Testable<VSLAMFactor> {
private:
/**
* Constructor
* @param z is the 2 dimensional location of point in image (the measurement)
* @param sigma is the standard deviation
* @param cameraFrameNumber is basically the frame number
* @param landmarkNumber is the index of the landmark
* @param K the constant calibration
*/
VSLAMFactor(const Point2& z, double sigma, int cameraFrameNumber, int landmarkNumber, const shared_ptrK & K);
// Keep a copy of measurement and calibration for I/O
Point2 z_;
boost::shared_ptr<Cal3_S2> K_;
public:
/**
* print
* @param s optional string naming the factor
*/
void print(const std::string& s="VSLAMFactor") const;
// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<VSLAMFactor> shared_ptr;
/**
* equals
*/
bool equals(const VSLAMFactor&, double tol=1e-9) const;
/**
* Default constructor
*/
VSLAMFactor();
/**
* predict the measurement
*/
Vector predict(const VSLAMConfig&) const;
/**
* Constructor
* @param z is the 2 dimensional location of point in image (the measurement)
* @param sigma is the standard deviation
* @param cameraFrameNumber is basically the frame number
* @param landmarkNumber is the index of the landmark
* @param K the constant calibration
*/
VSLAMFactor(const Point2& z, double sigma, int cameraFrameNumber,
int landmarkNumber, const shared_ptrK & K);
/**
* calculate the error of the factor
*/
Vector error_vector(const VSLAMConfig&) const;
/**
* print
* @param s optional string naming the factor
*/
void print(const std::string& s = "VSLAMFactor") const;
/**
* linerarization
*/
GaussianFactor::shared_ptr linearize(const VSLAMConfig&) const;
/**
* equals
*/
bool equals(const VSLAMFactor&, double tol = 1e-9) const;
int getCameraFrameNumber() const { return cameraFrameNumber_; }
int getLandmarkNumber() const { return landmarkNumber_; }
/** h(x) */
Point2 predict(const Pose3& pose, const Point3& point) const {
return SimpleCamera(*K_, pose).project(point);
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(cameraFrameNumber_);
ar & BOOST_SERIALIZATION_NVP(landmarkNumber_);
ar & BOOST_SERIALIZATION_NVP(cameraFrameName_);
ar & BOOST_SERIALIZATION_NVP(landmarkName_);
ar & BOOST_SERIALIZATION_NVP(K_);
}
};
/** h(x)-z */
Vector evaluateError(const Pose3& pose, const Point3& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
SimpleCamera camera(*K_, pose);
if (H1) *H1=Dproject_pose(camera,point);
if (H2) *H2=Dproject_point(camera,point);
Point2 reprojectionError(project(camera, point) - z_);
return reprojectionError.vector();
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(key1_);
ar & BOOST_SERIALIZATION_NVP(key2_);
ar & BOOST_SERIALIZATION_NVP(z_);
ar & BOOST_SERIALIZATION_NVP(K_);
}
};
}

12
cpp/VSLAMGraph.cpp
View File

@ -33,10 +33,8 @@ bool compareLandmark(const std::string& key,
/* ************************************************************************* */
void VSLAMGraph::addLandmarkConstraint(int j, const gtsam::Point3& p) {
typedef NonlinearEquality<VSLAMConfig> NLE;
VSLAMConfig feasible;
feasible.addLandmarkPoint(j,p);
boost::shared_ptr<NLE> factor(new NLE(symbol('l',j), feasible, 3, compareLandmark));
typedef NonlinearEquality<VSLAMConfig,VSLAMConfig::PointKey,Point3> NLE;
boost::shared_ptr<NLE> factor(new NLE(j, p));
push_back(factor);
}
@ -50,10 +48,8 @@ bool compareCamera(const std::string& key,
/* ************************************************************************* */
void VSLAMGraph::addCameraConstraint(int j, const gtsam::Pose3& p) {
typedef NonlinearEquality<VSLAMConfig> NLE;
VSLAMConfig feasible;
feasible.addCameraPose(j,p);
boost::shared_ptr<NLE> factor(new NLE(symbol('x',j), feasible, 6, compareCamera));
typedef NonlinearEquality<VSLAMConfig,VSLAMConfig::PoseKey,Pose3> NLE;
boost::shared_ptr<NLE> factor(new NLE(j,p));
push_back(factor);
}

2
cpp/VSLAMGraph.h
View File

@ -12,9 +12,9 @@
#include <set>
#include <fstream>
#include "VSLAMFactor.h"
#include "NonlinearFactorGraph.h"
#include "FactorGraph-inl.h"
#include "VSLAMFactor.h"
#include "VSLAMConfig.h"
#include "Testable.h"

110
cpp/graph-inl.h
View File

@ -1,19 +1,15 @@
/*
* graph-inl.h
*
* Created on: Jan 11, 2010
* Author: nikai
* Description: Graph algorithm using boost library
* @brief Graph algorithm using boost library
* @author: Kai Ni
*/
#pragma once
#include <boost/foreach.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/shared_ptr.hpp>
#include "graph.h"
using namespace std;
@ -21,31 +17,22 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
/**
* type definitions
*/
typedef boost::adjacency_list<
boost::vecS, boost::vecS, boost::undirectedS,
boost::property<boost::vertex_name_t, std::string>,
boost::property<boost::edge_weight_t, double> > SDGraph;
typedef boost::graph_traits<SDGraph>::vertex_descriptor BoostVertex;
typedef boost::graph_traits<SDGraph>::vertex_iterator BoostVertexIterator;
// some typedefs we need
//typedef boost::graph_traits<SDGraph>::vertex_iterator BoostVertexIterator;
//typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS,
// boost::property<boost::vertex_name_t, std::string> > SGraph;
//typedef boost::graph_traits<SGraph>::vertex_descriptor SVertex;
typedef boost::adjacency_list<
boost::vecS, boost::vecS, boost::directedS,
boost::property<boost::vertex_name_t, string> > SGraph;
typedef boost::graph_traits<SGraph>::vertex_descriptor SVertex;
/* ************************************************************************* */
/**
* Convert the factor graph to a boost undirected graph
*/
template<class G, class F>
SDGraph toBoostGraph(const G& graph) {
template<class G, class F, class Key>
SDGraph<Key> toBoostGraph(const G& graph) {
// convert the factor graph to boost graph
SDGraph g(0);
map<string, BoostVertex> key2vertex;
SDGraph<Key> g(0);
typedef typename boost::graph_traits<SDGraph<Key> >::vertex_descriptor BoostVertex;
map<Key, BoostVertex> key2vertex;
BoostVertex v1, v2;
BOOST_FOREACH(F factor, graph) {
if (factor->keys().size() > 2)
@ -54,8 +41,8 @@ SDGraph toBoostGraph(const G& graph) {
if (factor->keys().size() == 1)
continue;
string key1 = factor->keys().front();
string key2 = factor->keys().back();
Key key1 = factor->key1();
Key key2 = factor->key2();
if (key2vertex.find(key1) == key2vertex.end()) {
v1 = add_vertex(key1, g);
@ -77,16 +64,13 @@ SDGraph toBoostGraph(const G& graph) {
}
/* ************************************************************************* */
/**
* build the graph corresponding to the predecessor map. Excute action for each edge.
*/
template<class G, class V>
boost::tuple<G, V, map<string, V> > predecessorMap2Graph(const map<string, string>& p_map) {
template<class G, class V, class Key>
boost::tuple<G, V, map<Key, V> > predecessorMap2Graph(const PredecessorMap<Key>& p_map) {
G g(0);
map<string, V> key2vertex;
map<Key, V> key2vertex;
V v1, v2, root;
string child, parent;
Key child, parent;
bool foundRoot = false;
FOREACH_PAIR(child, parent, p_map) {
if (key2vertex.find(child) == key2vertex.end()) {
@ -101,7 +85,7 @@ boost::tuple<G, V, map<string, V> > predecessorMap2Graph(const map<string, strin
} else
v2 = key2vertex[parent];
if (child.compare(parent) == 0) {
if (child==parent) {
root = v1;
foundRoot = true;
} else
@ -111,49 +95,47 @@ boost::tuple<G, V, map<string, V> > predecessorMap2Graph(const map<string, strin
if (!foundRoot)
throw invalid_argument("predecessorMap2Graph: invalid predecessor map!");
return boost::tuple<G, V, map<string, V> >(g, root, key2vertex);
return boost::tuple<G, V, map<Key, V> >(g, root, key2vertex);
}
/* ************************************************************************* */
/**
* Visit each edge and compose the poses
*/
template <class V, class Pose, class PoseConfig>
template <class V,class Pose, class Config>
class compose_key_visitor : public boost::default_bfs_visitor {
public:
compose_key_visitor(boost::shared_ptr<PoseConfig> config_in) { config = config_in; }
template <typename Edge, typename Graph> void tree_edge(Edge edge, const Graph& g) const {
string key_from = boost::get(boost::vertex_name, g, boost::source(edge, g));
string key_to = boost::get(boost::vertex_name, g, boost::target(edge, g));
Pose relativePose = boost::get(boost::edge_weight, g, edge);
config->insert(key_to, compose(relativePose, config->get(key_from)));
}
private:
boost::shared_ptr<PoseConfig> config;
boost::shared_ptr<Config> config_;
public:
compose_key_visitor(boost::shared_ptr<Config> config_in) {config_ = config_in;}
template <typename Edge, typename Graph> void tree_edge(Edge edge, const Graph& g) const {
typename Config::Key key_from = boost::get(boost::vertex_name, g, boost::source(edge, g));
typename Config::Key key_to = boost::get(boost::vertex_name, g, boost::target(edge, g));
Pose relativePose = boost::get(boost::edge_weight, g, edge);
config_->insert(key_to, compose(relativePose, (*config_)[key_from]));
}
};
/* ************************************************************************* */
/**
* Compose the poses by following the chain sepcified by the spanning tree
*/
template<class G, class Factor, class Pose, class Config>
boost::shared_ptr<Config> composePoses(const G& graph, const map<string, string>& tree,
boost::shared_ptr<Config> composePoses(const G& graph, const PredecessorMap<typename Config::Key>& tree,
const Pose& rootPose) {
//TODO: change edge_weight_t to edge_pose_t
typedef typename boost::adjacency_list<
boost::vecS, boost::vecS, boost::directedS,
boost::property<boost::vertex_name_t, string>,
boost::property<boost::vertex_name_t, typename Config::Key>,
boost::property<boost::edge_weight_t, Pose> > PoseGraph;
typedef typename boost::graph_traits<PoseGraph>::vertex_descriptor PoseVertex;
typedef typename boost::graph_traits<PoseGraph>::edge_descriptor PoseEdge;
PoseGraph g;
PoseVertex root;
map<string, PoseVertex> key2vertex;
boost::tie(g, root, key2vertex) = predecessorMap2Graph<PoseGraph, PoseVertex>(tree);
map<typename Config::Key, PoseVertex> key2vertex;
boost::tie(g, root, key2vertex) =
predecessorMap2Graph<PoseGraph, PoseVertex, typename Config::Key>(tree);
// attach the relative poses to the edges
PoseEdge edge1, edge2;
@ -167,8 +149,11 @@ boost::shared_ptr<Config> composePoses(const G& graph, const map<string, string>
boost::shared_ptr<Factor> factor = boost::dynamic_pointer_cast<Factor>(nl_factor);
if (!factor) continue;
PoseVertex v_from = key2vertex.find(factor->keys().front())->second;
PoseVertex v_to = key2vertex.find(factor->keys().back())->second;
typename Config::Key key1 = factor->key1();
typename Config::Key key2 = factor->key2();
PoseVertex v_from = key2vertex.find(key1)->second;
PoseVertex v_to = key2vertex.find(key2)->second;
Pose measured = factor->measured();
tie(edge1, found1) = boost::edge(v_from, v_to, g);
@ -183,7 +168,8 @@ boost::shared_ptr<Config> composePoses(const G& graph, const map<string, string>
// compose poses
boost::shared_ptr<Config> config(new Config);
config->insert(boost::get(boost::vertex_name, g, root), rootPose);
typename Config::Key rootKey = boost::get(boost::vertex_name, g, root);
config->insert(rootKey, rootPose);
compose_key_visitor<PoseVertex, Pose, Config> vis(config);
boost::breadth_first_search(g, root, boost::visitor(vis));

59
cpp/graph.h Normal file
View File

@ -0,0 +1,59 @@
/*
* graph.h
* @brief Graph algorithm using boost library
* @author: Kai Ni
* Created on: Jan 11, 2010
*/
#pragma once
#include <map>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/shared_ptr.hpp>
namespace gtsam {
// type definitions :
/**
* SDGraph is undirected graph with variable keys and double edge weights
*/
template<class Key>
class SDGraph: public boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS,
boost::property<boost::vertex_name_t, Key>, boost::property<
boost::edge_weight_t, double> > {
};
/**
* Map from variable key to parent key
*/
template <class Key>
class PredecessorMap : public std::map<Key,Key> {};
/**
* Convert the factor graph to an SDGraph
* G = Graph type
* F = Factor type
* Key = Key type
*/
template<class G, class F, class Key> SDGraph<Key> toBoostGraph(const G& graph);
/**
* Build takes a predecessor map, and builds a directed graph corresponding to the tree.
* G = Graph type
* V = Vertex type
*/
template<class G, class V, class Key>
boost::tuple<G, V, std::map<Key, V> >
predecessorMap2Graph(const PredecessorMap<Key>& p_map);
/**
* Compose the poses by following the chain specified by the spanning tree
*/
template<class G, class Factor, class Pose, class Config>
boost::shared_ptr<Config>
composePoses(const G& graph, const PredecessorMap<typename Config::Key>& tree, const Pose& rootPose);
} // namespace gtsam

4
cpp/simulated2D.cpp
View File

@ -6,7 +6,7 @@
#include "simulated2D.h"
namespace gtsam {
namespace simulated2D {
/** prior on a single pose */
@ -64,4 +64,4 @@ Matrix Dmea2(const Vector& x, const Vector& l) {
}
/* ************************************************************************* */
} // namespace gtsam
} // namespace simulated2D

52
cpp/simulated2D.h
View File

@ -8,30 +8,44 @@
#pragma once
#include "VectorConfig.h"
#include "NonlinearFactor.h"
// \namespace
namespace gtsam {
namespace simulated2D {
/**
* Prior on a single pose
*/
Vector prior (const Vector& x);
Matrix Dprior(const Vector& x);
typedef gtsam::VectorConfig VectorConfig;
/**
* odometry between two poses
*/
Vector odo(const Vector& x1, const Vector& x2);
Matrix Dodo1(const Vector& x1, const Vector& x2);
Matrix Dodo2(const Vector& x1, const Vector& x2);
struct PoseKey: public std::string {
PoseKey(const std::string&s) :
std::string(s) {
}
};
struct PointKey: public std::string {
PointKey(const std::string&s) :
std::string(s) {
}
};
/**
* measurement between landmark and pose
*/
Vector mea(const Vector& x, const Vector& l);
Matrix Dmea1(const Vector& x, const Vector& l);
Matrix Dmea2(const Vector& x, const Vector& l);
/**
* Prior on a single pose
*/
Vector prior(const Vector& x);
Matrix Dprior(const Vector& x);
} // namespace gtsam
/**
* odometry between two poses
*/
Vector odo(const Vector& x1, const Vector& x2);
Matrix Dodo1(const Vector& x1, const Vector& x2);
Matrix Dodo2(const Vector& x1, const Vector& x2);
/**
* measurement between landmark and pose
*/
Vector mea(const Vector& x, const Vector& l);
Matrix Dmea1(const Vector& x, const Vector& l);
Matrix Dmea2(const Vector& x, const Vector& l);
} // namespace simulated2D

48
cpp/smallExample.cpp
View File

@ -8,6 +8,7 @@
#include <iostream>
#include <string>
#include <boost/optional.hpp>
using namespace std;
@ -37,7 +38,7 @@ namespace gtsam {
// prior on x1
double sigma1 = 0.1;
Vector mu = zero(2);
shared f1(new Point2Prior(mu, sigma1, "x1"));
shared f1(new simulated2D::Point2Prior(mu, sigma1, "x1"));
nlfg->push_back(f1);
// odometry between x1 and x2
@ -45,7 +46,7 @@ namespace gtsam {
Vector z2(2);
z2(0) = 1.5;
z2(1) = 0;
shared f2(new Simulated2DOdometry(z2, sigma2, "x1", "x2"));
shared f2(new simulated2D::Simulated2DOdometry(z2, sigma2, "x1", "x2"));
nlfg->push_back(f2);
// measurement between x1 and l1
@ -53,7 +54,7 @@ namespace gtsam {
Vector z3(2);
z3(0) = 0.;
z3(1) = -1.;
shared f3(new Simulated2DMeasurement(z3, sigma3, "x1", "l1"));
shared f3(new simulated2D::Simulated2DMeasurement(z3, sigma3, "x1", "l1"));
nlfg->push_back(f3);
// measurement between x2 and l1
@ -61,7 +62,7 @@ namespace gtsam {
Vector z4(2);
z4(0) = -1.5;
z4(1) = -1.;
shared f4(new Simulated2DMeasurement(z4, sigma4, "x2", "l1"));
shared f4(new simulated2D::Simulated2DMeasurement(z4, sigma4, "x2", "l1"));
nlfg->push_back(f4);
return nlfg;
@ -173,16 +174,35 @@ namespace gtsam {
// Some nonlinear functions to optimize
/* ************************************************************************* */
namespace smallOptimize {
Vector h(const Vector& v) {
double x = v(0);
return Vector_(2, cos(x), sin(x));
}
;
Matrix H(const Vector& v) {
double x = v(0);
return Matrix_(2, 1, -sin(x), cos(x));
}
;
struct UnaryFactor: public gtsam::NonlinearFactor1<VectorConfig,
std::string, Vector> {
Vector z_;
UnaryFactor(const Vector& z, double sigma, const std::string& key) :
gtsam::NonlinearFactor1<VectorConfig, std::string, Vector>(sigma, key),
z_(z) {
}
Vector evaluateError(const Vector& x, boost::optional<Matrix&> A =
boost::none) const {
if (A) *A = H(x);
return h(x) - z_;
}
};
}
/* ************************************************************************* */
@ -191,8 +211,8 @@ namespace gtsam {
new ExampleNonlinearFactorGraph);
Vector z = Vector_(2, 1.0, 0.0);
double sigma = 0.1;
boost::shared_ptr<NonlinearFactor1> factor(new NonlinearFactor1(z, sigma,
&smallOptimize::h, "x", &smallOptimize::H));
boost::shared_ptr<smallOptimize::UnaryFactor> factor(new smallOptimize::UnaryFactor(
z, sigma, "x"));
fg->push_back(factor);
return fg;
}
@ -214,7 +234,7 @@ namespace gtsam {
// prior on x1
Vector x1 = Vector_(2, 1.0, 0.0);
string key1 = symbol('x', 1);
shared prior(new Point2Prior(x1, sigma1, key1));
shared prior(new simulated2D::Point2Prior(x1, sigma1, key1));
nlfg.push_back(prior);
poses.insert(key1, x1);
@ -222,13 +242,13 @@ namespace gtsam {
// odometry between x_t and x_{t-1}
Vector odo = Vector_(2, 1.0, 0.0);
string key = symbol('x', t);
shared odometry(new Simulated2DOdometry(odo, sigma2, symbol('x', t - 1),
shared odometry(new simulated2D::Simulated2DOdometry(odo, sigma2, symbol('x', t - 1),
key));
nlfg.push_back(odometry);
// measurement on x_t is like perfect GPS
Vector xt = Vector_(2, (double) t, 0.0);
shared measurement(new Point2Prior(xt, sigma1, key));
shared measurement(new simulated2D::Point2Prior(xt, sigma1, key));
nlfg.push_back(measurement);
// initial estimate
@ -509,7 +529,7 @@ namespace gtsam {
// Create almost hard constraint on x11, sigma=0 will work for PCG not for normal
double sigma0 = 1e-3;
shared constraint(new Point2Prior(Vector_(2, 1.0, 1.0), sigma0, "x11"));
shared constraint(new simulated2D::Point2Prior(Vector_(2, 1.0, 1.0), sigma0, "x11"));
nlfg.push_back(constraint);
double sigma = 0.01;
@ -518,7 +538,7 @@ namespace gtsam {
Vector z1 = Vector_(2, 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 Simulated2DOdometry(z1, sigma, key(x, y), key(x + 1, y)));
shared f(new simulated2D::Simulated2DOdometry(z1, sigma, key(x, y), key(x + 1, y)));
nlfg.push_back(f);
}
@ -526,7 +546,7 @@ namespace gtsam {
Vector z2 = Vector_(2, 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 Simulated2DOdometry(z2, sigma, key(x, y), key(x, y + 1)));
shared f(new simulated2D::Simulated2DOdometry(z2, sigma, key(x, y), key(x, y + 1)));
nlfg.push_back(f);
}

4
cpp/testGaussianFactorGraph.cpp
View File

@ -730,7 +730,7 @@ TEST( GaussianFactorGraph, constrained_multi2 )
CHECK(assert_equal(expected, actual));
}
/* ************************************************************************* */
/* ************************************************************************* *
TEST( GaussianFactorGraph, findMinimumSpanningTree )
{
GaussianFactorGraph g;
@ -750,7 +750,7 @@ TEST( GaussianFactorGraph, findMinimumSpanningTree )
CHECK(tree["x4"].compare("x1")==0);
}
/* ************************************************************************* */
/* ************************************************************************* *
TEST( GaussianFactorGraph, split )
{
GaussianFactorGraph g;

24
cpp/testGraph.cpp
View File

@ -13,6 +13,7 @@
#include <CppUnitLite/TestHarness.h>
#include "Pose2Graph.h"
#include "LieConfig-inl.h"
#include "graph-inl.h"
using namespace std;
@ -24,22 +25,23 @@ TEST( Graph, composePoses )
{
Pose2Graph graph;
Matrix cov = eye(3);
graph.push_back(boost::shared_ptr<Pose2Factor>(new Pose2Factor("x1", "x2", Pose2(2.0, 0.0, 0.0), cov)));
graph.push_back(boost::shared_ptr<Pose2Factor>(new Pose2Factor("x2", "x3", Pose2(3.0, 0.0, 0.0), cov)));
graph.push_back(boost::shared_ptr<Pose2Factor>(new Pose2Factor(1,2, Pose2(2.0, 0.0, 0.0), cov)));
graph.push_back(boost::shared_ptr<Pose2Factor>(new Pose2Factor(2,3, Pose2(3.0, 0.0, 0.0), cov)));
map<string, string> tree;
tree.insert(make_pair("x1", "x2"));
tree.insert(make_pair("x2", "x2"));
tree.insert(make_pair("x3", "x2"));
PredecessorMap<Pose2Config::Key> tree;
tree.insert(make_pair(1,2));
tree.insert(make_pair(2,2));
tree.insert(make_pair(3,2));
Pose2 rootPose(3.0, 0.0, 0.0);
boost::shared_ptr<Pose2Config> actual = composePoses<Pose2Graph, Pose2Factor, Pose2, Pose2Config>
(graph, tree, rootPose);
boost::shared_ptr<Pose2Config> actual = composePoses<Pose2Graph, Pose2Factor,
Pose2, Pose2Config> (graph, tree, rootPose);
Pose2Config expected;
expected.insert("x1", Pose2(1.0, 0.0, 0.0));
expected.insert("x2", Pose2(3.0, 0.0, 0.0));
expected.insert("x3", Pose2(6.0, 0.0, 0.0));
expected.insert(1, Pose2(1.0, 0.0, 0.0));
expected.insert(2, Pose2(3.0, 0.0, 0.0));
expected.insert(3, Pose2(6.0, 0.0, 0.0));
LONGS_EQUAL(3, actual->size());
CHECK(assert_equal(expected, *actual));

24
cpp/testLieConfig.cpp
View File

@ -18,10 +18,10 @@ using namespace std;
/* ************************************************************************* */
TEST( LieConfig, equals1 )
{
LieConfig<Vector> expected;
LieConfig<string,Vector> expected;
Vector v = Vector_(3, 5.0, 6.0, 7.0);
expected.insert("a",v);
LieConfig<Vector> actual;
LieConfig<string,Vector> actual;
actual.insert("a",v);
CHECK(assert_equal(expected,actual));
}
@ -29,7 +29,7 @@ TEST( LieConfig, equals1 )
/* ************************************************************************* */
TEST( LieConfig, equals2 )
{
LieConfig<Vector> cfg1, cfg2;
LieConfig<string,Vector> cfg1, cfg2;
Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
Vector v2 = Vector_(3, 5.0, 6.0, 8.0);
cfg1.insert("x", v1);
@ -41,7 +41,7 @@ TEST( LieConfig, equals2 )
/* ************************************************************************* */
TEST( LieConfig, equals_nan )
{
LieConfig<Vector> cfg1, cfg2;
LieConfig<string,Vector> cfg1, cfg2;
Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
Vector v2 = Vector_(3, 0.0/0.0, 0.0/0.0, 0.0/0.0);
cfg1.insert("x", v1);
@ -53,7 +53,7 @@ TEST( LieConfig, equals_nan )
/* ************************************************************************* */
TEST(LieConfig, expmap_a)
{
LieConfig<Vector> config0;
LieConfig<string,Vector> config0;
config0.insert("v1", Vector_(3, 1.0, 2.0, 3.0));
config0.insert("v2", Vector_(3, 5.0, 6.0, 7.0));
@ -61,7 +61,7 @@ TEST(LieConfig, expmap_a)
increment.insert("v1", Vector_(3, 1.0, 1.1, 1.2));
increment.insert("v2", Vector_(3, 1.3, 1.4, 1.5));
LieConfig<Vector> expected;
LieConfig<string,Vector> expected;
expected.insert("v1", Vector_(3, 2.0, 3.1, 4.2));
expected.insert("v2", Vector_(3, 6.3, 7.4, 8.5));
@ -71,14 +71,14 @@ TEST(LieConfig, expmap_a)
/* ************************************************************************* */
TEST(LieConfig, expmap_b)
{
LieConfig<Vector> config0;
LieConfig<string,Vector> config0;
config0.insert("v1", Vector_(3, 1.0, 2.0, 3.0));
config0.insert("v2", Vector_(3, 5.0, 6.0, 7.0));
VectorConfig increment;
increment.insert("v2", Vector_(3, 1.3, 1.4, 1.5));
LieConfig<Vector> expected;
LieConfig<string,Vector> expected;
expected.insert("v1", Vector_(3, 1.0, 2.0, 3.0));
expected.insert("v2", Vector_(3, 6.3, 7.4, 8.5));
@ -88,7 +88,7 @@ TEST(LieConfig, expmap_b)
/* ************************************************************************* */
TEST(LieConfig, expmap_c)
{
LieConfig<Vector> config0;
LieConfig<string,Vector> config0;
config0.insert("v1", Vector_(3, 1.0, 2.0, 3.0));
config0.insert("v2", Vector_(3, 5.0, 6.0, 7.0));
@ -96,7 +96,7 @@ TEST(LieConfig, expmap_c)
1.0, 1.1, 1.2,
1.3, 1.4, 1.5);
LieConfig<Vector> expected;
LieConfig<string,Vector> expected;
expected.insert("v1", Vector_(3, 2.0, 3.1, 4.2));
expected.insert("v2", Vector_(3, 6.3, 7.4, 8.5));
@ -106,14 +106,14 @@ TEST(LieConfig, expmap_c)
/* ************************************************************************* */
TEST(LieConfig, expmap_d)
{
LieConfig<Vector> config0;
LieConfig<string,Vector> config0;
config0.insert("v1", Vector_(3, 1.0, 2.0, 3.0));
config0.insert("v2", Vector_(3, 5.0, 6.0, 7.0));
//config0.print("config0");
CHECK(equal(config0, config0));
CHECK(config0.equals(config0));
LieConfig<Pose2> poseconfig;
LieConfig<string,Pose2> poseconfig;
poseconfig.insert("p1", Pose2(1,2,3));
poseconfig.insert("p2", Pose2(0.3, 0.4, 0.5));
//poseconfig.print("poseconfig");

39
cpp/testNonlinearEquality.cpp
View File

@ -10,27 +10,22 @@
using namespace std;
using namespace gtsam;
typedef boost::shared_ptr<NonlinearEquality<VectorConfig> > shared_nle;
typedef NonlinearEquality<VectorConfig,string,Vector> NLE;
typedef boost::shared_ptr<NLE> shared_nle;
bool vector_compare(const std::string& key,
const VectorConfig& feasible,
const VectorConfig& input) {
Vector feas, lin;
feas = feasible[key];
lin = input[key];
return equal_with_abs_tol(lin, feas, 1e-5);
bool vector_compare(const Vector& a, const Vector& b) {
return equal_with_abs_tol(a, b, 1e-5);
}
/* ************************************************************************* */
TEST ( NonlinearEquality, linearization ) {
string key = "x";
Vector value = Vector_(2, 1.0, 2.0);
VectorConfig feasible, linearize;
feasible.insert(key, value);
VectorConfig linearize;
linearize.insert(key, value);
// create a nonlinear equality constraint
shared_nle nle(new NonlinearEquality<VectorConfig>(key, feasible, 2, *vector_compare));
shared_nle nle(new NLE(key, value,vector_compare));
// check linearize
GaussianFactor expLF(key, eye(2), zero(2), 0.0);
@ -38,17 +33,16 @@ TEST ( NonlinearEquality, linearization ) {
CHECK(assert_equal(*actualLF, expLF));
}
/* ************************************************************************* */
/* ********************************************************************** */
TEST ( NonlinearEquality, linearization_fail ) {
string key = "x";
Vector value = Vector_(2, 1.0, 2.0);
Vector wrong = Vector_(2, 3.0, 4.0);
VectorConfig feasible, bad_linearize;
feasible.insert(key, value);
VectorConfig bad_linearize;
bad_linearize.insert(key, wrong);
// create a nonlinear equality constraint
shared_nle nle(new NonlinearEquality<VectorConfig>(key, feasible, 2, *vector_compare));
shared_nle nle(new NLE(key, value,vector_compare));
// check linearize to ensure that it fails for bad linearization points
try {
@ -69,7 +63,7 @@ TEST ( NonlinearEquality, error ) {
bad_linearize.insert(key, wrong);
// create a nonlinear equality constraint
shared_nle nle(new NonlinearEquality<VectorConfig>(key, feasible, 2, *vector_compare));
shared_nle nle(new NLE(key, value,vector_compare));
// check error function outputs
Vector actual = nle->error_vector(feasible);
@ -84,23 +78,16 @@ TEST ( NonlinearEquality, equals ) {
string key1 = "x";
Vector value1 = Vector_(2, 1.0, 2.0);
Vector value2 = Vector_(2, 3.0, 4.0);
VectorConfig feasible1, feasible2, feasible3;
feasible1.insert(key1, value1);
feasible2.insert(key1, value2);
feasible3.insert(key1, value1);
feasible3.insert("y", value2);
// create some constraints to compare
shared_nle nle1(new NonlinearEquality<VectorConfig>(key1, feasible1, 2, *vector_compare));
shared_nle nle2(new NonlinearEquality<VectorConfig>(key1, feasible1, 2, *vector_compare));
shared_nle nle3(new NonlinearEquality<VectorConfig>(key1, feasible2, 2, *vector_compare));
shared_nle nle4(new NonlinearEquality<VectorConfig>(key1, feasible3, 2, *vector_compare));
shared_nle nle1(new NLE(key1, value1,vector_compare));
shared_nle nle2(new NLE(key1, value1,vector_compare));
shared_nle nle3(new NLE(key1, value2,vector_compare));
// verify
CHECK(nle1->equals(*nle2)); // basic equality = true
CHECK(nle2->equals(*nle1)); // test symmetry of equals()
CHECK(!nle1->equals(*nle3)); // test config
CHECK(nle4->equals(*nle1)); // test the full feasible set isn't getting compared
}

18
cpp/testNonlinearFactor.cpp
View File

@ -27,12 +27,12 @@ TEST( NonlinearFactor, equals )
double sigma = 1.0;
// create two nonlinear2 factors
Vector z3(2); z3(0) = 0. ; z3(1) = -1.;
Simulated2DMeasurement f0(z3, sigma, "x1", "l1");
Vector z3 = Vector_(2,0.,-1.);
simulated2D::Simulated2DMeasurement f0(z3, sigma, "x1", "l1");
// measurement between x2 and l1
Vector z4(2); z4(0)= -1.5 ; z4(1) = -1.;
Simulated2DMeasurement f1(z4, sigma, "x2", "l1");
Vector z4 = Vector_(2,-1.5, -1.);
simulated2D::Simulated2DMeasurement f1(z4, sigma, "x2", "l1");
CHECK(assert_equal(f0,f0));
CHECK(f0.equals(f0));
@ -68,7 +68,7 @@ TEST( NonlinearFactor, NonlinearFactor )
// calculate the error_vector from the factor "f1"
Vector actual_e = factor->error_vector(cfg);
Vector e(2); e(0) = -0.1; e(1) = -0.1;
Vector e(2); e(0) = 0.1; e(1) = 0.1;
CHECK(assert_equal(e,actual_e));
// the expected value for the error from the factor
@ -81,7 +81,7 @@ TEST( NonlinearFactor, NonlinearFactor )
DOUBLES_EQUAL(expected,actual,0.00000001);
}
/* ************************************************************************* */
/* ************************************************************************* *
TEST( NonlinearFactor, linearize_f1 )
{
// Grab a non-linear factor
@ -103,7 +103,7 @@ TEST( NonlinearFactor, linearize_f1 )
CHECK(assert_equal(nlf->error_vector(c),actual->get_b()));
}
/* ************************************************************************* */
/* ************************************************************************* *
TEST( NonlinearFactor, linearize_f2 )
{
// Grab a non-linear factor
@ -121,7 +121,7 @@ TEST( NonlinearFactor, linearize_f2 )
CHECK(expected->equals(*actual));
}
/* ************************************************************************* */
/* ************************************************************************* *
TEST( NonlinearFactor, linearize_f3 )
{
// Grab a non-linear factor
@ -139,7 +139,7 @@ TEST( NonlinearFactor, linearize_f3 )
CHECK(expected->equals(*actual));
}
/* ************************************************************************* */
/* ************************************************************************* *
TEST( NonlinearFactor, linearize_f4 )
{
// Grab a non-linear factor

4
cpp/testOrdering.cpp
View File

@ -11,11 +11,11 @@ using namespace std;
using namespace gtsam;
using namespace boost::assign;
/* ************************************************************************* */
/* ************************************************************************* *
// x1 -> x2
// -> x3 -> x4
// -> x5
TEST ( Ordering, constructor ) {
TEST ( Ordering, constructorFromTree ) {
map<string, string> p_map;
p_map.insert(make_pair("x1", "x1"));
p_map.insert(make_pair("x2", "x1"));

20
cpp/testPose2Config.cpp
View File

@ -16,12 +16,12 @@ TEST( Pose2Config, pose2Circle )
{
// expected is 4 poses tangent to circle with radius 1m
Pose2Config expected;
expected.insert("p0", Pose2( 1, 0, M_PI_2));
expected.insert("p1", Pose2( 0, 1, - M_PI ));
expected.insert("p2", Pose2(-1, 0, - M_PI_2));
expected.insert("p3", Pose2( 0, -1, 0 ));
expected.insert(0, Pose2( 1, 0, M_PI_2));
expected.insert(1, Pose2( 0, 1, - M_PI ));
expected.insert(2, Pose2(-1, 0, - M_PI_2));
expected.insert(3, Pose2( 0, -1, 0 ));
Pose2Config actual = pose2Circle(4,1.0,'p');
Pose2Config actual = pose2Circle(4,1.0);
CHECK(assert_equal(expected,actual));
}
@ -30,14 +30,14 @@ TEST( Pose2Config, expmap )
{
// expected is circle shifted to right
Pose2Config expected;
expected.insert("p0", Pose2( 1.1, 0, M_PI_2));
expected.insert("p1", Pose2( 0.1, 1, - M_PI ));
expected.insert("p2", Pose2(-0.9, 0, - M_PI_2));
expected.insert("p3", Pose2( 0.1, -1, 0 ));
expected.insert(0, Pose2( 1.1, 0, M_PI_2));
expected.insert(1, Pose2( 0.1, 1, - M_PI ));
expected.insert(2, Pose2(-0.9, 0, - M_PI_2));
expected.insert(3, Pose2( 0.1, -1, 0 ));
// Note expmap coordinates are in local coordinates, so shifting to right requires thought !!!
Vector delta = Vector_(12, 0.0,-0.1,0.0, -0.1,0.0,0.0, 0.0,0.1,0.0, 0.1,0.0,0.0);
Pose2Config actual = expmap(pose2Circle(4,1.0,'p'),delta);
Pose2Config actual = expmap(pose2Circle(4,1.0),delta);
CHECK(assert_equal(expected,actual));
}

39
cpp/testPose2Factor.cpp
View File

@ -20,43 +20,43 @@ static Matrix covariance = Matrix_(3,3,
);
/* ************************************************************************* */
// Very simple test establishing Ax-b \approx h(x)-z
// Very simple test establishing Ax-b \approx z-h(x)
TEST( Pose2Factor, error )
{
// Choose a linearization point
Pose2 p1; // robot at origin
Pose2 p2(1, 0, 0); // robot at (1,0)
Pose2Config x0;
x0.insert("p1", p1);
x0.insert("p2", p2);
x0.insert(1, p1);
x0.insert(2, p2);
// Create factor
Pose2 z = between(p1,p2);
Pose2Factor factor("p1", "p2", z, covariance);
Pose2Factor factor(1, 2, z, covariance);
// Actual linearization
boost::shared_ptr<GaussianFactor> linear = factor.linearize(x0);
// Check error at x0 = zero !
// Check error at x0, i.e. delta = zero !
VectorConfig delta;
delta.insert("p1", zero(3));
delta.insert("p2", zero(3));
delta.insert("x1", zero(3));
delta.insert("x2", zero(3));
Vector error_at_zero = Vector_(3,0.0,0.0,0.0);
CHECK(assert_equal(error_at_zero,factor.error_vector(x0)));
CHECK(assert_equal(-error_at_zero,linear->error_vector(delta)));
// Check error after increasing p2
VectorConfig plus = delta + VectorConfig("p2", Vector_(3, 0.1, 0.0, 0.0));
VectorConfig plus = delta + VectorConfig("x2", Vector_(3, 0.1, 0.0, 0.0));
Pose2Config x1 = expmap(x0, plus);
Vector error_at_plus = Vector_(3,-0.1/sx,0.0,0.0);
Vector error_at_plus = Vector_(3,0.1/sx,0.0,0.0); // h(x)-z = 0.1 !
CHECK(assert_equal(error_at_plus,factor.error_vector(x1)));
CHECK(assert_equal(-error_at_plus,linear->error_vector(plus)));
CHECK(assert_equal(error_at_plus,linear->error_vector(plus)));
}
/* ************************************************************************* */
// common Pose2Factor for tests below
static Pose2 measured(2,2,M_PI_2);
static Pose2Factor factor("p1","p2",measured, covariance);
static Pose2Factor factor(1,2,measured, covariance);
/* ************************************************************************* */
TEST( Pose2Factor, rhs )
@ -65,8 +65,8 @@ TEST( Pose2Factor, rhs )
Pose2 p1(1.1,2,M_PI_2); // robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2)
Pose2 p2(-1,4.1,M_PI); // robot at (-1,4.1) looking at negative (ground truth is at -1,4)
Pose2Config x0;
x0.insert("p1",p1);
x0.insert("p2",p2);
x0.insert(1,p1);
x0.insert(2,p2);
// Actual linearization
boost::shared_ptr<GaussianFactor> linear = factor.linearize(x0);
@ -75,7 +75,7 @@ TEST( Pose2Factor, rhs )
Pose2 hx0 = between(p1,p2);
CHECK(assert_equal(Pose2(2.1, 2.1, M_PI_2),hx0));
Vector expected_b = Vector_(3, -0.1/sx, 0.1/sy, 0.0);
CHECK(assert_equal(expected_b,factor.error_vector(x0)));
CHECK(assert_equal(expected_b,-factor.error_vector(x0)));
CHECK(assert_equal(expected_b,linear->get_b()));
}
@ -83,10 +83,7 @@ TEST( Pose2Factor, rhs )
// The error |A*dx-b| approximates (h(x0+dx)-z) = -error_vector
// Hence i.e., b = approximates z-h(x0) = error_vector(x0)
Vector h(const Pose2& p1,const Pose2& p2) {
Pose2Config x;
x.insert("p1",p1);
x.insert("p2",p2);
return -factor.error_vector(x);
return factor.evaluateError(p1,p2);
}
/* ************************************************************************* */
@ -96,8 +93,8 @@ TEST( Pose2Factor, linearize )
Pose2 p1(1,2,M_PI_2);
Pose2 p2(-1,4,M_PI);
Pose2Config x0;
x0.insert("p1",p1);
x0.insert("p2",p2);
x0.insert(1,p1);
x0.insert(2,p2);
// expected linearization
Matrix square_root_inverse_covariance = Matrix_(3,3,
@ -118,7 +115,7 @@ TEST( Pose2Factor, linearize )
Vector expected_b = Vector_(3, 0.0, 0.0, 0.0);
// expected linear factor
GaussianFactor expected("p1", expectedH1, "p2", expectedH2, expected_b, 1.0);
GaussianFactor expected("x1", expectedH1, "x2", expectedH2, expected_b, 1.0);
// Actual linearization
boost::shared_ptr<GaussianFactor> actual = factor.linearize(x0);

60
cpp/testPose2Graph.cpp
View File

@ -32,9 +32,9 @@ TEST( Pose2Graph, constructor )
{
// create a factor between unknown poses p1 and p2
Pose2 measured(2,2,M_PI_2);
Pose2Factor constraint("x1","x2",measured, covariance);
Pose2Factor constraint(1,2,measured, covariance);
Pose2Graph graph;
graph.add("x1","x2",measured, covariance);
graph.add(1,2,measured, covariance);
// get the size of the graph
size_t actual = graph.size();
// verify
@ -48,16 +48,16 @@ TEST( Pose2Graph, linerization )
{
// create a factor between unknown poses p1 and p2
Pose2 measured(2,2,M_PI_2);
Pose2Factor constraint("x1","x2",measured, covariance);
Pose2Factor constraint(1,2,measured, covariance);
Pose2Graph graph;
graph.add("x1","x2",measured, covariance);
graph.add(1,2,measured, covariance);
// Choose a linearization point
Pose2 p1(1.1,2,M_PI_2); // robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2)
Pose2 p2(-1,4.1,M_PI); // robot at (-1,4) looking at negative (ground truth is at 4.1,2)
Pose2Config config;
config.insert("x1",p1);
config.insert("x2",p2);
config.insert(1,p1);
config.insert(2,p2);
// Linearize
GaussianFactorGraph lfg_linearized = graph.linearize(config);
//lfg_linearized.print("lfg_actual");
@ -86,17 +86,17 @@ TEST(Pose2Graph, optimize) {
// create a Pose graph with one equality constraint and one measurement
shared_ptr<Pose2Graph> fg(new Pose2Graph);
fg->addConstraint("p0", Pose2(0,0,0));
fg->add("p0", "p1", Pose2(1,2,M_PI_2), covariance);
fg->addConstraint(0, Pose2(0,0,0));
fg->add(0, 1, Pose2(1,2,M_PI_2), covariance);
// Create initial config
boost::shared_ptr<Pose2Config> initial(new Pose2Config());
initial->insert("p0", Pose2(0,0,0));
initial->insert("p1", Pose2(0,0,0));
initial->insert(0, Pose2(0,0,0));
initial->insert(1, Pose2(0,0,0));
// Choose an ordering and optimize
shared_ptr<Ordering> ordering(new Ordering);
*ordering += "p0","p1";
*ordering += "x0","x1";
typedef NonlinearOptimizer<Pose2Graph, Pose2Config> Optimizer;
Optimizer optimizer0(fg, ordering, initial);
Optimizer::verbosityLevel verbosity = Optimizer::SILENT;
@ -105,8 +105,8 @@ TEST(Pose2Graph, optimize) {
// Check with expected config
Pose2Config expected;
expected.insert("p0", Pose2(0,0,0));
expected.insert("p1", Pose2(1,2,M_PI_2));
expected.insert(0, Pose2(0,0,0));
expected.insert(1, Pose2(1,2,M_PI_2));
CHECK(assert_equal(expected, *optimizer.config()));
}
@ -115,32 +115,32 @@ TEST(Pose2Graph, optimize) {
TEST(Pose2Graph, optimizeCircle) {
// Create a hexagon of poses
Pose2Config hexagon = pose2Circle(6,1.0,'p');
Pose2 p0 = hexagon["p0"], p1 = hexagon["p1"];
Pose2Config hexagon = pose2Circle(6,1.0);
Pose2 p0 = hexagon[0], p1 = hexagon[1];
// create a Pose graph with one equality constraint and one measurement
shared_ptr<Pose2Graph> fg(new Pose2Graph);
fg->addConstraint("p0", p0);
fg->addConstraint(0, p0);
Pose2 delta = between(p0,p1);
fg->add("p0", "p1", delta, covariance);
fg->add("p1", "p2", delta, covariance);
fg->add("p2", "p3", delta, covariance);
fg->add("p3", "p4", delta, covariance);
fg->add("p4", "p5", delta, covariance);
fg->add("p5", "p0", delta, covariance);
fg->add(0, 1, delta, covariance);
fg->add(1,2, delta, covariance);
fg->add(2,3, delta, covariance);
fg->add(3,4, delta, covariance);
fg->add(4,5, delta, covariance);
fg->add(5, 0, delta, covariance);
// Create initial config
boost::shared_ptr<Pose2Config> initial(new Pose2Config());
initial->insert("p0", p0);
initial->insert("p1", expmap(hexagon["p1"],Vector_(3,-0.1, 0.1,-0.1)));
initial->insert("p2", expmap(hexagon["p2"],Vector_(3, 0.1,-0.1, 0.1)));
initial->insert("p3", expmap(hexagon["p3"],Vector_(3,-0.1, 0.1,-0.1)));
initial->insert("p4", expmap(hexagon["p4"],Vector_(3, 0.1,-0.1, 0.1)));
initial->insert("p5", expmap(hexagon["p5"],Vector_(3,-0.1, 0.1,-0.1)));
initial->insert(0, p0);
initial->insert(1, expmap(hexagon[1],Vector_(3,-0.1, 0.1,-0.1)));
initial->insert(2, expmap(hexagon[2],Vector_(3, 0.1,-0.1, 0.1)));
initial->insert(3, expmap(hexagon[3],Vector_(3,-0.1, 0.1,-0.1)));
initial->insert(4, expmap(hexagon[4],Vector_(3, 0.1,-0.1, 0.1)));
initial->insert(5, expmap(hexagon[5],Vector_(3,-0.1, 0.1,-0.1)));
// Choose an ordering and optimize
shared_ptr<Ordering> ordering(new Ordering);
*ordering += "p0","p1","p2","p3","p4","p5";
*ordering += "x0","x1","x2","x3","x4","x5";
typedef NonlinearOptimizer<Pose2Graph, Pose2Config> Optimizer;
Optimizer optimizer0(fg, ordering, initial);
Optimizer::verbosityLevel verbosity = Optimizer::SILENT;
@ -153,7 +153,7 @@ TEST(Pose2Graph, optimizeCircle) {
CHECK(assert_equal(hexagon, actual));
// Check loop closure
CHECK(assert_equal(delta,between(actual["p5"],actual["p0"])));
CHECK(assert_equal(delta,between(actual[5],actual[0])));
}
/* ************************************************************************* */

20
cpp/testPose3Config.cpp
View File

@ -23,12 +23,12 @@ TEST( Pose3Config, pose3Circle )
{
// expected is 4 poses tangent to circle with radius 1m
Pose3Config expected;
expected.insert("p0", Pose3(R1, Point3( 1, 0, 0)));
expected.insert("p1", Pose3(R2, Point3( 0, 1, 0)));
expected.insert("p2", Pose3(R3, Point3(-1, 0, 0)));
expected.insert("p3", Pose3(R4, Point3( 0,-1, 0)));
expected.insert(0, Pose3(R1, Point3( 1, 0, 0)));
expected.insert(1, Pose3(R2, Point3( 0, 1, 0)));
expected.insert(2, Pose3(R3, Point3(-1, 0, 0)));
expected.insert(3, Pose3(R4, Point3( 0,-1, 0)));
Pose3Config actual = pose3Circle(4,1.0,'p');
Pose3Config actual = pose3Circle(4,1.0);
CHECK(assert_equal(expected,actual));
}
@ -37,10 +37,10 @@ TEST( Pose3Config, expmap )
{
// expected is circle shifted to East
Pose3Config expected;
expected.insert("p0", Pose3(R1, Point3( 1.1, 0, 0)));
expected.insert("p1", Pose3(R2, Point3( 0.1, 1, 0)));
expected.insert("p2", Pose3(R3, Point3(-0.9, 0, 0)));
expected.insert("p3", Pose3(R4, Point3( 0.1,-1, 0)));
expected.insert(0, Pose3(R1, Point3( 1.1, 0, 0)));
expected.insert(1, Pose3(R2, Point3( 0.1, 1, 0)));
expected.insert(2, Pose3(R3, Point3(-0.9, 0, 0)));
expected.insert(3, Pose3(R4, Point3( 0.1,-1, 0)));
// Note expmap coordinates are in global coordinates with non-compose expmap
// so shifting to East requires little thought, different from with Pose2 !!!
@ -49,7 +49,7 @@ TEST( Pose3Config, expmap )
0.0,0.0,0.0, 0.1, 0.0, 0.0,
0.0,0.0,0.0, 0.1, 0.0, 0.0,
0.0,0.0,0.0, 0.1, 0.0, 0.0);
Pose3Config actual = expmap(pose3Circle(4,1.0,'p'),delta);
Pose3Config actual = expmap(pose3Circle(4,1.0),delta);
CHECK(assert_equal(expected,actual));
}

10
cpp/testPose3Factor.cpp
View File

@ -25,16 +25,16 @@ TEST( Pose3Factor, error )
// Create factor
Matrix measurement_covariance = eye(6);
Pose3Factor factor("t1", "t2", z, measurement_covariance);
Pose3Factor factor(1,2, z, measurement_covariance);
// Create config
Pose3Config x;
x.insert("t1",t1);
x.insert("t2",t2);
x.insert(1,t1);
x.insert(2,t2);
// Get error z-h(x) -> logmap(h(x),z) = logmap(between(t1,t2),z)
// Get error h(x)-z -> logmap(z,h(x)) = logmap(z,between(t1,t2))
Vector actual = factor.error_vector(x);
Vector expected = logmap(between(t1,t2),z);
Vector expected = logmap(z,between(t1,t2));
CHECK(assert_equal(expected,actual));
}

36
cpp/testPose3Graph.cpp
View File

@ -13,10 +13,10 @@ using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#include "Pose3Graph.h"
#include "NonlinearOptimizer-inl.h"
#include "NonlinearEquality.h"
#include "Ordering.h"
#include "Pose3Graph.h"
using namespace std;
using namespace gtsam;
@ -29,32 +29,32 @@ static Matrix covariance = eye(6);
TEST(Pose3Graph, optimizeCircle) {
// Create a hexagon of poses
Pose3Config hexagon = pose3Circle(6,1.0,'p');
Pose3 p0 = hexagon["p0"], p1 = hexagon["p1"];
Pose3Config hexagon = pose3Circle(6,1.0);
Pose3 p0 = hexagon[0], p1 = hexagon[1];
// create a Pose graph with one equality constraint and one measurement
shared_ptr<Pose3Graph> fg(new Pose3Graph);
fg->addConstraint("p0", p0);
fg->addConstraint(0, p0);
Pose3 delta = between(p0,p1);
fg->add("p0", "p1", delta, covariance);
fg->add("p1", "p2", delta, covariance);
fg->add("p2", "p3", delta, covariance);
fg->add("p3", "p4", delta, covariance);
fg->add("p4", "p5", delta, covariance);
fg->add("p5", "p0", delta, covariance);
fg->add(0,1, delta, covariance);
fg->add(1,2, delta, covariance);
fg->add(2,3, delta, covariance);
fg->add(3,4, delta, covariance);
fg->add(4,5, delta, covariance);
fg->add(5,0, delta, covariance);
// Create initial config
boost::shared_ptr<Pose3Config> initial(new Pose3Config());
initial->insert("p0", p0);
initial->insert("p1", expmap(hexagon["p1"],Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
initial->insert("p2", expmap(hexagon["p2"],Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
initial->insert("p3", expmap(hexagon["p3"],Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
initial->insert("p4", expmap(hexagon["p4"],Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
initial->insert("p5", expmap(hexagon["p5"],Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
initial->insert(0, p0);
initial->insert(1, expmap(hexagon[1],Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
initial->insert(2, expmap(hexagon[2],Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
initial->insert(3, expmap(hexagon[3],Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
initial->insert(4, expmap(hexagon[4],Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
initial->insert(5, expmap(hexagon[5],Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
// Choose an ordering and optimize
shared_ptr<Ordering> ordering(new Ordering);
*ordering += "p0","p1","p2","p3","p4","p5";
*ordering += "x0","x1","x2","x3","x4","x5";
typedef NonlinearOptimizer<Pose3Graph, Pose3Config> Optimizer;
Optimizer optimizer0(fg, ordering, initial);
Optimizer::verbosityLevel verbosity = Optimizer::SILENT;
@ -67,7 +67,7 @@ TEST(Pose3Graph, optimizeCircle) {
CHECK(assert_equal(hexagon, actual,1e-5));
// Check loop closure
CHECK(assert_equal(delta,between(actual["p5"],actual["p0"]),1e-5));
CHECK(assert_equal(delta,between(actual[5],actual[0]),1e-5));
}
/* ************************************************************************* */

31
cpp/testSQP.cpp
View File

@ -242,19 +242,16 @@ TEST (SQP, problem1_sqp ) {
/* ********************************************************************* */
// components for nonlinear factor graphs
bool vector_compare(const std::string& key,
const VectorConfig& feasible,
const VectorConfig& input) {
Vector feas, lin;
feas = feasible[key];
lin = input[key];
return equal_with_abs_tol(lin, feas, 1e-5);
bool vector_compare(const Vector& a, const Vector& b) {
return equal_with_abs_tol(a, b, 1e-5);
}
typedef NonlinearFactorGraph<VectorConfig> NLGraph;
typedef boost::shared_ptr<NonlinearFactor<VectorConfig> > shared;
typedef boost::shared_ptr<NonlinearConstraint<VectorConfig> > shared_c;
typedef boost::shared_ptr<NonlinearEquality<VectorConfig> > shared_eq;
typedef NonlinearEquality<VectorConfig,string,Vector> NLE;
typedef boost::shared_ptr<NLE> shared_eq;
typedef boost::shared_ptr<VectorConfig> shared_cfg;
typedef NonlinearOptimizer<NLGraph,VectorConfig> Optimizer;
@ -268,24 +265,26 @@ TEST (SQP, two_pose_truth ) {
// position (1, 1) constraint for x1
// position (5, 6) constraint for x2
VectorConfig feas;
feas.insert("x1", Vector_(2, 1.0, 1.0));
feas.insert("x2", Vector_(2, 5.0, 6.0));
Vector feas1 = Vector_(2, 1.0, 1.0);
Vector feas2 = Vector_(2, 5.0, 6.0);
feas.insert("x1", feas1);
feas.insert("x2", feas2);
// constant constraint on x1
shared_eq ef1(new NonlinearEquality<VectorConfig>("x1", feas, 2, *vector_compare));
shared_eq ef1(new NLE("x1", feas1, vector_compare));
// constant constraint on x2
shared_eq ef2(new NonlinearEquality<VectorConfig>("x2", feas, 2, *vector_compare));
shared_eq ef2(new NLE("x2", feas2, vector_compare));
// measurement from x1 to l1
Vector z1 = Vector_(2, 0.0, 5.0);
double sigma1 = 0.1;
shared f1(new Simulated2DMeasurement(z1, sigma1, "x1", "l1"));
shared f1(new simulated2D::Simulated2DMeasurement(z1, sigma1, "x1", "l1"));
// measurement from x2 to l1
Vector z2 = Vector_(2, -4.0, 0.0);
double sigma2 = 0.1;
shared f2(new Simulated2DMeasurement(z2, sigma2, "x2", "l1"));
shared f2(new simulated2D::Simulated2DMeasurement(z2, sigma2, "x2", "l1"));
// construct the graph
shared_ptr<NLGraph> graph(new NLGraph());
@ -377,12 +376,12 @@ TEST (SQP, two_pose ) {
// measurement from x1 to l1
Vector z1 = Vector_(2, 0.0, 5.0);
double sigma1 = 0.1;
shared f1(new Simulated2DMeasurement(z1, sigma1, "x1", "l1"));
shared f1(new simulated2D::Simulated2DMeasurement(z1, sigma1, "x1", "l1"));
// measurement from x2 to l2
Vector z2 = Vector_(2, -4.0, 0.0);
double sigma2 = 0.1;
shared f2(new Simulated2DMeasurement(z2, sigma2, "x2", "l2"));
shared f2(new simulated2D::Simulated2DMeasurement(z2, sigma2, "x2", "l2"));
// equality constraint between l1 and l2
list<string> keys2; keys2 += "l1", "l2";

51
cpp/testSQPOptimizer.cpp
View File

@ -102,12 +102,12 @@ NLGraph linearMapWarpGraph() {
// measurement from x1 to l1
Vector z1 = Vector_(2, 0.0, 5.0);
double sigma1 = 0.1;
shared f1(new Simulated2DMeasurement(z1, sigma1, "x1", "l1"));
shared f1(new simulated2D::Simulated2DMeasurement(z1, sigma1, "x1", "l1"));
// measurement from x2 to l2
Vector z2 = Vector_(2, -4.0, 0.0);
double sigma2 = 0.1;
shared f2(new Simulated2DMeasurement(z2, sigma2, "x2", "l2"));
shared f2(new simulated2D::Simulated2DMeasurement(z2, sigma2, "x2", "l2"));
// equality constraint between l1 and l2
list<string> keys; keys += "l1", "l2";
@ -208,20 +208,15 @@ TEST ( SQPOptimizer, map_warp ) {
// states, which enforces a minimum distance.
/* ********************************************************************* */
bool vector_compare(const std::string& key,
const VectorConfig& feasible,
const VectorConfig& input) {
Vector feas, lin;
feas = feasible[key];
lin = input[key];
return equal_with_abs_tol(lin, feas, 1e-5);
bool vector_compare(const Vector& a, const Vector& b) {
return equal_with_abs_tol(a, b, 1e-5);
}
typedef NonlinearConstraint1<VectorConfig> NLC1;
typedef boost::shared_ptr<NLC1> shared_NLC1;
typedef NonlinearConstraint2<VectorConfig> NLC2;
typedef boost::shared_ptr<NLC2> shared_NLC2;
typedef NonlinearEquality<VectorConfig> NLE;
typedef NonlinearEquality<VectorConfig,string,Vector> NLE;
typedef boost::shared_ptr<NLE> shared_NLE;
namespace sqp_avoid1 {
@ -255,22 +250,24 @@ Matrix jac_g2(const VectorConfig& config, const list<string>& keys) {
pair<NLGraph, VectorConfig> obstacleAvoidGraph() {
// fix start, end, obstacle positions
VectorConfig feasible;
feasible.insert("x1", Vector_(2, 0.0, 0.0));
feasible.insert("x3", Vector_(2, 10.0, 0.0));
feasible.insert("obs", Vector_(2, 5.0, -0.5));
shared_NLE e1(new NLE("x1", feasible, 2, *vector_compare));
shared_NLE e2(new NLE("x3", feasible, 2, *vector_compare));
shared_NLE e3(new NLE("obs", feasible, 2, *vector_compare));
Vector feas1 = Vector_(2, 0.0, 0.0), feas2 = Vector_(2, 10.0, 0.0), feas3 =
Vector_(2, 5.0, -0.5);
feasible.insert("x1", feas1);
feasible.insert("x3", feas2);
feasible.insert("obs", feas3);
shared_NLE e1(new NLE("x1", feas1, vector_compare));
shared_NLE e2(new NLE("x3", feas2, vector_compare));
shared_NLE e3(new NLE("obs", feas3, vector_compare));
// measurement from x1 to x2
Vector x1x2 = Vector_(2, 5.0, 0.0);
double sigma1 = 0.1;
shared f1(new Simulated2DOdometry(x1x2, sigma1, "x1", "x2"));
shared f1(new simulated2D::Simulated2DOdometry(x1x2, sigma1, "x1", "x2"));
// measurement from x2 to x3
Vector x2x3 = Vector_(2, 5.0, 0.0);
double sigma2 = 0.1;
shared f2(new Simulated2DOdometry(x2x3, sigma2, "x2", "x3"));
shared f2(new simulated2D::Simulated2DOdometry(x2x3, sigma2, "x2", "x3"));
// create a binary inequality constraint that forces the middle point away from
// the obstacle
@ -385,22 +382,24 @@ Matrix jac_g2(const VectorConfig& config, const list<string>& keys) {
pair<NLGraph, VectorConfig> obstacleAvoidGraphGeneral() {
// fix start, end, obstacle positions
VectorConfig feasible;
feasible.insert("x1", Vector_(2, 0.0, 0.0));
feasible.insert("x3", Vector_(2, 10.0, 0.0));
feasible.insert("obs", Vector_(2, 5.0, -0.5));
shared_NLE e1(new NLE("x1", feasible, 2, *vector_compare));
shared_NLE e2(new NLE("x3", feasible, 2, *vector_compare));
shared_NLE e3(new NLE("obs", feasible, 2, *vector_compare));
Vector feas1 = Vector_(2, 0.0, 0.0), feas2 = Vector_(2, 10.0, 0.0), feas3 =
Vector_(2, 5.0, -0.5);
feasible.insert("x1", feas1);
feasible.insert("x3", feas2);
feasible.insert("obs", feas3);
shared_NLE e1(new NLE("x1", feas1,vector_compare));
shared_NLE e2(new NLE("x3", feas2, vector_compare));
shared_NLE e3(new NLE("obs", feas3, vector_compare));
// measurement from x1 to x2
Vector x1x2 = Vector_(2, 5.0, 0.0);
double sigma1 = 0.1;
shared f1(new Simulated2DOdometry(x1x2, sigma1, "x1", "x2"));
shared f1(new simulated2D::Simulated2DOdometry(x1x2, sigma1, "x1", "x2"));
// measurement from x2 to x3
Vector x2x3 = Vector_(2, 5.0, 0.0);
double sigma2 = 0.1;
shared f2(new Simulated2DOdometry(x2x3, sigma2, "x2", "x3"));
shared f2(new simulated2D::Simulated2DOdometry(x2x3, sigma2, "x2", "x3"));
double radius = 1.0;

1
cpp/testSimulated2D.cpp
View File

@ -13,6 +13,7 @@
using namespace gtsam;
using namespace std;
using namespace simulated2D;
/* ************************************************************************* */
TEST( simulated2D, Dprior )

15
cpp/testSimulated3D.cpp
View File

@ -12,14 +12,15 @@
#include "Simulated3D.h"
using namespace gtsam;
using namespace simulated3D;
/* ************************************************************************* */
TEST( simulated3D, Dprior_3D )
TEST( simulated3D, Dprior )
{
Pose3 x1(rodriguez(0, 0, 1.57), Point3(1, 5, 0));
Vector v = logmap(x1);
Matrix numerical = numericalDerivative11(prior_3D,v);
Matrix computed = Dprior_3D(v);
Matrix numerical = numericalDerivative11(prior,v);
Matrix computed = Dprior(v);
CHECK(assert_equal(numerical,computed,1e-9));
}
@ -30,8 +31,8 @@ TEST( simulated3D, DOdo1 )
Vector v1 = logmap(x1);
Pose3 x2(rodriguez(0, 0, 0), Point3(2, 3, 0));
Vector v2 = logmap(x2);
Matrix numerical = numericalDerivative21(odo_3D,v1,v2);
Matrix computed = Dodo1_3D(v1,v2);
Matrix numerical = numericalDerivative21(odo,v1,v2);
Matrix computed = Dodo1(v1,v2);
CHECK(assert_equal(numerical,computed,1e-9));
}
@ -42,8 +43,8 @@ TEST( simulated3D, DOdo2 )
Vector v1 = logmap(x1);
Pose3 x2(rodriguez(0, 0, 0), Point3(2, 3, 0));
Vector v2 = logmap(x2);
Matrix numerical = numericalDerivative22(odo_3D,v1,v2);
Matrix computed = Dodo2_3D(v1,v2);
Matrix numerical = numericalDerivative22(odo,v1,v2);
Matrix computed = Dodo2(v1,v2);
CHECK(assert_equal(numerical,computed,1e-9));
}

17
cpp/testVSLAMConfig.cpp
View File

@ -5,7 +5,8 @@
*/
#include <CppUnitLite/TestHarness.h>
#include <VSLAMConfig.h>
#include "VectorConfig.h"
#include "VSLAMConfig.h"
using namespace std;
using namespace gtsam;
@ -18,17 +19,17 @@ TEST( VSLAMConfig, update_with_large_delta) {
init.addCameraPose(1, Pose3());
init.addLandmarkPoint(1, Point3(1.0, 2.0, 3.0));
VectorConfig delta;
delta.insert("x1", Vector_(6, 0.0, 0.0, 0.0, 0.1, 0.1, 0.1));
delta.insert("l1", Vector_(3, 0.1, 0.1, 0.1));
delta.insert("x2", Vector_(6, 0.0, 0.0, 0.0, 100.1, 4.1, 9.1));
VSLAMConfig actual = expmap(init, delta);
VSLAMConfig expected;
expected.addCameraPose(1, Pose3(Rot3(), Point3(0.1, 0.1, 0.1)));
expected.addLandmarkPoint(1, Point3(1.1, 2.1, 3.1));
CHECK(assert_equal(actual, expected));
VectorConfig delta;
delta.insert("x1", Vector_(6, 0.0, 0.0, 0.0, 0.1, 0.1, 0.1));
delta.insert("l1", Vector_(3, 0.1, 0.1, 0.1));
delta.insert("x2", Vector_(6, 0.0, 0.0, 0.0, 100.1, 4.1, 9.1));
VSLAMConfig actual = expmap(init, delta);
CHECK(assert_equal(expected,actual));
}
/* ************************************************************************* */

2
cpp/testVSLAMFactor.cpp
View File

@ -38,7 +38,7 @@ TEST( VSLAMFactor, error )
VSLAMConfig config;
Rot3 R;Point3 t1(0,0,-6); Pose3 x1(R,t1); config.addCameraPose(1, x1);
Point3 l1; config.addLandmarkPoint(1, l1);
CHECK(assert_equal(Vector_(2,320.,240.),factor->predict(config)));
CHECK(assert_equal(Point2(320.,240.),factor->predict(x1,l1)));
// Which yields an error of 3^2/2 = 4.5
DOUBLES_EQUAL(4.5,factor->error(config),1e-9);