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

svn restored from 1733. 

this commit updates gtsam to version 1774, which now appears as 1734.
release/4.3a0
Chris Beall 2010-01-16 01:16:59 +00:00
parent 5c0cd093fd
commit a956c1a8be
52 changed files with 1042 additions and 513 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
cpp/BetweenFactor.h
View File

@ -65,8 +65,11 @@ namespace gtsam {
// h - z // h - z
T hx = between(p1, p2); T hx = between(p1, p2);
// TODO should be done by noise model // TODO should be done by noise model
if (H1) *H1 = square_root_inverse_covariance_ * Dbetween1(p1, p2); if (H1 || H2) {
if (H2) *H2 = square_root_inverse_covariance_ * Dbetween2(p1, p2); between(p1,p2,H1,H2);
if (H1) *H1 = square_root_inverse_covariance_ * *H1;
if (H2) *H2 = square_root_inverse_covariance_ * *H2;
}
// manifold equivalent of h(x)-z -> log(z,h(x)) // manifold equivalent of h(x)-z -> log(z,h(x))
// TODO use noise model, error vector is not whitened yet // TODO use noise model, error vector is not whitened yet
return square_root_inverse_covariance_ * logmap(measured_, hx); return square_root_inverse_covariance_ * logmap(measured_, hx);

2
cpp/Factor.h
View File

@ -36,7 +36,7 @@ namespace gtsam {
* provide the appropriate values at the appropriate time. * provide the appropriate values at the appropriate time.
*/ */
template <class Config> template <class Config>
class Factor : boost::noncopyable, public Testable< Factor<Config> > class Factor : public Testable< Factor<Config> >
{ {
public: public:

33
cpp/FactorGraph-inl.h
View File

@ -24,6 +24,12 @@
#include "FactorGraph.h" #include "FactorGraph.h"
#include "graph-inl.h" #include "graph-inl.h"
#define INSTANTIATE_FACTOR_GRAPH(F) \
template class FactorGraph<F>; \
/*template boost::shared_ptr<F> removeAndCombineFactors(FactorGraph<F>&, const std::string&);*/ \
template FactorGraph<F> combine(const FactorGraph<F>&, const FactorGraph<F>&);
// trick from some reading group // trick from some reading group
#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) #define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
@ -271,10 +277,10 @@ void FactorGraph<Factor>::associateFactor(int index, sharedFactor factor) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Factor> template <class Key> template<class Factor> template <class Key, class Factor2>
PredecessorMap<Key> FactorGraph<Factor>::findMinimumSpanningTree() const { PredecessorMap<Key> FactorGraph<Factor>::findMinimumSpanningTree() const {
SDGraph<Key> g = gtsam::toBoostGraph<FactorGraph<Factor>, sharedFactor, Key>(*this); SDGraph<Key> g = gtsam::toBoostGraph<FactorGraph<Factor>, Factor2, Key>(*this);
// find minimum spanning tree // find minimum spanning tree
vector<typename SDGraph<Key>::Vertex> p_map(boost::num_vertices(g)); vector<typename SDGraph<Key>::Vertex> p_map(boost::num_vertices(g));
@ -285,8 +291,8 @@ PredecessorMap<Key> FactorGraph<Factor>::findMinimumSpanningTree() const {
typename SDGraph<Key>::vertex_iterator itVertex = boost::vertices(g).first; typename SDGraph<Key>::vertex_iterator itVertex = boost::vertices(g).first;
typename vector<typename SDGraph<Key>::Vertex>::iterator vi; typename vector<typename SDGraph<Key>::Vertex>::iterator vi;
for (vi = p_map.begin(); vi!=p_map.end(); itVertex++, vi++) { for (vi = p_map.begin(); vi!=p_map.end(); itVertex++, vi++) {
string key = boost::get(boost::vertex_name, g, *itVertex); Key key = boost::get(boost::vertex_name, g, *itVertex);
string parent = boost::get(boost::vertex_name, g, *vi); Key parent = boost::get(boost::vertex_name, g, *vi);
// printf("%s parent: %s\n", key.c_str(), parent.c_str()); // printf("%s parent: %s\n", key.c_str(), parent.c_str());
tree.insert(key, parent); tree.insert(key, parent);
} }
@ -294,8 +300,8 @@ PredecessorMap<Key> FactorGraph<Factor>::findMinimumSpanningTree() const {
return tree; return tree;
} }
template<class Factor> template <class Key> template<class Factor> template <class Key, class Factor2>
void FactorGraph<Factor>::split(map<Key, Key> tree, FactorGraph<Factor>& Ab1, FactorGraph<Factor>& Ab2) const { void FactorGraph<Factor>::split(const PredecessorMap<Key>& tree, FactorGraph<Factor>& Ab1, FactorGraph<Factor>& Ab2) const{
BOOST_FOREACH(sharedFactor factor, factors_){ BOOST_FOREACH(sharedFactor factor, factors_){
if (factor->keys().size() > 2) if (factor->keys().size() > 2)
@ -306,14 +312,17 @@ void FactorGraph<Factor>::split(map<Key, Key> tree, FactorGraph<Factor>& Ab1, Fa
continue; continue;
} }
string key1 = factor->keys().front(); boost::shared_ptr<Factor2> factor2 = boost::dynamic_pointer_cast<Factor2>(factor);
string key2 = factor->keys().back(); if (!factor2) continue;
Key key1 = factor2->key1();
Key key2 = factor2->key2();
// if the tree contains the key // if the tree contains the key
if (tree.find(key1) != tree.end() && tree[key1].compare(key2) == 0 || if (tree.find(key1) != tree.end() && tree.find(key1)->second.compare(key2) == 0 ||
tree.find(key2) != tree.end() && tree[key2].compare(key1) == 0) tree.find(key2) != tree.end() && tree.find(key2)->second.compare(key1) == 0)
Ab1.push_back(factor); Ab1.push_back(factor2);
else else
Ab2.push_back(factor); Ab2.push_back(factor2);
} }
} }

4
cpp/FactorGraph.h
View File

@ -120,13 +120,13 @@ namespace gtsam {
/** /**
* find the minimum spanning tree. * find the minimum spanning tree.
*/ */
template<class Key> PredecessorMap<Key> findMinimumSpanningTree() const; template<class Key, class Factor2> PredecessorMap<Key> findMinimumSpanningTree() const;
/** /**
* Split the graph into two parts: one corresponds to the given spanning tre, * Split the graph into two parts: one corresponds to the given spanning tre,
* and the other corresponds to the rest of the factors * and the other corresponds to the rest of the factors
*/ */
template<class Key> void split(std::map<Key, Key> tree, template<class Key, class Factor2> void split(const PredecessorMap<Key>& tree,
FactorGraph<Factor>& Ab1, FactorGraph<Factor>& Ab2) const; FactorGraph<Factor>& Ab1, FactorGraph<Factor>& Ab2) const;
private: private:

2
cpp/GaussianFactor.h
View File

@ -27,7 +27,7 @@ namespace gtsam {
* GaussianFactor is non-mutable (all methods const!). * GaussianFactor is non-mutable (all methods const!).
* The factor value is exp(-0.5*||Ax-b||^2) * The factor value is exp(-0.5*||Ax-b||^2)
*/ */
class GaussianFactor: public Factor<VectorConfig> { class GaussianFactor: boost::noncopyable, public Factor<VectorConfig> {
public: public:
typedef boost::shared_ptr<GaussianFactor> shared_ptr; typedef boost::shared_ptr<GaussianFactor> shared_ptr;

2
cpp/ISAM2-inl.h
View File

@ -10,7 +10,7 @@ using namespace boost::assign;
#include <set> #include <set>
#include "NonlinearFactorGraph.h" #include "NonlinearFactorGraph-inl.h"
#include "GaussianFactor.h" #include "GaussianFactor.h"
#include "VectorConfig.h" #include "VectorConfig.h"

1
cpp/Key.h
View File

@ -43,6 +43,7 @@ namespace gtsam {
bool operator< (const Symbol& compare) const { return j_<compare.j_;} bool operator< (const Symbol& compare) const { return j_<compare.j_;}
bool operator== (const Symbol& compare) const { return j_==compare.j_;} bool operator== (const Symbol& compare) const { return j_==compare.j_;}
int compare(const Symbol& compare) const {return j_-compare.j_;}
private: private:

9
cpp/Lie-inl.h
View File

@ -7,6 +7,15 @@
#include "Lie.h" #include "Lie.h"
#define INSTANTIATE_LIE(T) \
template T operator*(const T&, const T&); \
template T between(const T&, const T&); \
template Vector logmap(const T&, const T&); \
template T expmap(const T&, const Vector&); \
template bool equal(const T&, const T&, double); \
template bool equal(const T&, const T&); \
template class Lie<T>;
namespace gtsam { namespace gtsam {
template<class T> template<class T>
size_t Lie<T>::dim() const { size_t Lie<T>::dim() const {

13
cpp/LieConfig-inl.h
View File

@ -5,8 +5,6 @@
* Author: richard * Author: richard
*/ */
#include "LieConfig.h"
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp> #include <boost/tuple/tuple.hpp>
#include <utility> #include <utility>
@ -14,6 +12,15 @@
#include <stdexcept> #include <stdexcept>
#include "VectorConfig.h" #include "VectorConfig.h"
#include "Lie-inl.h"
#include "LieConfig.h"
#define INSTANTIATE_LIE_CONFIG(J,T) \
/*INSTANTIATE_LIE(T);*/ \
template LieConfig<J,T> expmap(const LieConfig<J,T>&, const VectorConfig&); \
template LieConfig<J,T> expmap(const LieConfig<J,T>&, const Vector&); \
template class LieConfig<J,T>;
using namespace std; using namespace std;
@ -47,7 +54,7 @@ namespace gtsam {
template<class J, class T> template<class J, class T>
void LieConfig<J,T>::insert(const J& name, const T& val) { void LieConfig<J,T>::insert(const J& name, const T& val) {
values_.insert(make_pair(name, val)); values_.insert(make_pair(name, val));
dim_ += dim(val); dim_ += gtsam::dim(val);
} }
template<class J, class T> template<class J, class T>

43
cpp/LieConfig.h
View File

@ -13,30 +13,12 @@
#include "Vector.h" #include "Vector.h"
#include "Testable.h" #include "Testable.h"
#include "Lie.h"
#include "Key.h"
namespace boost { template<class T> class optional; } namespace boost { template<class T> class optional; }
namespace gtsam { class VectorConfig; }
namespace gtsam { namespace gtsam {
class VectorConfig;
// TODO: why are these defined *before* the class ?
template<class J, class T> class LieConfig;
/** Dimensionality of the tangent space */
template<class J, class T>
size_t dim(const LieConfig<J,T>& c);
/** Add a delta config */
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 J, class T>
LieConfig<J,T> expmap(const LieConfig<J,T>& c, const Vector& delta);
/** /**
* Lie type configuration * Lie type configuration
*/ */
@ -63,7 +45,7 @@ namespace gtsam {
LieConfig() : dim_(0) {} LieConfig() : dim_(0) {}
LieConfig(const LieConfig& config) : LieConfig(const LieConfig& config) :
values_(config.values_), dim_(dim(config)) {} values_(config.values_), dim_(config.dim_) {}
virtual ~LieConfig() {} virtual ~LieConfig() {}
/** print */ /** print */
@ -84,6 +66,11 @@ namespace gtsam {
/** The number of variables in this config */ /** The number of variables in this config */
size_t size() const { return values_.size(); } size_t size() const { return values_.size(); }
/**
* The dimensionality of the tangent space
*/
size_t dim() const { return dim_; }
const_iterator begin() const { return values_.begin(); } const_iterator begin() const { return values_.begin(); }
const_iterator end() const { return values_.end(); } const_iterator end() const { return values_.end(); }
iterator begin() { return values_.begin(); } iterator begin() { return values_.begin(); }
@ -105,7 +92,7 @@ namespace gtsam {
/** Replace all keys and variables */ /** Replace all keys and variables */
LieConfig& operator=(const LieConfig& rhs) { LieConfig& operator=(const LieConfig& rhs) {
values_ = rhs.values_; values_ = rhs.values_;
dim_ = dim(rhs); dim_ = rhs.dim_;
return (*this); return (*this);
} }
@ -115,14 +102,18 @@ namespace gtsam {
dim_ = 0; dim_ = 0;
} }
// 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 */ /** Dimensionality of the tangent space */
template<class J, class T> template<class J, class T>
size_t dim(const LieConfig<J,T>& c) { return c.dim_; } inline size_t dim(const LieConfig<J,T>& c) { return c.dim(); }
/** Add a delta config */
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 J, class T>
LieConfig<J,T> expmap(const LieConfig<J,T>& c, const Vector& delta);
} }

23
cpp/Makefile.am
View File

@ -200,11 +200,11 @@ testSimulated3D_LDADD = libgtsam.la
check_PROGRAMS += testSimulated3D check_PROGRAMS += testSimulated3D
# Pose SLAM headers # Pose SLAM headers
headers += BetweenFactor.h LieConfig.h LieConfig-inl.h TupleConfig.h headers += BetweenFactor.h LieConfig.h LieConfig-inl.h TupleConfig.h TupleConfig-inl.h
# 2D Pose constraints # 2D Pose constraints
headers += Pose2Factor.h Pose2Prior.h headers += Pose2Prior.h
sources += Pose2Config.cpp Pose2Graph.cpp sources += Pose2Graph.cpp
check_PROGRAMS += testPose2Factor testPose2Config testPose2Graph check_PROGRAMS += testPose2Factor testPose2Config testPose2Graph
testPose2Factor_SOURCES = $(example) testPose2Factor.cpp testPose2Factor_SOURCES = $(example) testPose2Factor.cpp
testPose2Factor_LDADD = libgtsam.la testPose2Factor_LDADD = libgtsam.la
@ -213,14 +213,12 @@ testPose2Config_LDADD = libgtsam.la
testPose2Graph_SOURCES = $(example) testPose2Graph.cpp testPose2Graph_SOURCES = $(example) testPose2Graph.cpp
testPose2Graph_LDADD = libgtsam.la testPose2Graph_LDADD = libgtsam.la
# 2D Bearing and Range # 2D SLAM using Bearing and Range
headers += BearingFactor.h RangeFactor.h headers +=
sources += sources += planarSLAM.cpp
check_PROGRAMS += testBearingFactor testRangeFactor check_PROGRAMS += testPlanarSLAM
testBearingFactor_SOURCES = $(example) testBearingFactor.cpp testPlanarSLAM_SOURCES = $(example) testPlanarSLAM.cpp
testBearingFactor_LDADD = libgtsam.la testPlanarSLAM_LDADD = libgtsam.la
testRangeFactor_SOURCES = $(example) testRangeFactor.cpp
testRangeFactor_LDADD = libgtsam.la
# 3D Pose constraints # 3D Pose constraints
headers += Pose3Factor.h headers += Pose3Factor.h
@ -242,8 +240,7 @@ testSimpleCamera_SOURCES = testSimpleCamera.cpp
testSimpleCamera_LDADD = libgtsam.la testSimpleCamera_LDADD = libgtsam.la
# Visual SLAM # Visual SLAM
headers += VSLAMConfig.h sources += visualSLAM.cpp
sources += VSLAMGraph.cpp VSLAMFactor.cpp
check_PROGRAMS += testVSLAMFactor testVSLAMGraph testVSLAMConfig check_PROGRAMS += testVSLAMFactor testVSLAMGraph testVSLAMConfig
testVSLAMFactor_SOURCES = testVSLAMFactor.cpp testVSLAMFactor_SOURCES = testVSLAMFactor.cpp
testVSLAMFactor_LDADD = libgtsam.la testVSLAMFactor_LDADD = libgtsam.la

7
cpp/NonlinearFactor.h
View File

@ -20,6 +20,11 @@
#include "Matrix.h" #include "Matrix.h"
#include "GaussianFactor.h" #include "GaussianFactor.h"
#define INSTANTIATE_NONLINEAR_FACTOR1(C,J,X) \
template class gtsam::NonlinearFactor1<C,J,X>;
#define INSTANTIATE_NONLINEAR_FACTOR2(C,J1,X1,J2,X2) \
template class gtsam::NonlinearFactor2<C,J1,X1,J2,X2>;
namespace gtsam { namespace gtsam {
// TODO class NoiseModel {}; // TODO class NoiseModel {};
@ -173,7 +178,7 @@ namespace gtsam {
return Base::equals(*p, tol) && (key_ == p->key_); return Base::equals(*p, tol) && (key_ == p->key_);
} }
/** error function z-h(x) */ /** error function h(x)-z */
inline Vector error_vector(const Config& x) const { inline Vector error_vector(const Config& x) const {
Key j = key_; Key j = key_;
const X& xj = x[j]; const X& xj = x[j];

5
cpp/NonlinearFactorGraph-inl.h
View File

@ -11,6 +11,11 @@
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include "GaussianFactorGraph.h" #include "GaussianFactorGraph.h"
#include "NonlinearFactorGraph.h" #include "NonlinearFactorGraph.h"
#include "FactorGraph-inl.h"
#define INSTANTIATE_NONLINEAR_FACTOR_GRAPH(C) \
INSTANTIATE_FACTOR_GRAPH(NonlinearFactor<C>); \
template class NonlinearFactorGraph<C>;
using namespace std; using namespace std;

5
cpp/NonlinearFactorGraph.h
View File

@ -41,6 +41,11 @@ namespace gtsam {
return exp(-0.5 * error(c)); return exp(-0.5 * error(c));
} }
template<class F>
void add(const F& factor) {
push_back(boost::shared_ptr<F>(new F(factor)));
}
/** /**
* linearize a nonlinear factor graph * linearize a nonlinear factor graph
*/ */

3
cpp/NonlinearOptimizer-inl.h
View File

@ -13,6 +13,9 @@
#include <boost/tuple/tuple.hpp> #include <boost/tuple/tuple.hpp>
#include "NonlinearOptimizer.h" #include "NonlinearOptimizer.h"
#define INSTANTIATE_NONLINEAR_OPTIMIZER(G,C) \
template class NonlinearOptimizer<G,C>;
using namespace std; using namespace std;
namespace gtsam { namespace gtsam {

2
cpp/Point2.cpp
View File

@ -12,7 +12,7 @@ using namespace std;
namespace gtsam { namespace gtsam {
/** Explicit instantiation of base class to export members */ /** Explicit instantiation of base class to export members */
template class Lie<Point2>; INSTANTIATE_LIE(Point2);
/* ************************************************************************* */ /* ************************************************************************* */
void Point2::print(const string& s) const { void Point2::print(const string& s) const {

2
cpp/Point3.cpp
View File

@ -10,7 +10,7 @@
namespace gtsam { namespace gtsam {
/** Explicit instantiation of base class to export members */ /** Explicit instantiation of base class to export members */
template class Lie<Point3>; INSTANTIATE_LIE(Point3);
/* ************************************************************************* */ /* ************************************************************************* */
bool Point3::equals(const Point3 & q, double tol) const { bool Point3::equals(const Point3 & q, double tol) const {

72
cpp/Pose2.cpp
View File

@ -11,7 +11,7 @@ using namespace std;
namespace gtsam { namespace gtsam {
/** Explicit instantiation of base class to export members */ /** Explicit instantiation of base class to export members */
template class Lie<Pose2>; INSTANTIATE_LIE(Pose2);
/* ************************************************************************* */ /* ************************************************************************* */
void Pose2::print(const string& s) const { void Pose2::print(const string& s) const {
@ -46,23 +46,61 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix Dbetween1(const Pose2& p1, const Pose2& p2) { Pose2 between(const Pose2& p1, const Pose2& p2, boost::optional<Matrix&> H1,
Point2 dt = p2.t()-p1.t(); boost::optional<Matrix&> H2) {
Matrix dT1 = -invcompose(p2.r(), p1.r()).matrix(); Rot2 dR = between(p1.r(), p2.r());
Matrix dR1; Point2 dt = p2.t() - p1.t();
unrotate(p2.r(), dt, dR1); Point2 q = unrotate(p1.r(), dt);
return Matrix_(3,3, Pose2 dp(dR, q);
dT1(0,0), dT1(0,1), dR1(0,0), if (H1) {
dT1(1,0), dT1(1,1), dR1(1,0), Matrix dT1 = -invcompose(p2.r(), p1.r()).matrix();
0.0, 0.0, -1.0); Matrix dR1;
} unrotate(p2.r(), dt, dR1); // FD to Richard: I do *not* understand this
*H1 = Matrix_(3,3,
dT1(0,0), dT1(0,1), dR1(0,0),
dT1(1,0), dT1(1,1), dR1(1,0),
0.0, 0.0, -1.0);
}
if (H2) *H2 = Matrix_(3,3,
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0);
return dp;
}
Matrix Dbetween2(const Pose2& p1, const Pose2& p2) { /* ************************************************************************* */
return Matrix_(3,3, Rot2 bearing(const Pose2& pose, const Point2& point) {
1.0, 0.0, 0.0, Point2 d = transform_to(pose, point);
0.0, 1.0, 0.0, return relativeBearing(d);
0.0, 0.0, 1.0); }
}
Rot2 bearing(const Pose2& pose, const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
if (!H1 && !H2) return bearing(pose, point);
Point2 d = transform_to(pose, point);
Matrix D_result_d;
Rot2 result = relativeBearing(d, D_result_d);
if (H1) *H1 = D_result_d * Dtransform_to1(pose, point);
if (H2) *H2 = D_result_d * Dtransform_to2(pose, point);
return result;
}
/* ************************************************************************* */
double range(const Pose2& pose, const Point2& point) {
Point2 d = transform_to(pose, point);
return d.norm();
}
double range(const Pose2& pose, const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
if (!H1 && !H2) return range(pose, point);
Point2 d = transform_to(pose, point);
double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2);
Matrix D_result_d = Matrix_(1, 2, x / n, y / n);
if (H1) *H1 = D_result_d * Dtransform_to1(pose, point);
if (H2) *H2 = D_result_d * Dtransform_to2(pose, point);
return n;
}
/* ************************************************************************* */ /* ************************************************************************* */
} // namespace gtsam } // namespace gtsam

33
cpp/Pose2.h
View File

@ -98,11 +98,8 @@ namespace gtsam {
return rotate(pose.r(), point)+pose.t(); } return rotate(pose.r(), point)+pose.t(); }
/** Return relative pose between p1 and p2, in p1 coordinate frame */ /** Return relative pose between p1 and p2, in p1 coordinate frame */
/** todo: make sure compiler finds this version of between. */ Pose2 between(const Pose2& p1, const Pose2& p2,
//inline Pose2 between(const Pose2& p0, const Pose2& p2) { boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
// return Pose2(p0.r().invcompose(p2.r()), p0.r().unrotate(p2.t()-p0.t())); }
Matrix Dbetween1(const Pose2& p0, const Pose2& p2);
Matrix Dbetween2(const Pose2& p0, const Pose2& p2);
/** same as compose (pre-multiply this*p1) */ /** same as compose (pre-multiply this*p1) */
inline Pose2 operator*(const Pose2& p1, const Pose2& p0) { return compose(p1, p0); } inline Pose2 operator*(const Pose2& p1, const Pose2& p0) { return compose(p1, p0); }
@ -112,7 +109,33 @@ namespace gtsam {
inline Point2 operator*(const Pose2& pose, const Point2& point) { inline Point2 operator*(const Pose2& pose, const Point2& point) {
return transform_from(pose, point); } return transform_from(pose, point); }
/**
* Calculate bearing to a landmark
* @param pose 2D pose of robot
* @param point 2D location of landmark
* @return 2D rotation \in SO(2)
*/
Rot2 bearing(const Pose2& pose, const Point2& point);
/**
* Calculate bearing and optional derivative(s)
*/
Rot2 bearing(const Pose2& pose, const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
/**
* Calculate range to a landmark
* @param pose 2D pose of robot
* @param point 2D location of landmark
* @return range (double)
*/
double range(const Pose2& pose, const Point2& point);
/**
* Calculate range and optional derivative(s)
*/
double range(const Pose2& pose, const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
} // namespace gtsam } // namespace gtsam

18
cpp/Pose2Graph.cpp
View File

@ -10,23 +10,41 @@
#include "NonlinearOptimizer-inl.h" #include "NonlinearOptimizer-inl.h"
#include "NonlinearEquality.h" #include "NonlinearEquality.h"
#include "graph-inl.h" #include "graph-inl.h"
#include "LieConfig-inl.h"
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
namespace gtsam { namespace gtsam {
/** Explicit instantiation of templated methods and functions */
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);
// explicit instantiation so all the code is there and we can link with it // explicit instantiation so all the code is there and we can link with it
template class FactorGraph<NonlinearFactor<Pose2Config> > ; template class FactorGraph<NonlinearFactor<Pose2Config> > ;
template class NonlinearFactorGraph<Pose2Config> ; template class NonlinearFactorGraph<Pose2Config> ;
template class NonlinearEquality<Pose2Config,Pose2Config::Key,Pose2> ; template class NonlinearEquality<Pose2Config,Pose2Config::Key,Pose2> ;
template class NonlinearOptimizer<Pose2Graph, Pose2Config>; template class NonlinearOptimizer<Pose2Graph, Pose2Config>;
/* ************************************************************************* */
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(i, Pose2(cos(theta), sin(theta), M_PI_2 + theta));
return x;
}
/* ************************************************************************* */
void Pose2Graph::addConstraint(const Pose2Config::Key& key, const Pose2& pose) { void Pose2Graph::addConstraint(const Pose2Config::Key& key, const Pose2& pose) {
push_back(sharedFactor(new NonlinearEquality<Pose2Config, Pose2Config::Key, push_back(sharedFactor(new NonlinearEquality<Pose2Config, Pose2Config::Key,
Pose2> (key, pose))); Pose2> (key, pose)));
} }
/* ************************************************************************* */
bool Pose2Graph::equals(const Pose2Graph& p, double tol) const { bool Pose2Graph::equals(const Pose2Graph& p, double tol) const {
return false; return false;
} }

22
cpp/Pose2Graph.h
View File

@ -7,11 +7,31 @@
#pragma once #pragma once
#include "Pose2Factor.h"
#include "NonlinearFactorGraph.h" #include "NonlinearFactorGraph.h"
#include "Pose2.h"
#include "LieConfig.h"
#include "BetweenFactor.h"
#include "Key.h"
namespace gtsam { namespace gtsam {
/**
* Pose2Config is now simply a typedef
*/
typedef LieConfig<Symbol<Pose2,'x'>,Pose2> Pose2Config;
typedef BetweenFactor<Pose2Config, Pose2Config::Key, Pose2> Pose2Factor;
/**
* Create a circle of n 2D poses tangent to circle of radius R, first pose at (R,0)
* @param n number of poses
* @param R radius of circle
* @param c character to use for keys
* @return circle of n 2D poses
*/
Pose2Config pose2Circle(size_t n, double R);
/** /**
* Non-linear factor graph for visual SLAM * Non-linear factor graph for visual SLAM
*/ */

21
cpp/Pose3.cpp
View File

@ -6,6 +6,7 @@
#include <iostream> #include <iostream>
#include "Pose3.h" #include "Pose3.h"
#include "Lie-inl.h" #include "Lie-inl.h"
#include "LieConfig.h"
using namespace std; using namespace std;
using namespace boost::numeric::ublas; using namespace boost::numeric::ublas;
@ -13,7 +14,7 @@ using namespace boost::numeric::ublas;
namespace gtsam { namespace gtsam {
/** Explicit instantiation of base class to export members */ /** Explicit instantiation of base class to export members */
template class Lie<Pose3>; INSTANTIATE_LIE(Pose3);
/* ************************************************************************* */ /* ************************************************************************* */
void Pose3::print(const string& s) const { void Pose3::print(const string& s) const {
@ -185,16 +186,14 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
// between = compose(p2,inverse(p1)); // between = compose(p2,inverse(p1));
Pose3 between(const Pose3& p1, const Pose3& p2, boost::optional<Matrix&> H1,
Matrix Dbetween1(const Pose3& p1, const Pose3& p2){ boost::optional<Matrix&> H2) {
Pose3 invp1 = inverse(p1); Pose3 invp1 = inverse(p1);
return Dcompose2(p2,invp1) * Dinverse(p1); Pose3 result = compose(p2, invp1);
} if (H1) *H1 = Dcompose2(p2, invp1) * Dinverse(p1);
if (H2) *H2 = Dcompose1(p2, invp1);
Matrix Dbetween2(const Pose3& p1, const Pose3& p2){ return result;
Pose3 invp1 = inverse(p1); }
return Dcompose1(p2,invp1);
}
/* ************************************************************************* */ /* ************************************************************************* */
} // namespace gtsam } // namespace gtsam

9
cpp/Pose3.h
View File

@ -46,6 +46,9 @@ namespace gtsam {
const Rot3& rotation() const { return R_; } const Rot3& rotation() const { return R_; }
const Point3& translation() const { return t_; } const Point3& translation() const { return t_; }
double x() const { return t_.x(); }
double y() const { return t_.y(); }
double z() const { return t_.z(); }
/** convert to 4*4 matrix */ /** convert to 4*4 matrix */
Matrix matrix() const; Matrix matrix() const;
@ -110,7 +113,6 @@ namespace gtsam {
return concatVectors(2, &r, &t); return concatVectors(2, &r, &t);
} }
/** receives the point in Pose coordinates and transforms it to world coordinates */ /** receives the point in Pose coordinates and transforms it to world coordinates */
Point3 transform_from(const Pose3& pose, const Point3& p); Point3 transform_from(const Pose3& pose, const Point3& p);
inline Point3 operator*(const Pose3& pose, const Point3& p) { return transform_from(pose, p); } inline Point3 operator*(const Pose3& pose, const Point3& p) { return transform_from(pose, p); }
@ -135,9 +137,10 @@ namespace gtsam {
/** /**
* Return relative pose between p1 and p2, in p1 coordinate frame * Return relative pose between p1 and p2, in p1 coordinate frame
* as well as optionally the derivatives
*/ */
Matrix Dbetween1(const Pose3& p1, const Pose3& p2); Pose3 between(const Pose3& p1, const Pose3& p2,
Matrix Dbetween2(const Pose3& p1, const Pose3& p2); boost::optional<Matrix&> H1, boost::optional<Matrix&> H2);
/** direct measurement of a pose */ /** direct measurement of a pose */
Vector hPose(const Vector& x); Vector hPose(const Vector& x);

1
cpp/Pose3Config.h
View File

@ -8,6 +8,7 @@
#include "Pose3.h" #include "Pose3.h"
#include "LieConfig.h" #include "LieConfig.h"
#include "Key.h"
namespace gtsam { namespace gtsam {

4
cpp/Rot2.cpp
View File

@ -12,8 +12,8 @@ using namespace std;
namespace gtsam { namespace gtsam {
/** Explicit instantiation of base class to export members */ /** Explicit instantiation of base class to export members */
template class Lie<Rot2> ; INSTANTIATE_LIE(Rot2);
/* ************************************************************************* */ /* ************************************************************************* */
void Rot2::print(const string& s) const { void Rot2::print(const string& s) const {

2
cpp/Rot3.cpp
View File

@ -14,7 +14,7 @@ using namespace std;
namespace gtsam { namespace gtsam {
/** Explicit instantiation of base class to export members */ /** Explicit instantiation of base class to export members */
template class Lie<Rot3>; INSTANTIATE_LIE(Rot3);
/* ************************************************************************* */ /* ************************************************************************* */
// static member functions to construct rotations // static member functions to construct rotations

28
cpp/TupleConfig-inl.h Normal file
View File

@ -0,0 +1,28 @@
/*
* TupleConfig-inl.h
*
* Created on: Jan 14, 2010
* Author: richard
*/
#include "LieConfig-inl.h"
#include "TupleConfig.h"
#define INSTANTIATE_PAIR_CONFIG(J1,X1,J2,X2) \
/*INSTANTIATE_LIE_CONFIG(J1,X1);*/ \
/*INSTANTIATE_LIE_CONFIG(J2,X2);*/ \
template class PairConfig<J1,X1,J2,X2>; \
/*template void PairConfig<J1,X1,J2,X2>::print(const std::string&) const;*/ \
template PairConfig<J1,X1,J2,X2> expmap(PairConfig<J1,X1,J2,X2>, const VectorConfig&);
namespace gtsam {
template<class J1, class X1, class J2, class X2>
void PairConfig<J1,X1,J2,X2>::print(const std::string& s) const {
std::cout << "TupleConfig " << s << ", size " << size_ << "\n";
first.print(s + "Config1: ");
second.print(s + "Config2: ");
}
}

48
cpp/TupleConfig.h
View File

@ -5,9 +5,7 @@
* Author: Richard Roberts and Manohar Paluri * Author: Richard Roberts and Manohar Paluri
*/ */
#include <iostream> #include "LieConfig.h"
#include "LieConfig-inl.h"
#pragma once #pragma once
@ -18,22 +16,24 @@ namespace gtsam {
*/ */
template<class J1, class X1, class J2, class X2> template<class J1, class X1, class J2, class X2>
class PairConfig : public Testable<PairConfig<J1, X1, J2, X2> > { class PairConfig : public Testable<PairConfig<J1, X1, J2, X2> > {
public: public:
// typedef J1 Key1;
// typedef J2 Key2; // publicly available types
// typedef X1 Value1;
// typedef X2 Value2;
typedef LieConfig<J1, X1> Config1; typedef LieConfig<J1, X1> Config1;
typedef LieConfig<J2, X2> Config2; typedef LieConfig<J2, X2> Config2;
// Two configs in the pair as in std:pair
LieConfig<J1, X1> first;
LieConfig<J2, X2> second;
private: private:
LieConfig<J1, X1> config1_;
LieConfig<J2, X2> config2_;
size_t size_; size_t size_;
size_t dim_; size_t dim_;
PairConfig(const LieConfig<J1,X1>& config1, const LieConfig<J2,X2>& config2) : PairConfig(const LieConfig<J1,X1>& config1, const LieConfig<J2,X2>& config2) :
config1_(config1), config2_(config2), first(config1), second(config2),
size_(config1.size()+config2.size()), dim_(gtsam::dim(config1)+gtsam::dim(config2)) {} size_(config1.size()+config2.size()), dim_(gtsam::dim(config1)+gtsam::dim(config2)) {}
public: public:
@ -47,30 +47,26 @@ namespace gtsam {
* Copy constructor * Copy constructor
*/ */
PairConfig(const PairConfig<J1, X1, J2, X2>& c): PairConfig(const PairConfig<J1, X1, J2, X2>& c):
config1_(c.config1_), config2_(c.config2_), size_(c.size_), dim_(c.dim_) {} first(c.first), second(c.second), size_(c.size_), dim_(c.dim_) {}
/** /**
* Print * Print
*/ */
void print(const std::string& s = "") const { void print(const std::string& s = "") const;
std::cout << "TupleConfig " << s << ", size " << size_ << "\n";
config1_.print(s + "Config1: ");
config2_.print(s + "Config2: ");
}
/** /**
* Test for equality in keys and values * Test for equality in keys and values
*/ */
bool equals(const PairConfig<J1, X1, J2, X2>& c, double tol=1e-9) const { bool equals(const PairConfig<J1, X1, J2, X2>& c, double tol=1e-9) const {
return config1_.equals(c.config1_, tol) && config2_.equals(c.config2_, tol); } return first.equals(c.first, tol) && second.equals(c.second, tol); }
/** /**
* operator[] syntax to get a value by j, throws invalid_argument if * operator[] syntax to get a value by j, throws invalid_argument if
* value with specified j is not present. Will generate compile-time * value with specified j is not present. Will generate compile-time
* errors if j type does not match that on which the Config is templated. * errors if j type does not match that on which the Config is templated.
*/ */
const X1& operator[](const J1& j) const { return config1_[j]; } const X1& operator[](const J1& j) const { return first[j]; }
const X2& operator[](const J2& j) const { return config2_[j]; } const X2& operator[](const J2& j) const { return second[j]; }
/** /**
* size is the total number of variables in this config. * size is the total number of variables in this config.
@ -103,26 +99,26 @@ namespace gtsam {
* expmap each element * expmap each element
*/ */
PairConfig<J1,X1,J2,X2> expmap(const VectorConfig& delta) { PairConfig<J1,X1,J2,X2> expmap(const VectorConfig& delta) {
return PairConfig(gtsam::expmap(config1_, delta), gtsam::expmap(config2_, delta)); } return PairConfig(gtsam::expmap(first, delta), gtsam::expmap(second, delta)); }
/** /**
* Insert a variable with the given j * Insert a variable with the given j
*/ */
void insert(const J1& j, const X1& value) { insert_helper(config1_, j, value); } void insert(const J1& j, const X1& value) { insert_helper(first, j, value); }
void insert(const J2& j, const X2& value) { insert_helper(config2_, j, value); } void insert(const J2& j, const X2& value) { insert_helper(second, j, value); }
/** /**
* Remove the variable with the given j. Throws invalid_argument if the * Remove the variable with the given j. Throws invalid_argument if the
* j is not present in the config. * j is not present in the config.
*/ */
void erase(const J1& j) { erase_helper(config1_, j); } void erase(const J1& j) { erase_helper(first, j); }
void erase(const J2& j) { erase_helper(config2_, j); } void erase(const J2& j) { erase_helper(second, j); }
/** /**
* Check if a variable exists * Check if a variable exists
*/ */
bool exists(const J1& j) const { return config1_.exists(j); } bool exists(const J1& j) const { return first.exists(j); }
bool exists(const J2& j) const { return config2_.exists(j); } bool exists(const J2& j) const { return second.exists(j); }
}; };

20
cpp/VSLAMConfig.cpp Normal file
View File

@ -0,0 +1,20 @@
/**
* @file VSLAMConfig.cpp
* @brief LieConfig instantiations
* @author Frank Dellaert
*/
#include "VSLAMConfig.h"
#include "LieConfig-inl.h"
#include "TupleConfig.h"
namespace gtsam {
// template class LieConfig<VSLAMPoseKey, Pose3> ; // not this one as duplicate
template class LieConfig<VSLAMPointKey, Point3> ;
template class PairConfig<VSLAMPoseKey, Pose3, VSLAMPointKey, Point3> ;
/* ************************************************************************* */
} // namespace gtsam

42
cpp/VSLAMFactor.cpp
View File

@ -1,42 +0,0 @@
/**
* @file VSLAMFactor.cpp
* @brief A non-linear factor specialized to the Visual SLAM problem
* @author Alireza Fathi
*/
#include <boost/bind.hpp>
#include <boost/bind/placeholders.hpp>
#include "VSLAMFactor.h"
#include "SimpleCamera.h"
using namespace std;
namespace gtsam {
template class NonlinearFactor2<VSLAMConfig, VSLAMPoseKey, Pose3, VSLAMPointKey, Point3>;
/* ************************************************************************* */
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 {
VSLAMFactorBase::print(s);
z_.print(s + ".z");
}
/* ************************************************************************* */
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

91
cpp/VSLAMFactor.h
View File

@ -1,91 +0,0 @@
/**
* @file VSLAMFactor.h
* @brief A Nonlinear Factor, specialized for visual SLAM
* @author Alireza Fathi
*/
#pragma once
#include <boost/optional.hpp>
#include "NonlinearFactor.h"
#include "SimpleCamera.h"
#include "VSLAMConfig.h"
#include "Cal3_S2.h"
namespace gtsam {
typedef NonlinearFactor2<VSLAMConfig,
VSLAMPoseKey, Pose3, VSLAMPointKey, Point3> VSLAMFactorBase;
/**
* Non-linear factor for a constraint derived from a 2D measurement,
* i.e. the main building block for visual SLAM.
*/
class VSLAMFactor: public VSLAMFactorBase , Testable<VSLAMFactor> {
private:
// Keep a copy of measurement and calibration for I/O
Point2 z_;
boost::shared_ptr<Cal3_S2> K_;
public:
// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<VSLAMFactor> shared_ptr;
/**
* Default constructor
*/
VSLAMFactor();
/**
* 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);
/**
* print
* @param s optional string naming the factor
*/
void print(const std::string& s = "VSLAMFactor") const;
/**
* equals
*/
bool equals(const VSLAMFactor&, double tol = 1e-9) const;
/** h(x) */
Point2 predict(const Pose3& pose, const Point3& point) const {
return SimpleCamera(*K_, pose).project(point);
}
/** 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_);
}
};
}

59
cpp/VSLAMGraph.cpp
View File

@ -1,59 +0,0 @@
/**
* @file VSLAMGraph.h
* @brief A factor graph for the VSLAM problem
* @author Alireza Fathi
* @author Carlos Nieto
*/
#include <set>
#include <fstream>
#include <boost/foreach.hpp>
#include "VSLAMGraph.h"
#include "NonlinearFactorGraph-inl.h"
#include "NonlinearOptimizer-inl.h"
#include "NonlinearEquality.h"
using namespace std;
namespace gtsam {
// explicit instantiation so all the code is there and we can link with it
template class FactorGraph<NonlinearFactor<VSLAMConfig> >;
template class NonlinearFactorGraph<VSLAMConfig>;
template class NonlinearOptimizer<VSLAMGraph,VSLAMConfig>;
/* ************************************************************************* */
bool compareLandmark(const std::string& key,
const VSLAMConfig& feasible,
const VSLAMConfig& input) {
int j = atoi(key.substr(1, key.size() - 1).c_str());
return feasible[VSLAMPointKey(j)].equals(input[VSLAMPointKey(j)]);
}
/* ************************************************************************* */
void VSLAMGraph::addLandmarkConstraint(int j, const gtsam::Point3& p) {
typedef NonlinearEquality<VSLAMConfig,VSLAMPointKey,Point3> NLE;
boost::shared_ptr<NLE> factor(new NLE(j, p));
push_back(factor);
}
/* ************************************************************************* */
bool compareCamera(const std::string& key,
const VSLAMConfig& feasible,
const VSLAMConfig& input) {
int j = atoi(key.substr(1, key.size() - 1).c_str());
return feasible[VSLAMPoseKey(j)].equals(input[VSLAMPoseKey(j)]);
}
/* ************************************************************************* */
void VSLAMGraph::addCameraConstraint(int j, const gtsam::Pose3& p) {
typedef NonlinearEquality<VSLAMConfig,VSLAMPoseKey,Pose3> NLE;
boost::shared_ptr<NLE> factor(new NLE(j,p));
push_back(factor);
}
/* ************************************************************************* */
} // namespace gtsam

68
cpp/VSLAMGraph.h
View File

@ -1,68 +0,0 @@
/**
* @file VSLAMGraph.h
* @brief A factor graph for the VSLAM problem
* @author Alireza Fathi
* @author Carlos Nieto
*/
#pragma once
#include <vector>
#include <map>
#include <set>
#include <fstream>
#include "VSLAMFactor.h"
#include "NonlinearFactorGraph.h"
#include "FactorGraph-inl.h"
#include "VSLAMConfig.h"
#include "Testable.h"
namespace gtsam{
/**
* Non-linear factor graph for visual SLAM
*/
class VSLAMGraph : public gtsam::NonlinearFactorGraph<VSLAMConfig>{
public:
/** default constructor is empty graph */
VSLAMGraph() {}
/**
* print out graph
*/
void print(const std::string& s = "") const {
gtsam::NonlinearFactorGraph<VSLAMConfig>::print(s);
}
/**
* equals
*/
bool equals(const VSLAMGraph& p, double tol=1e-9) const {
return gtsam::NonlinearFactorGraph<VSLAMConfig>::equals(p, tol);
}
/**
* Add a constraint on a landmark (for now, *must* be satisfied in any Config)
* @param j index of landmark
* @param p to which point to constrain it to
*/
void addLandmarkConstraint(int j, const Point3& p = Point3());
/**
* Add a constraint on a camera (for now, *must* be satisfied in any Config)
* @param j index of camera
* @param p to which pose to constrain it to
*/
void addCameraConstraint(int j, const Pose3& p = Pose3());
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {}
};
} // namespace gtsam

16
cpp/graph-inl.h
View File

@ -17,12 +17,6 @@ using namespace std;
namespace gtsam { namespace gtsam {
// some typedefs we need
//typedef boost::graph_traits<SDGraph>::vertex_iterator BoostVertexIterator;
/* ************************************************************************* */ /* ************************************************************************* */
template<class G, class F, class Key> template<class G, class F, class Key>
SDGraph<Key> toBoostGraph(const G& graph) { SDGraph<Key> toBoostGraph(const G& graph) {
@ -31,13 +25,17 @@ SDGraph<Key> toBoostGraph(const G& graph) {
typedef typename boost::graph_traits<SDGraph<Key> >::vertex_descriptor BoostVertex; typedef typename boost::graph_traits<SDGraph<Key> >::vertex_descriptor BoostVertex;
map<Key, BoostVertex> key2vertex; map<Key, BoostVertex> key2vertex;
BoostVertex v1, v2; BoostVertex v1, v2;
BOOST_FOREACH(F factor, graph) { typename G::const_iterator itFactor;
if (factor->keys().size() > 2) for(itFactor=graph.begin(); itFactor!=graph.end(); itFactor++) {
if ((*itFactor)->keys().size() > 2)
throw(invalid_argument("toBoostGraph: only support factors with at most two keys")); throw(invalid_argument("toBoostGraph: only support factors with at most two keys"));
if (factor->keys().size() == 1) if ((*itFactor)->keys().size() == 1)
continue; continue;
boost::shared_ptr<F> factor = boost::dynamic_pointer_cast<F>(*itFactor);
if (!factor) continue;
Key key1 = factor->key1(); Key key1 = factor->key1();
Key key2 = factor->key2(); Key key2 = factor->key2();

47
cpp/planarSLAM.cpp Normal file
View File

@ -0,0 +1,47 @@
/**
* @file planarSLAM.cpp
* @brief: bearing/range measurements in 2D plane
* @authors Frank Dellaert
**/
#include "planarSLAM.h"
#include "NonlinearFactorGraph-inl.h"
#include "NonlinearOptimizer-inl.h"
#include "TupleConfig-inl.h"
// Use planarSLAM namespace for specific SLAM instance
namespace gtsam {
using namespace planarSLAM;
INSTANTIATE_PAIR_CONFIG(PoseKey, Pose2, PointKey, Point2)
INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Config)
INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Config)
namespace planarSLAM {
void Graph::addPoseConstraint(const PoseKey& i, const Pose2& p) {
sharedFactor factor(new Constraint(i, p));
push_back(factor);
}
void Graph::addOdometry(const PoseKey& i, const PoseKey& j, const Pose2& z,
const Matrix& cov) {
sharedFactor factor(new Odometry(i, j, z, cov));
push_back(factor);
}
void Graph::addBearing(const PoseKey& i, const PointKey& j, const Rot2& z,
double sigma) {
sharedFactor factor(new Bearing(i, j, z, sigma));
push_back(factor);
}
void Graph::addRange(const PoseKey& i, const PointKey& j, double z,
double sigma) {
sharedFactor factor(new Range(i, j, z, sigma));
push_back(factor);
}
} // planarSLAM
} // gtsam

106
cpp/planarSLAM.h Normal file
View File

@ -0,0 +1,106 @@
/**
* @file planarSLAM.h
* @brief: bearing/range measurements in 2D plane
* @authors Frank Dellaert
**/
#pragma once
#include "Key.h"
#include "Pose2.h"
#include "Point2.h"
#include "NonlinearFactor.h"
#include "TupleConfig.h"
#include "NonlinearEquality.h"
#include "BetweenFactor.h"
#include "NonlinearFactorGraph.h"
#include "NonlinearOptimizer.h"
// We use gtsam namespace for generally useful factors
namespace gtsam {
/**
* Binary factor for a bearing measurement
*/
template<class Config, class PoseKey, class PointKey>
class BearingFactor: public NonlinearFactor2<Config, PoseKey, Pose2,
PointKey, Point2> {
private:
Rot2 z_; /** measurement */
typedef NonlinearFactor2<Config, PoseKey, Pose2, PointKey, Point2> Base;
public:
BearingFactor(); /* Default constructor */
BearingFactor(const PoseKey& i, const PointKey& j, const Rot2& z,
double sigma) :
Base(sigma, i, j), z_(z) {
}
/** h(x)-z -> between(z,h(x)) for Rot2 manifold */
Vector evaluateError(const Pose2& pose, const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
Rot2 hx = bearing(pose, point, H1, H2);
return logmap(between(z_, hx));
}
}; // BearingFactor
/**
* Binary factor for a range measurement
*/
template<class Config, class PoseKey, class PointKey>
class RangeFactor: public NonlinearFactor2<Config, PoseKey, Pose2, PointKey,
Point2> {
private:
double z_; /** measurement */
typedef NonlinearFactor2<Config, PoseKey, Pose2, PointKey, Point2> Base;
public:
RangeFactor(); /* Default constructor */
RangeFactor(const PoseKey& i, const PointKey& j, double z, double sigma) :
Base(sigma, i, j), z_(z) {
}
/** h(x)-z */
Vector evaluateError(const Pose2& pose, const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
double hx = gtsam::range(pose, point, H1, H2);
return Vector_(1, hx - z_);
}
}; // RangeFactor
// Use planarSLAM namespace for specific SLAM instance
namespace planarSLAM {
// Keys and Config
typedef Symbol<Pose2, 'x'> PoseKey;
typedef Symbol<Point2, 'l'> PointKey;
typedef PairConfig<PoseKey, Pose2, PointKey, Point2> Config;
// Factors
typedef NonlinearEquality<Config, PoseKey, Pose2> Constraint;
typedef BetweenFactor<Config, PoseKey, Pose2> Odometry;
typedef BearingFactor<Config, PoseKey, PointKey> Bearing;
typedef RangeFactor<Config, PoseKey, PointKey> Range;
// Graph
struct Graph: public NonlinearFactorGraph<Config> {
void addPoseConstraint(const PoseKey& i, const Pose2& p);
void addOdometry(const PoseKey& i, const PoseKey& j, const Pose2& z, const Matrix& cov);
void addBearing(const PoseKey& i, const PointKey& j, const Rot2& z, double sigma);
void addRange(const PoseKey& i, const PointKey& j, double z, double sigma);
};
// Optimizer
typedef NonlinearOptimizer<Graph, Config> Optimizer;
} // planarSLAM
} // namespace gtsam

6
cpp/testGaussianFactorGraph.cpp
View File

@ -743,7 +743,7 @@ TEST( GaussianFactorGraph, findMinimumSpanningTree )
g.add("x2", I, "x4", I, b, 0); g.add("x2", I, "x4", I, b, 0);
g.add("x3", I, "x4", I, b, 0); g.add("x3", I, "x4", I, b, 0);
map<string, string> tree = g.findMinimumSpanningTree<string>(); map<string, string> tree = g.findMinimumSpanningTree<string, GaussianFactor>();
CHECK(tree["x1"].compare("x1")==0); CHECK(tree["x1"].compare("x1")==0);
CHECK(tree["x2"].compare("x1")==0); CHECK(tree["x2"].compare("x1")==0);
CHECK(tree["x3"].compare("x1")==0); CHECK(tree["x3"].compare("x1")==0);
@ -762,14 +762,14 @@ TEST( GaussianFactorGraph, split )
g.add("x2", I, "x3", I, b, 0); g.add("x2", I, "x3", I, b, 0);
g.add("x2", I, "x4", I, b, 0); g.add("x2", I, "x4", I, b, 0);
map<string, string> tree; PredecessorMap<string> tree;
tree["x1"] = "x1"; tree["x1"] = "x1";
tree["x2"] = "x1"; tree["x2"] = "x1";
tree["x3"] = "x1"; tree["x3"] = "x1";
tree["x4"] = "x1"; tree["x4"] = "x1";
GaussianFactorGraph Ab1, Ab2; GaussianFactorGraph Ab1, Ab2;
g.split<string>(tree, Ab1, Ab2); g.split<string, GaussianFactor>(tree, Ab1, Ab2);
LONGS_EQUAL(3, Ab1.size()); LONGS_EQUAL(3, Ab1.size());
LONGS_EQUAL(2, Ab2.size()); LONGS_EQUAL(2, Ab2.size());
} }

2
cpp/testOrdering.cpp
View File

@ -6,7 +6,7 @@
#include <boost/assign/std/list.hpp> // for operator += #include <boost/assign/std/list.hpp> // for operator +=
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include "Ordering-inl.h" #include "Ordering-inl.h"
#include "Pose2Config.h" #include "Pose2Graph.h"
using namespace std; using namespace std;

90
cpp/testPlanarSLAM.cpp Normal file
View File

@ -0,0 +1,90 @@
/**
* @file testPlanarSLAM.cpp
* @authors Frank Dellaert
**/
#include <iostream>
#include <CppUnitLite/TestHarness.h>
#include "planarSLAM.h"
using namespace std;
using namespace gtsam;
// some shared test values
Pose2 x1, x2(1, 1, 0), x3(1, 1, M_PI_4);
Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3);
/* ************************************************************************* */
TEST( planarSLAM, BearingFactor )
{
// Create factor
Rot2 z(M_PI_4 + 0.1); // h(x) - z = -0.1
double sigma = 0.1;
planarSLAM::Bearing factor(2, 3, z, sigma);
// create config
planarSLAM::Config c;
c.insert(2, x2);
c.insert(3, l3);
// Check error
Vector actual = factor.error_vector(c);
CHECK(assert_equal(Vector_(1,-0.1),actual));
}
/* ************************************************************************* */
TEST( planarSLAM, RangeFactor )
{
// Create factor
double z(sqrt(2) - 0.22); // h(x) - z = 0.22
double sigma = 0.1;
planarSLAM::Range factor(2, 3, z, sigma);
// create config
planarSLAM::Config c;
c.insert(2, x2);
c.insert(3, l3);
// Check error
Vector actual = factor.error_vector(c);
CHECK(assert_equal(Vector_(1,0.22),actual));
}
/* ************************************************************************* */
TEST( planarSLAM, constructor )
{
// create config
planarSLAM::Config c;
c.insert(2, x2);
c.insert(3, x3);
c.insert(3, l3);
// create graph
planarSLAM::Graph G;
// Add pose constraint
G.addPoseConstraint(2, x2); // make it feasible :-)
// Add odometry
G.addOdometry(2, 3, Pose2(0, 0, M_PI_4), eye(3));
// Create bearing factor
Rot2 z1(M_PI_4 + 0.1); // h(x) - z = -0.1
double sigma1 = 0.1;
G.addBearing(2, 3, z1, sigma1);
// Create range factor
double z2(sqrt(2) - 0.22); // h(x) - z = 0.22
double sigma2 = 0.1;
G.addRange(2, 3, z2, sigma2);
Vector expected = Vector_(8, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.1, 0.22);
CHECK(assert_equal(expected,G.error_vector(c)));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

99
cpp/testPose2.cpp
View File

@ -152,13 +152,20 @@ TEST(Pose2, compose_c)
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Pose2, between ) TEST( Pose2, between )
{ {
//cout << "between" << endl; // <
//
// ^
//
// *--0--*--*
Pose2 p1(M_PI_2, Point2(1,2)); // robot at (1,2) looking towards y Pose2 p1(M_PI_2, Point2(1,2)); // robot at (1,2) looking towards y
Pose2 p2(M_PI, Point2(-1,4)); // robot at (-1,4) loooking at negative x Pose2 p2(M_PI, Point2(-1,4)); // robot at (-1,4) loooking at negative x
Matrix actualH1,actualH2;
Pose2 expected(M_PI_2, Point2(2,2)); Pose2 expected(M_PI_2, Point2(2,2));
Pose2 actual = between(p1,p2); Pose2 actual1 = between(p1,p2);
CHECK(assert_equal(expected,actual)); Pose2 actual2 = between(p1,p2,actualH1,actualH2);
CHECK(assert_equal(expected,actual1));
CHECK(assert_equal(expected,actual2));
Matrix expectedH1 = Matrix_(3,3, Matrix expectedH1 = Matrix_(3,3,
0.0,-1.0,-2.0, 0.0,-1.0,-2.0,
@ -166,7 +173,6 @@ TEST( Pose2, between )
0.0, 0.0,-1.0 0.0, 0.0,-1.0
); );
Matrix numericalH1 = numericalDerivative21(between<Pose2>, p1, p2, 1e-5); Matrix numericalH1 = numericalDerivative21(between<Pose2>, p1, p2, 1e-5);
Matrix actualH1 = Dbetween1(p1,p2);
CHECK(assert_equal(expectedH1,actualH1)); CHECK(assert_equal(expectedH1,actualH1));
CHECK(assert_equal(numericalH1,actualH1)); CHECK(assert_equal(numericalH1,actualH1));
@ -176,11 +182,25 @@ TEST( Pose2, between )
0.0, 0.0, 1.0 0.0, 0.0, 1.0
); );
Matrix numericalH2 = numericalDerivative22(between<Pose2>, p1, p2, 1e-5); Matrix numericalH2 = numericalDerivative22(between<Pose2>, p1, p2, 1e-5);
Matrix actualH2 = Dbetween2(p1,p2);
CHECK(assert_equal(expectedH2,actualH2)); CHECK(assert_equal(expectedH2,actualH2));
CHECK(assert_equal(numericalH2,actualH2)); CHECK(assert_equal(numericalH2,actualH2));
} }
/* ************************************************************************* */
// reverse situation for extra test
TEST( Pose2, between2 )
{
Pose2 p2(M_PI_2, Point2(1,2)); // robot at (1,2) looking towards y
Pose2 p1(M_PI, Point2(-1,4)); // robot at (-1,4) loooking at negative x
Matrix actualH1,actualH2;
between(p1,p2,actualH1,actualH2);
Matrix numericalH1 = numericalDerivative21(between<Pose2>, p1, p2, 1e-5);
CHECK(assert_equal(numericalH1,actualH1));
Matrix numericalH2 = numericalDerivative22(between<Pose2>, p1, p2, 1e-5);
CHECK(assert_equal(numericalH2,actualH2));
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Pose2, round_trip ) TEST( Pose2, round_trip )
{ {
@ -197,6 +217,75 @@ TEST(Pose2, members)
CHECK(pose.dim() == 3); CHECK(pose.dim() == 3);
} }
/* ************************************************************************* */
// some shared test values
Pose2 x1, x2(1, 1, 0), x3(1, 1, M_PI_4);
Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3);
/* ************************************************************************* */
TEST( Pose2, bearing )
{
Matrix expectedH1, actualH1, expectedH2, actualH2;
// establish bearing is indeed zero
CHECK(assert_equal(Rot2(),bearing(x1,l1)));
// establish bearing is indeed 45 degrees
CHECK(assert_equal(Rot2(M_PI_4),bearing(x1,l2)));
// establish bearing is indeed 45 degrees even if shifted
Rot2 actual23 = bearing(x2, l3, actualH1, actualH2);
CHECK(assert_equal(Rot2(M_PI_4),actual23));
// Check numerical derivatives
expectedH1 = numericalDerivative21(bearing, x2, l3, 1e-5);
CHECK(assert_equal(expectedH1,actualH1));
expectedH2 = numericalDerivative22(bearing, x2, l3, 1e-5);
CHECK(assert_equal(expectedH1,actualH1));
// establish bearing is indeed 45 degrees even if rotated
Rot2 actual34 = bearing(x3, l4, actualH1, actualH2);
CHECK(assert_equal(Rot2(M_PI_4),actual34));
// Check numerical derivatives
expectedH1 = numericalDerivative21(bearing, x3, l4, 1e-5);
expectedH2 = numericalDerivative22(bearing, x3, l4, 1e-5);
CHECK(assert_equal(expectedH1,actualH1));
CHECK(assert_equal(expectedH1,actualH1));
}
/* ************************************************************************* */
TEST( Pose2, range )
{
Matrix expectedH1, actualH1, expectedH2, actualH2;
// establish range is indeed zero
DOUBLES_EQUAL(1,gtsam::range(x1,l1),1e-9);
// establish range is indeed 45 degrees
DOUBLES_EQUAL(sqrt(2),gtsam::range(x1,l2),1e-9);
// Another pair
double actual23 = gtsam::range(x2, l3, actualH1, actualH2);
DOUBLES_EQUAL(sqrt(2),actual23,1e-9);
// Check numerical derivatives
expectedH1 = numericalDerivative21(range, x2, l3, 1e-5);
CHECK(assert_equal(expectedH1,actualH1));
expectedH2 = numericalDerivative22(range, x2, l3, 1e-5);
CHECK(assert_equal(expectedH1,actualH1));
// Another test
double actual34 = gtsam::range(x3, l4, actualH1, actualH2);
DOUBLES_EQUAL(2,actual34,1e-9);
// Check numerical derivatives
expectedH1 = numericalDerivative21(range, x3, l4, 1e-5);
expectedH2 = numericalDerivative22(range, x3, l4, 1e-5);
CHECK(assert_equal(expectedH1,actualH1));
CHECK(assert_equal(expectedH1,actualH1));
}
/* ************************************************************************* */ /* ************************************************************************* */
int main() { int main() {
TestResult tr; TestResult tr;

2
cpp/testPose2Config.cpp
View File

@ -6,7 +6,7 @@
#include <iostream> #include <iostream>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include "Pose2Config.h" #include "Pose2Graph.h"
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;

2
cpp/testPose2Factor.cpp
View File

@ -6,7 +6,7 @@
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include "numericalDerivative.h" #include "numericalDerivative.h"
#include "Pose2Factor.h" #include "Pose2Graph.h"
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;

40
cpp/testPose2Graph.cpp
View File

@ -12,8 +12,8 @@ using namespace boost::assign;
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include "NonlinearOptimizer-inl.h" #include "NonlinearOptimizer-inl.h"
#include "FactorGraph-inl.h"
#include "Ordering.h" #include "Ordering.h"
#include "Pose2Config.h"
#include "Pose2Graph.h" #include "Pose2Graph.h"
using namespace std; using namespace std;
@ -156,6 +156,44 @@ TEST(Pose2Graph, optimizeCircle) {
CHECK(assert_equal(delta,between(actual[5],actual[0]))); CHECK(assert_equal(delta,between(actual[5],actual[0])));
} }
/* ************************************************************************* */
// test optimization with 6 poses arranged in a hexagon and a loop closure
TEST(Pose2Graph, findMinimumSpanningTree) {
typedef Pose2Config::Key Key;
Pose2Graph G, T, C;
Matrix cov = eye(3);
G.push_back(Pose2Graph::sharedFactor(new Pose2Factor(Key(1), Key(2), Pose2(0.,0.,0.), cov)));
G.push_back(Pose2Graph::sharedFactor(new Pose2Factor(Key(1), Key(3), Pose2(0.,0.,0.), cov)));
G.push_back(Pose2Graph::sharedFactor(new Pose2Factor(Key(2), Key(3), Pose2(0.,0.,0.), cov)));
PredecessorMap<Key> tree = G.findMinimumSpanningTree<Key, Pose2Factor>();
CHECK(tree[Key(1)] == Key(1));
CHECK(tree[Key(2)] == Key(1));
CHECK(tree[Key(3)] == Key(1));
}
/* ************************************************************************* */
// test optimization with 6 poses arranged in a hexagon and a loop closure
TEST(Pose2Graph, split) {
typedef Pose2Config::Key Key;
Pose2Graph G, T, C;
Matrix cov = eye(3);
G.push_back(Pose2Graph::sharedFactor(new Pose2Factor(Key(1), Key(2), Pose2(0.,0.,0.), cov)));
G.push_back(Pose2Graph::sharedFactor(new Pose2Factor(Key(1), Key(3), Pose2(0.,0.,0.), cov)));
G.push_back(Pose2Graph::sharedFactor(new Pose2Factor(Key(2), Key(3), Pose2(0.,0.,0.), cov)));
PredecessorMap<Key> tree;
tree.insert(Key(1),Key(2));
tree.insert(Key(2),Key(2));
tree.insert(Key(3),Key(2));
G.split<Key, Pose2Factor>(tree, T, C);
LONGS_EQUAL(2, T.size());
LONGS_EQUAL(1, C.size());
}
/* ************************************************************************* */ /* ************************************************************************* */
int main() { int main() {
TestResult tr; TestResult tr;

5
cpp/testPose3.cpp
View File

@ -312,14 +312,13 @@ TEST( Pose3, between )
Pose3 T(R,t); Pose3 T(R,t);
Pose3 expected = pose1 * inverse(T); Pose3 expected = pose1 * inverse(T);
Pose3 actual = between(T, pose1); Matrix actualH1,actualH2;
Pose3 actual = between(T, pose1, actualH1,actualH2);
CHECK(assert_equal(expected,actual)); CHECK(assert_equal(expected,actual));
Matrix numericalH1 = numericalDerivative21(between<Pose3> , T, pose1, 1e-5); Matrix numericalH1 = numericalDerivative21(between<Pose3> , T, pose1, 1e-5);
Matrix actualH1 = Dbetween1(T, pose1);
CHECK(assert_equal(numericalH1,actualH1)); // chain rule does not work ?? CHECK(assert_equal(numericalH1,actualH1)); // chain rule does not work ??
Matrix actualH2 = Dbetween2(T, pose1);
Matrix numericalH2 = numericalDerivative22(between<Pose3> , T, pose1, 1e-5); Matrix numericalH2 = numericalDerivative22(between<Pose3> , T, pose1, 1e-5);
CHECK(assert_equal(numericalH2,actualH2)); CHECK(assert_equal(numericalH2,actualH2));
} }

23
cpp/testRot2.cpp
View File

@ -97,6 +97,29 @@ TEST( Rot2, unrotate)
CHECK(assert_equal(numerical2,H2)); CHECK(assert_equal(numerical2,H2));
} }
/* ************************************************************************* */
TEST( Rot2, relativeBearing )
{
Point2 l1(1, 0), l2(1, 1);
Matrix expectedH, actualH;
// establish relativeBearing is indeed zero
Rot2 actual1 = relativeBearing(l1, actualH);
CHECK(assert_equal(Rot2(),actual1));
// Check numerical derivative
expectedH = numericalDerivative11(relativeBearing, l1, 1e-5);
CHECK(assert_equal(expectedH,actualH));
// establish relativeBearing is indeed 45 degrees
Rot2 actual2 = relativeBearing(l2, actualH);
CHECK(assert_equal(Rot2(M_PI_4),actual2));
// Check numerical derivative
expectedH = numericalDerivative11(relativeBearing, l2, 1e-5);
CHECK(assert_equal(expectedH,actualH));
}
/* ************************************************************************* */ /* ************************************************************************* */
int main() { int main() {
TestResult tr; TestResult tr;

91
cpp/testSQP.cpp
View File

@ -12,17 +12,11 @@
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <GaussianFactorGraph.h> #include <GaussianFactorGraph.h>
#include <NonlinearFactor.h>
#include <NonlinearEquality.h>
#include <NonlinearFactorGraph.h>
#include <NonlinearOptimizer.h> #include <NonlinearOptimizer.h>
#include <SQPOptimizer.h> #include <SQPOptimizer.h>
#include <simulated2D.h> #include <simulated2D.h>
#include <Ordering.h> #include <Ordering.h>
#include <VSLAMConfig.h> #include <visualSLAM.h>
#include <VSLAMFactor.h>
#include <VSLAMGraph.h>
#include <SimpleCamera.h>
// templated implementations // templated implementations
#include <NonlinearFactorGraph-inl.h> #include <NonlinearFactorGraph-inl.h>
@ -468,10 +462,13 @@ size_t w=640,h=480;
Cal3_S2 K(fov,w,h); Cal3_S2 K(fov,w,h);
boost::shared_ptr<Cal3_S2> shK(new Cal3_S2(K)); boost::shared_ptr<Cal3_S2> shK(new Cal3_S2(K));
using namespace gtsam::visualSLAM;
using namespace boost;
// typedefs for visual SLAM example // typedefs for visual SLAM example
typedef boost::shared_ptr<VSLAMFactor> shared_vf; typedef boost::shared_ptr<ProjectionFactor> shared_vf;
typedef NonlinearOptimizer<VSLAMGraph,VSLAMConfig> VOptimizer; typedef NonlinearOptimizer<Graph,Config> VOptimizer;
typedef SQPOptimizer<VSLAMGraph, VSLAMConfig> SOptimizer; typedef SQPOptimizer<Graph, Config> SOptimizer;
/** /**
* Ground truth for a visual SLAM example with stereo vision * Ground truth for a visual SLAM example with stereo vision
@ -492,26 +489,26 @@ TEST (SQP, stereo_truth ) {
Point3 landmarkNoisy(1.0, 6.0, 0.0); Point3 landmarkNoisy(1.0, 6.0, 0.0);
// create truth config // create truth config
boost::shared_ptr<VSLAMConfig> truthConfig(new VSLAMConfig); boost::shared_ptr<Config> truthConfig(new Config);
truthConfig->insert(1, camera1.pose()); truthConfig->insert(1, camera1.pose());
truthConfig->insert(2, camera2.pose()); truthConfig->insert(2, camera2.pose());
truthConfig->insert(1, landmark); truthConfig->insert(1, landmark);
// create graph // create graph
shared_ptr<VSLAMGraph> graph(new VSLAMGraph()); shared_ptr<Graph> graph(new Graph());
// create equality constraints for poses // create equality constraints for poses
graph->addCameraConstraint(1, camera1.pose()); graph->push_back(shared_ptr<PoseConstraint>(new PoseConstraint(1, camera1.pose())));
graph->addCameraConstraint(2, camera2.pose()); graph->push_back(shared_ptr<PoseConstraint>(new PoseConstraint(2, camera2.pose())));
// create VSLAM factors // create VSLAM factors
Point2 z1 = camera1.project(landmark); Point2 z1 = camera1.project(landmark);
if (verbose) z1.print("z1"); if (verbose) z1.print("z1");
shared_vf vf1(new VSLAMFactor(z1, 1.0, 1, 1, shK)); shared_vf vf1(new ProjectionFactor(z1, 1.0, 1, 1, shK));
graph->push_back(vf1); graph->push_back(vf1);
Point2 z2 = camera2.project(landmark); Point2 z2 = camera2.project(landmark);
if (verbose) z2.print("z2"); if (verbose) z2.print("z2");
shared_vf vf2(new VSLAMFactor(z2, 1.0, 2, 1, shK)); shared_vf vf2(new ProjectionFactor(z2, 1.0, 2, 1, shK));
graph->push_back(vf2); graph->push_back(vf2);
if (verbose) graph->print("Graph after construction"); if (verbose) graph->print("Graph after construction");
@ -559,32 +556,32 @@ TEST (SQP, stereo_truth_noisy ) {
Point3 landmarkNoisy(1.0, noisyDist, 0.0); // initial point is too far out Point3 landmarkNoisy(1.0, noisyDist, 0.0); // initial point is too far out
// create truth config // create truth config
boost::shared_ptr<VSLAMConfig> truthConfig(new VSLAMConfig); boost::shared_ptr<Config> truthConfig(new Config);
truthConfig->insert(1, camera1.pose()); truthConfig->insert(1, camera1.pose());
truthConfig->insert(2, camera2.pose()); truthConfig->insert(2, camera2.pose());
truthConfig->insert(1, landmark); truthConfig->insert(1, landmark);
// create config // create config
boost::shared_ptr<VSLAMConfig> noisyConfig(new VSLAMConfig); boost::shared_ptr<Config> noisyConfig(new Config);
noisyConfig->insert(1, camera1.pose()); noisyConfig->insert(1, camera1.pose());
noisyConfig->insert(2, camera2.pose()); noisyConfig->insert(2, camera2.pose());
noisyConfig->insert(1, landmarkNoisy); noisyConfig->insert(1, landmarkNoisy);
// create graph // create graph
shared_ptr<VSLAMGraph> graph(new VSLAMGraph()); shared_ptr<Graph> graph(new Graph());
// create equality constraints for poses // create equality constraints for poses
graph->addCameraConstraint(1, camera1.pose()); graph->push_back(shared_ptr<PoseConstraint>(new PoseConstraint(1, camera1.pose())));
graph->addCameraConstraint(2, camera2.pose()); graph->push_back(shared_ptr<PoseConstraint>(new PoseConstraint(2, camera2.pose())));
// create VSLAM factors // create VSLAM factors
Point2 z1 = camera1.project(landmark); Point2 z1 = camera1.project(landmark);
if (verbose) z1.print("z1"); if (verbose) z1.print("z1");
shared_vf vf1(new VSLAMFactor(z1, 1.0, 1, 1, shK)); shared_vf vf1(new ProjectionFactor(z1, 1.0, 1, 1, shK));
graph->push_back(vf1); graph->push_back(vf1);
Point2 z2 = camera2.project(landmark); Point2 z2 = camera2.project(landmark);
if (verbose) z2.print("z2"); if (verbose) z2.print("z2");
shared_vf vf2(new VSLAMFactor(z2, 1.0, 2, 1, shK)); shared_vf vf2(new ProjectionFactor(z2, 1.0, 2, 1, shK));
graph->push_back(vf2); graph->push_back(vf2);
if (verbose) { if (verbose) {
@ -628,25 +625,25 @@ namespace sqp_stereo {
// binary constraint between landmarks // binary constraint between landmarks
/** g(x) = x-y = 0 */ /** g(x) = x-y = 0 */
Vector g(const VSLAMConfig& config, const list<string>& keys) { Vector g(const Config& config, const list<string>& keys) {
return config[VSLAMPointKey(getNum(keys.front()))].vector() return config[PointKey(getNum(keys.front()))].vector()
- config[VSLAMPointKey(getNum(keys.back()))].vector(); - config[PointKey(getNum(keys.back()))].vector();
} }
/** jacobian at l1 */ /** jacobian at l1 */
Matrix G1(const VSLAMConfig& config, const list<string>& keys) { Matrix G1(const Config& config, const list<string>& keys) {
return eye(3); return eye(3);
} }
/** jacobian at l2 */ /** jacobian at l2 */
Matrix G2(const VSLAMConfig& config, const list<string>& keys) { Matrix G2(const Config& config, const list<string>& keys) {
return -1.0 * eye(3); return -1.0 * eye(3);
} }
} // \namespace sqp_stereo } // \namespace sqp_stereo
/* ********************************************************************* */ /* ********************************************************************* */
VSLAMGraph stereoExampleGraph() { Graph stereoExampleGraph() {
// create initial estimates // create initial estimates
Rot3 faceDownY(Matrix_(3,3, Rot3 faceDownY(Matrix_(3,3,
1.0, 0.0, 0.0, 1.0, 0.0, 0.0,
@ -660,26 +657,26 @@ VSLAMGraph stereoExampleGraph() {
Point3 landmark2(1.0, 5.0, 0.0); Point3 landmark2(1.0, 5.0, 0.0);
// create graph // create graph
VSLAMGraph graph; Graph graph;
// create equality constraints for poses // create equality constraints for poses
graph.addCameraConstraint(1, camera1.pose()); graph.push_back(shared_ptr<PoseConstraint>(new PoseConstraint(1, camera1.pose())));
graph.addCameraConstraint(2, camera2.pose()); graph.push_back(shared_ptr<PoseConstraint>(new PoseConstraint(2, camera2.pose())));
// create VSLAM factors // create factors
Point2 z1 = camera1.project(landmark1); Point2 z1 = camera1.project(landmark1);
shared_vf vf1(new VSLAMFactor(z1, 1.0, 1, 1, shK)); shared_vf vf1(new ProjectionFactor(z1, 1.0, 1, 1, shK));
graph.push_back(vf1); graph.push_back(vf1);
Point2 z2 = camera2.project(landmark2); Point2 z2 = camera2.project(landmark2);
shared_vf vf2(new VSLAMFactor(z2, 1.0, 2, 2, shK)); shared_vf vf2(new ProjectionFactor(z2, 1.0, 2, 2, shK));
graph.push_back(vf2); graph.push_back(vf2);
// create the binary equality constraint between the landmarks // create the binary equality constraint between the landmarks
// NOTE: this is really just a linear constraint that is exactly the same // NOTE: this is really just a linear constraint that is exactly the same
// as the previous examples // as the previous examples
list<string> keys; keys += "l1", "l2"; list<string> keys; keys += "l1", "l2";
boost::shared_ptr<NonlinearConstraint2<VSLAMConfig> > c2( boost::shared_ptr<NonlinearConstraint2<Config> > c2(
new NonlinearConstraint2<VSLAMConfig>( new NonlinearConstraint2<Config>(
boost::bind(sqp_stereo::g, _1, keys), boost::bind(sqp_stereo::g, _1, keys),
"l1", boost::bind(sqp_stereo::G1, _1, keys), "l1", boost::bind(sqp_stereo::G1, _1, keys),
"l2", boost::bind(sqp_stereo::G2, _1, keys), "l2", boost::bind(sqp_stereo::G2, _1, keys),
@ -690,7 +687,7 @@ VSLAMGraph stereoExampleGraph() {
} }
/* ********************************************************************* */ /* ********************************************************************* */
boost::shared_ptr<VSLAMConfig> stereoExampleTruthConfig() { boost::shared_ptr<Config> stereoExampleTruthConfig() {
// create initial estimates // create initial estimates
Rot3 faceDownY(Matrix_(3,3, Rot3 faceDownY(Matrix_(3,3,
1.0, 0.0, 0.0, 1.0, 0.0, 0.0,
@ -704,7 +701,7 @@ boost::shared_ptr<VSLAMConfig> stereoExampleTruthConfig() {
Point3 landmark2(1.0, 5.0, 0.0); Point3 landmark2(1.0, 5.0, 0.0);
// create config // create config
boost::shared_ptr<VSLAMConfig> truthConfig(new VSLAMConfig); boost::shared_ptr<Config> truthConfig(new Config);
truthConfig->insert(1, camera1.pose()); truthConfig->insert(1, camera1.pose());
truthConfig->insert(2, camera2.pose()); truthConfig->insert(2, camera2.pose());
truthConfig->insert(1, landmark1); truthConfig->insert(1, landmark1);
@ -721,11 +718,11 @@ TEST (SQP, stereo_sqp ) {
bool verbose = false; bool verbose = false;
// get a graph // get a graph
VSLAMGraph graph = stereoExampleGraph(); Graph graph = stereoExampleGraph();
if (verbose) graph.print("Graph after construction"); if (verbose) graph.print("Graph after construction");
// get the truth config // get the truth config
boost::shared_ptr<VSLAMConfig> truthConfig = stereoExampleTruthConfig(); boost::shared_ptr<Config> truthConfig = stereoExampleTruthConfig();
// create ordering // create ordering
Ordering ord; Ordering ord;
@ -749,7 +746,7 @@ TEST (SQP, stereo_sqp_noisy ) {
bool verbose = false; bool verbose = false;
// get a graph // get a graph
VSLAMGraph graph = stereoExampleGraph(); Graph graph = stereoExampleGraph();
// create initial data // create initial data
Rot3 faceDownY(Matrix_(3,3, Rot3 faceDownY(Matrix_(3,3,
@ -762,7 +759,7 @@ TEST (SQP, stereo_sqp_noisy ) {
Point3 landmark2(1.5, 5.0, 0.0); Point3 landmark2(1.5, 5.0, 0.0);
// noisy config // noisy config
boost::shared_ptr<VSLAMConfig> initConfig(new VSLAMConfig); boost::shared_ptr<Config> initConfig(new Config);
initConfig->insert(1, pose1); initConfig->insert(1, pose1);
initConfig->insert(2, pose2); initConfig->insert(2, pose2);
initConfig->insert(1, landmark1); initConfig->insert(1, landmark1);
@ -793,7 +790,7 @@ TEST (SQP, stereo_sqp_noisy ) {
<< "Final Error: " << optimizer.error() << endl; << "Final Error: " << optimizer.error() << endl;
// get the truth config // get the truth config
boost::shared_ptr<VSLAMConfig> truthConfig = stereoExampleTruthConfig(); boost::shared_ptr<Config> truthConfig = stereoExampleTruthConfig();
if (verbose) { if (verbose) {
initConfig->print("Initial Config"); initConfig->print("Initial Config");
@ -814,7 +811,7 @@ TEST (SQP, stereo_sqp_noisy_manualLagrange ) {
bool verbose = false; bool verbose = false;
// get a graph // get a graph
VSLAMGraph graph = stereoExampleGraph(); Graph graph = stereoExampleGraph();
// create initial data // create initial data
Rot3 faceDownY(Matrix_(3,3, Rot3 faceDownY(Matrix_(3,3,
@ -827,7 +824,7 @@ TEST (SQP, stereo_sqp_noisy_manualLagrange ) {
Point3 landmark2(1.5, 5.0, 0.0); Point3 landmark2(1.5, 5.0, 0.0);
// noisy config // noisy config
boost::shared_ptr<VSLAMConfig> initConfig(new VSLAMConfig); boost::shared_ptr<Config> initConfig(new Config);
initConfig->insert(1, pose1); initConfig->insert(1, pose1);
initConfig->insert(2, pose2); initConfig->insert(2, pose2);
initConfig->insert(1, landmark1); initConfig->insert(1, landmark1);
@ -863,7 +860,7 @@ TEST (SQP, stereo_sqp_noisy_manualLagrange ) {
<< "Final Error: " << optimizer.error() << endl; << "Final Error: " << optimizer.error() << endl;
// get the truth config // get the truth config
boost::shared_ptr<VSLAMConfig> truthConfig = stereoExampleTruthConfig(); boost::shared_ptr<Config> truthConfig = stereoExampleTruthConfig();
if (verbose) { if (verbose) {
initConfig->print("Initial Config"); initConfig->print("Initial Config");

4
cpp/testTupleConfig.cpp
View File

@ -10,8 +10,10 @@
#include <Pose2.h> #include <Pose2.h>
#include <Point2.h> #include <Point2.h>
#include "TupleConfig.h"
#include "Vector.h" #include "Vector.h"
#include "Key.h"
#include "VectorConfig.h"
#include "TupleConfig-inl.h"
using namespace gtsam; using namespace gtsam;
using namespace std; using namespace std;

13
cpp/testVSLAMConfig.cpp
View File

@ -1,25 +1,26 @@
/* /*
* @file testVSLAMConfig.cpp * @file testConfig.cpp
* @brief Tests for the Visual SLAM configuration class * @brief Tests for the Visual SLAM configuration class
* @author Alex Cunningham * @author Alex Cunningham
*/ */
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include "VectorConfig.h" #include "VectorConfig.h"
#include "VSLAMConfig.h" #include "visualSLAM.h"
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
using namespace gtsam::visualSLAM;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( VSLAMConfig, update_with_large_delta) { TEST( Config, update_with_large_delta) {
// this test ensures that if the update for delta is larger than // this test ensures that if the update for delta is larger than
// the size of the config, it only updates existing variables // the size of the config, it only updates existing variables
VSLAMConfig init; Config init;
init.insert(1, Pose3()); init.insert(1, Pose3());
init.insert(1, Point3(1.0, 2.0, 3.0)); init.insert(1, Point3(1.0, 2.0, 3.0));
VSLAMConfig expected; Config expected;
expected.insert(1, Pose3(Rot3(), Point3(0.1, 0.1, 0.1))); expected.insert(1, Pose3(Rot3(), Point3(0.1, 0.1, 0.1)));
expected.insert(1, Point3(1.1, 2.1, 3.1)); expected.insert(1, Point3(1.1, 2.1, 3.1));
@ -27,7 +28,7 @@ TEST( VSLAMConfig, update_with_large_delta) {
delta.insert("x1", Vector_(6, 0.0, 0.0, 0.0, 0.1, 0.1, 0.1)); 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("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)); delta.insert("x2", Vector_(6, 0.0, 0.0, 0.0, 100.1, 4.1, 9.1));
VSLAMConfig actual = expmap(init, delta); Config actual = expmap(init, delta);
CHECK(assert_equal(expected,actual)); CHECK(assert_equal(expected,actual));
} }

32
cpp/testVSLAMFactor.cpp
View File

@ -4,14 +4,13 @@
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include "VSLAMConfig.h" #include "visualSLAM.h"
#include "VSLAMFactor.h"
#include "VSLAMGraph.h"
#include "Point3.h" #include "Point3.h"
#include "Pose3.h" #include "Pose3.h"
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
using namespace gtsam::visualSLAM;
// make cube // make cube
Point3 x000(-1, -1, -1), x001(-1, -1, +1), x010(-1, +1, -1), x011(-1, +1, +1), Point3 x000(-1, -1, -1), x001(-1, -1, +1), x010(-1, +1, -1), x011(-1, +1, +1),
@ -25,20 +24,21 @@ Cal3_S2 K(fov,w,h);
// make cameras // make cameras
/* ************************************************************************* */ /* ************************************************************************* */
TEST( VSLAMFactor, error ) TEST( ProjectionFactor, error )
{ {
// Create the factor with a measurement that is 3 pixels off in x // Create the factor with a measurement that is 3 pixels off in x
Point2 z(323.,240.); Point2 z(323.,240.);
double sigma=1.0; double sigma=1.0;
int cameraFrameNumber=1, landmarkNumber=1; int cameraFrameNumber=1, landmarkNumber=1;
boost::shared_ptr<VSLAMFactor> boost::shared_ptr<ProjectionFactor>
factor(new VSLAMFactor(z, sigma, cameraFrameNumber, landmarkNumber, shared_ptrK(new Cal3_S2(K)))); factor(new ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, shared_ptrK(new Cal3_S2(K))));
// For the following configuration, the factor predicts 320,240 // For the following configuration, the factor predicts 320,240
VSLAMConfig config; Config config;
Rot3 R;Point3 t1(0,0,-6); Pose3 x1(R,t1); config.insert(1, x1); Rot3 R;Point3 t1(0,0,-6); Pose3 x1(R,t1); config.insert(1, x1);
Point3 l1; config.insert(1, l1); Point3 l1; config.insert(1, l1);
CHECK(assert_equal(Point2(320.,240.),factor->predict(x1,l1))); // Point should project to Point2(320.,240.)
CHECK(assert_equal(Vector_(2, -3.0, 0.0), factor->error_vector(config)));
// Which yields an error of 3^2/2 = 4.5 // Which yields an error of 3^2/2 = 4.5
DOUBLES_EQUAL(4.5,factor->error(config),1e-9); DOUBLES_EQUAL(4.5,factor->error(config),1e-9);
@ -52,7 +52,7 @@ TEST( VSLAMFactor, error )
CHECK(assert_equal(expected,*actual,1e-3)); CHECK(assert_equal(expected,*actual,1e-3));
// linearize graph // linearize graph
VSLAMGraph graph; Graph graph;
graph.push_back(factor); graph.push_back(factor);
GaussianFactorGraph expected_lfg; GaussianFactorGraph expected_lfg;
expected_lfg.push_back(actual); expected_lfg.push_back(actual);
@ -63,24 +63,24 @@ TEST( VSLAMFactor, error )
VectorConfig delta; VectorConfig delta;
delta.insert("x1",Vector_(6, 0.,0.,0., 1.,1.,1.)); delta.insert("x1",Vector_(6, 0.,0.,0., 1.,1.,1.));
delta.insert("l1",Vector_(3, 1.,2.,3.)); delta.insert("l1",Vector_(3, 1.,2.,3.));
VSLAMConfig actual_config = expmap(config, delta); Config actual_config = expmap(config, delta);
VSLAMConfig expected_config; Config expected_config;
Point3 t2(1,1,-5); Pose3 x2(R,t2); expected_config.insert(1, x2); Point3 t2(1,1,-5); Pose3 x2(R,t2); expected_config.insert(1, x2);
Point3 l2(1,2,3); expected_config.insert(1, l2); Point3 l2(1,2,3); expected_config.insert(1, l2);
CHECK(assert_equal(expected_config,actual_config,1e-9)); CHECK(assert_equal(expected_config,actual_config,1e-9));
} }
TEST( VSLAMFactor, equals ) TEST( ProjectionFactor, equals )
{ {
// Create two identical factors and make sure they're equal // Create two identical factors and make sure they're equal
Vector z = Vector_(2,323.,240.); Vector z = Vector_(2,323.,240.);
double sigma=1.0; double sigma=1.0;
int cameraFrameNumber=1, landmarkNumber=1; int cameraFrameNumber=1, landmarkNumber=1;
boost::shared_ptr<VSLAMFactor> boost::shared_ptr<ProjectionFactor>
factor1(new VSLAMFactor(z, sigma, cameraFrameNumber, landmarkNumber, shared_ptrK(new Cal3_S2(K)))); factor1(new ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, shared_ptrK(new Cal3_S2(K))));
boost::shared_ptr<VSLAMFactor> boost::shared_ptr<ProjectionFactor>
factor2(new VSLAMFactor(z, sigma, cameraFrameNumber, landmarkNumber, shared_ptrK(new Cal3_S2(K)))); factor2(new ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, shared_ptrK(new Cal3_S2(K))));
CHECK(assert_equal(*factor1, *factor2)); CHECK(assert_equal(*factor1, *factor2));
} }

61
cpp/testVSLAMGraph.cpp
View File

@ -1,5 +1,5 @@
/** /**
* @file testVSLAMGraph.cpp * @file testGraph.cpp
* @brief Unit test for two cameras and four landmarks * @brief Unit test for two cameras and four landmarks
* single camera * single camera
* @author Chris Beall * @author Chris Beall
@ -11,13 +11,16 @@
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
using namespace boost; using namespace boost;
#include "VSLAMGraph.h"
#include "NonlinearFactorGraph-inl.h" #include "NonlinearFactorGraph-inl.h"
#include "NonlinearOptimizer-inl.h" #include "NonlinearOptimizer-inl.h"
#include "Ordering-inl.h"
#include "visualSLAM.h"
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
typedef NonlinearOptimizer<VSLAMGraph,VSLAMConfig> Optimizer; using namespace gtsam::visualSLAM;
using namespace boost;
typedef NonlinearOptimizer<Graph,Config> Optimizer;
/* ************************************************************************* */ /* ************************************************************************* */
Point3 landmark1(-1.0,-1.0, 0.0); Point3 landmark1(-1.0,-1.0, 0.0);
@ -40,7 +43,7 @@ Pose3 camera2(Matrix_(3,3,
Point3(0,0,5.00)); Point3(0,0,5.00));
/* ************************************************************************* */ /* ************************************************************************* */
VSLAMGraph testGraph() { Graph testGraph() {
Point2 z11(-100, 100); Point2 z11(-100, 100);
Point2 z12(-100,-100); Point2 z12(-100,-100);
Point2 z13( 100,-100); Point2 z13( 100,-100);
@ -52,30 +55,30 @@ VSLAMGraph testGraph() {
double sigma = 1; double sigma = 1;
shared_ptrK sK(new Cal3_S2(625, 625, 0, 0, 0)); shared_ptrK sK(new Cal3_S2(625, 625, 0, 0, 0));
VSLAMGraph g; Graph g;
g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z11, sigma, 1, 1, sK))); g.add(ProjectionFactor(z11, sigma, 1, 1, sK));
g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z12, sigma, 1, 2, sK))); g.add(ProjectionFactor(z12, sigma, 1, 2, sK));
g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z13, sigma, 1, 3, sK))); g.add(ProjectionFactor(z13, sigma, 1, 3, sK));
g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z14, sigma, 1, 4, sK))); g.add(ProjectionFactor(z14, sigma, 1, 4, sK));
g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z21, sigma, 2, 1, sK))); g.add(ProjectionFactor(z21, sigma, 2, 1, sK));
g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z22, sigma, 2, 2, sK))); g.add(ProjectionFactor(z22, sigma, 2, 2, sK));
g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z23, sigma, 2, 3, sK))); g.add(ProjectionFactor(z23, sigma, 2, 3, sK));
g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z24, sigma, 2, 4, sK))); g.add(ProjectionFactor(z24, sigma, 2, 4, sK));
return g; return g;
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( VSLAMGraph, optimizeLM) TEST( Graph, optimizeLM)
{ {
// build a graph // build a graph
shared_ptr<VSLAMGraph> graph(new VSLAMGraph(testGraph())); shared_ptr<Graph> graph(new Graph(testGraph()));
// add 3 landmark constraints // add 3 landmark constraints
graph->addLandmarkConstraint(1, landmark1); graph->add(PointConstraint(1, landmark1));
graph->addLandmarkConstraint(2, landmark2); graph->add(PointConstraint(2, landmark2));
graph->addLandmarkConstraint(3, landmark3); graph->add(PointConstraint(3, landmark3));
// Create an initial configuration corresponding to the ground truth // Create an initial configuration corresponding to the ground truth
boost::shared_ptr<VSLAMConfig> initialEstimate(new VSLAMConfig); boost::shared_ptr<Config> initialEstimate(new Config);
initialEstimate->insert(1, camera1); initialEstimate->insert(1, camera1);
initialEstimate->insert(2, camera2); initialEstimate->insert(2, camera2);
initialEstimate->insert(1, landmark1); initialEstimate->insert(1, landmark1);
@ -109,16 +112,16 @@ TEST( VSLAMGraph, optimizeLM)
/* ************************************************************************* */ /* ************************************************************************* */
TEST( VSLAMGraph, optimizeLM2) TEST( Graph, optimizeLM2)
{ {
// build a graph // build a graph
shared_ptr<VSLAMGraph> graph(new VSLAMGraph(testGraph())); shared_ptr<Graph> graph(new Graph(testGraph()));
// add 2 camera constraints // add 2 camera constraints
graph->addCameraConstraint(1, camera1); graph->add(PoseConstraint(1, camera1));
graph->addCameraConstraint(2, camera2); graph->add(PoseConstraint(2, camera2));
// Create an initial configuration corresponding to the ground truth // Create an initial configuration corresponding to the ground truth
boost::shared_ptr<VSLAMConfig> initialEstimate(new VSLAMConfig); boost::shared_ptr<Config> initialEstimate(new Config);
initialEstimate->insert(1, camera1); initialEstimate->insert(1, camera1);
initialEstimate->insert(2, camera2); initialEstimate->insert(2, camera2);
initialEstimate->insert(1, landmark1); initialEstimate->insert(1, landmark1);
@ -153,16 +156,16 @@ TEST( VSLAMGraph, optimizeLM2)
/* ************************************************************************* */ /* ************************************************************************* */
TEST( VSLAMGraph, CHECK_ORDERING) TEST( Graph, CHECK_ORDERING)
{ {
// build a graph // build a graph
shared_ptr<VSLAMGraph> graph(new VSLAMGraph(testGraph())); shared_ptr<Graph> graph(new Graph(testGraph()));
// add 2 camera constraints // add 2 camera constraints
graph->addCameraConstraint(1, camera1); graph->add(PoseConstraint(1, camera1));
graph->addCameraConstraint(2, camera2); graph->add(PoseConstraint(2, camera2));
// Create an initial configuration corresponding to the ground truth // Create an initial configuration corresponding to the ground truth
boost::shared_ptr<VSLAMConfig> initialEstimate(new VSLAMConfig); boost::shared_ptr<Config> initialEstimate(new Config);
initialEstimate->insert(1, camera1); initialEstimate->insert(1, camera1);
initialEstimate->insert(2, camera2); initialEstimate->insert(2, camera2);
initialEstimate->insert(1, landmark1); initialEstimate->insert(1, landmark1);

63
cpp/visualSLAM.cpp Normal file
View File

@ -0,0 +1,63 @@
/*
* visualSLAM.cpp
*
* Created on: Jan 14, 2010
* Author: richard
*/
#include "visualSLAM.h"
#include "TupleConfig-inl.h"
#include "NonlinearOptimizer-inl.h"
#include "NonlinearFactorGraph-inl.h"
namespace gtsam {
INSTANTIATE_PAIR_CONFIG(visualSLAM::PoseKey, Pose3, visualSLAM::PointKey, Point3)
INSTANTIATE_NONLINEAR_FACTOR_GRAPH(visualSLAM::Config)
INSTANTIATE_NONLINEAR_OPTIMIZER(visualSLAM::Graph, visualSLAM::Config)
namespace visualSLAM {
/* ************************************************************************* */
void ProjectionFactor::print(const std::string& s) const {
Base::print(s);
z_.print(s + ".z");
}
/* ************************************************************************* */
bool ProjectionFactor::equals(const ProjectionFactor& p, double tol) const {
return Base::equals(p, tol) && z_.equals(p.z_, tol)
&& K_->equals(*p.K_, tol);
}
// /* ************************************************************************* */
// bool compareLandmark(const std::string& key,
// const VSLAMConfig& feasible,
// const VSLAMConfig& input) {
// int j = atoi(key.substr(1, key.size() - 1).c_str());
// return feasible[VSLAMPointKey(j)].equals(input[VSLAMPointKey(j)]);
// }
//
// /* ************************************************************************* */
// void VSLAMGraph::addLandmarkConstraint(int j, const Point3& p) {
// typedef NonlinearEquality<VSLAMConfig,VSLAMPointKey,Point3> NLE;
// boost::shared_ptr<NLE> factor(new NLE(j, p));
// push_back(factor);
// }
//
// /* ************************************************************************* */
// bool compareCamera(const std::string& key,
// const VSLAMConfig& feasible,
// const VSLAMConfig& input) {
// int j = atoi(key.substr(1, key.size() - 1).c_str());
// return feasible[VSLAMPoseKey(j)].equals(input[VSLAMPoseKey(j)]);
// }
//
// /* ************************************************************************* */
// void VSLAMGraph::addCameraConstraint(int j, const Pose3& p) {
// typedef NonlinearEquality<VSLAMConfig,VSLAMPoseKey,Pose3> NLE;
// boost::shared_ptr<NLE> factor(new NLE(j,p));
// push_back(factor);
// }
}
}

152
cpp/visualSLAM.h Normal file
View File

@ -0,0 +1,152 @@
/*
* visualSLAM.h
*
* Created on: Jan 14, 2010
* Author: Richard Roberts and Chris Beall
*/
#pragma once
#include "Key.h"
#include "Pose3.h"
#include "Point3.h"
#include "NonlinearFactorGraph.h"
#include "Cal3_S2.h"
#include "Point2.h"
#include "SimpleCamera.h"
#include "TupleConfig.h"
#include "NonlinearEquality.h"
namespace gtsam { namespace visualSLAM {
/**
* Typedefs that make up the visualSLAM namespace.
*/
typedef Symbol<Pose3,'x'> PoseKey;
typedef Symbol<Point3,'l'> PointKey;
typedef PairConfig<PoseKey, Pose3, PointKey, Point3> Config;
typedef NonlinearFactorGraph<Config> Graph;
typedef NonlinearEquality<Config, PoseKey, Pose3> PoseConstraint;
typedef NonlinearEquality<Config, PointKey, Point3> PointConstraint;
/**
* Non-linear factor for a constraint derived from a 2D measurement,
* i.e. the main building block for visual SLAM.
*/
class ProjectionFactor: public NonlinearFactor2<Config, PoseKey, Pose3, PointKey, Point3>, Testable<ProjectionFactor> {
private:
// Keep a copy of measurement and calibration for I/O
Point2 z_;
boost::shared_ptr<Cal3_S2> K_;
public:
// shorthand for base class type
typedef NonlinearFactor2<Config, PoseKey, Pose3, PointKey, Point3> Base;
// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<ProjectionFactor> shared_ptr;
/**
* Default constructor
*/
ProjectionFactor() : K_(new Cal3_S2(444, 555, 666, 777, 888)) {}
/**
* 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
*/
ProjectionFactor(const Point2& z, double sigma, PoseKey j_pose, PointKey j_landmark, const shared_ptrK& K) :
z_(z), K_(K), Base(sigma, j_pose, j_landmark) {}
/**
* print
* @param s optional string naming the factor
*/
void print(const std::string& s = "ProjectionFactor") const;
/**
* equals
*/
bool equals(const ProjectionFactor&, double tol = 1e-9) const;
// /** h(x) */
// Point2 predict(const Pose3& pose, const Point3& point) const {
// return SimpleCamera(*K_, pose).project(point);
// }
/** 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_);
}
};
// /**
// * Non-linear factor graph for visual SLAM
// */
// class VSLAMGraph : public NonlinearFactorGraph<VSLAMConfig>{
//
// public:
//
// /** default constructor is empty graph */
// VSLAMGraph() {}
//
// /**
// * print out graph
// */
// void print(const std::string& s = "") const {
// NonlinearFactorGraph<VSLAMConfig>::print(s);
// }
//
// /**
// * equals
// */
// bool equals(const VSLAMGraph& p, double tol=1e-9) const {
// return NonlinearFactorGraph<VSLAMConfig>::equals(p, tol);
// }
//
// /**
// * Add a constraint on a landmark (for now, *must* be satisfied in any Config)
// * @param j index of landmark
// * @param p to which point to constrain it to
// */
// void addLandmarkConstraint(int j, const Point3& p = Point3());
//
// /**
// * Add a constraint on a camera (for now, *must* be satisfied in any Config)
// * @param j index of camera
// * @param p to which pose to constrain it to
// */
// void addCameraConstraint(int j, const Pose3& p = Pose3());
//
// private:
// /** Serialization function */
// friend class boost::serialization::access;
// template<class Archive>
// void serialize(Archive & ar, const unsigned int version) {}
// };
} } // namespaces