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

testSimmulated2D passes. Too many warnings in boost from clang!

release/4.3a0
Duy-Nguyen Ta 2012-01-29 21:12:58 +00:00
parent d3d5ee3b39
commit 5b5bbfdfff
21 changed files with 120 additions and 145 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/nonlinear/DoglegOptimizerImpl.h
View File

@ -7,7 +7,7 @@
#include <gtsam/linear/GaussianBayesNet.h> #include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianISAM.h> // To get optimize(BayesTree<GaussianConditional>) #include <gtsam/linear/GaussianISAM.h> // To get optimize(BayesTree<GaussianConditional>)
#include <gtsam/nonlinear/NonlinearFactorGraph.h> //#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Ordering.h> #include <gtsam/nonlinear/Ordering.h>
namespace gtsam { namespace gtsam {

4
gtsam/nonlinear/Makefile.am
View File

@ -22,10 +22,10 @@ check_PROGRAMS += tests/testValues tests/testDynamicValues tests/testKey tests/t
# Nonlinear nonlinear # Nonlinear nonlinear
headers += Key.h headers += Key.h
headers += NonlinearFactorGraph.h NonlinearFactorGraph-inl.h headers += NonlinearFactorGraph.h
headers += NonlinearOptimizer-inl.h NonlinearOptimization.h NonlinearOptimization-inl.h NonlinearOptimizationParameters.h headers += NonlinearOptimizer-inl.h NonlinearOptimization.h NonlinearOptimization-inl.h NonlinearOptimizationParameters.h
headers += NonlinearFactor.h headers += NonlinearFactor.h
sources += NonlinearOptimizer.cpp Ordering.cpp DoglegOptimizerImpl.cpp sources += NonlinearFactorGraph.cpp NonlinearOptimizer.cpp Ordering.cpp DoglegOptimizerImpl.cpp
headers += DoglegOptimizer.h DoglegOptimizer-inl.h headers += DoglegOptimizer.h DoglegOptimizer-inl.h
# Nonlinear iSAM(2) # Nonlinear iSAM(2)

9
gtsam/nonlinear/NonlinearFactorGraph-inl.h → gtsam/nonlinear/NonlinearFactorGraph.cpp
View File

@ -10,19 +10,18 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file NonlinearFactorGraph-inl.h * @file NonlinearFactorGraph.cpp
* @brief Factor Graph Consisting of non-linear factors * @brief Factor Graph Consisting of non-linear factors
* @author Frank Dellaert * @author Frank Dellaert
* @author Carlos Nieto * @author Carlos Nieto
* @author Christian Potthast * @author Christian Potthast
*/ */
#pragma once #include <cmath>
#include <boost/foreach.hpp>
#include <gtsam/inference/FactorGraph.h> #include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/inference.h> #include <gtsam/inference/inference.h>
#include <boost/foreach.hpp> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <cmath>
using namespace std; using namespace std;

1
gtsam/nonlinear/NonlinearFactorGraph.h
View File

@ -100,4 +100,3 @@ namespace gtsam {
} // namespace } // namespace
#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>

48
gtsam/nonlinear/Values-inl.h → gtsam/nonlinear/ValuesOld-inl.h
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file Values.cpp * @file ValuesOld.cpp
* @author Richard Roberts * @author Richard Roberts
*/ */
@ -34,8 +34,8 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class J> template<class J>
void Values<J>::print(const string &s) const { void ValuesOld<J>::print(const string &s) const {
cout << "Values " << s << ", size " << values_.size() << "\n"; cout << "ValuesOld " << s << ", size " << values_.size() << "\n";
BOOST_FOREACH(const KeyValuePair& v, values_) { BOOST_FOREACH(const KeyValuePair& v, values_) {
gtsam::print(v.second, (string)(v.first)); gtsam::print(v.second, (string)(v.first));
} }
@ -43,7 +43,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class J> template<class J>
bool Values<J>::equals(const Values<J>& expected, double tol) const { bool ValuesOld<J>::equals(const ValuesOld<J>& expected, double tol) const {
if (values_.size() != expected.values_.size()) return false; if (values_.size() != expected.values_.size()) return false;
BOOST_FOREACH(const KeyValuePair& v, values_) { BOOST_FOREACH(const KeyValuePair& v, values_) {
if (!expected.exists(v.first)) return false; if (!expected.exists(v.first)) return false;
@ -55,7 +55,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class J> template<class J>
const typename J::Value& Values<J>::at(const J& j) const { const typename J::Value& ValuesOld<J>::at(const J& j) const {
const_iterator it = values_.find(j); const_iterator it = values_.find(j);
if (it == values_.end()) throw KeyDoesNotExist<J>("retrieve", j); if (it == values_.end()) throw KeyDoesNotExist<J>("retrieve", j);
else return it->second; else return it->second;
@ -63,7 +63,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class J> template<class J>
size_t Values<J>::dim() const { size_t ValuesOld<J>::dim() const {
size_t n = 0; size_t n = 0;
BOOST_FOREACH(const KeyValuePair& value, values_) BOOST_FOREACH(const KeyValuePair& value, values_)
n += value.second.dim(); n += value.second.dim();
@ -72,27 +72,27 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class J> template<class J>
VectorValues Values<J>::zero(const Ordering& ordering) const { VectorValues ValuesOld<J>::zero(const Ordering& ordering) const {
return VectorValues::Zero(this->dims(ordering)); return VectorValues::Zero(this->dims(ordering));
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class J> template<class J>
void Values<J>::insert(const J& name, const typename J::Value& val) { void ValuesOld<J>::insert(const J& name, const typename J::Value& val) {
if(!values_.insert(make_pair(name, val)).second) if(!values_.insert(make_pair(name, val)).second)
throw KeyAlreadyExists<J>(name, val); throw KeyAlreadyExists<J>(name, val);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class J> template<class J>
void Values<J>::insert(const Values<J>& cfg) { void ValuesOld<J>::insert(const ValuesOld<J>& cfg) {
BOOST_FOREACH(const KeyValuePair& v, cfg.values_) BOOST_FOREACH(const KeyValuePair& v, cfg.values_)
insert(v.first, v.second); insert(v.first, v.second);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class J> template<class J>
void Values<J>::update(const Values<J>& cfg) { void ValuesOld<J>::update(const ValuesOld<J>& cfg) {
BOOST_FOREACH(const KeyValuePair& v, values_) { BOOST_FOREACH(const KeyValuePair& v, values_) {
boost::optional<typename J::Value> t = cfg.exists_(v.first); boost::optional<typename J::Value> t = cfg.exists_(v.first);
if (t) values_[v.first] = *t; if (t) values_[v.first] = *t;
@ -101,13 +101,13 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class J> template<class J>
void Values<J>::update(const J& j, const typename J::Value& val) { void ValuesOld<J>::update(const J& j, const typename J::Value& val) {
values_[j] = val; values_[j] = val;
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class J> template<class J>
std::list<J> Values<J>::keys() const { std::list<J> ValuesOld<J>::keys() const {
std::list<J> ret; std::list<J> ret;
BOOST_FOREACH(const KeyValuePair& v, values_) BOOST_FOREACH(const KeyValuePair& v, values_)
ret.push_back(v.first); ret.push_back(v.first);
@ -116,14 +116,14 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class J> template<class J>
void Values<J>::erase(const J& j) { void ValuesOld<J>::erase(const J& j) {
size_t dim; // unused size_t dim; // unused
erase(j, dim); erase(j, dim);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class J> template<class J>
void Values<J>::erase(const J& j, size_t& dim) { void ValuesOld<J>::erase(const J& j, size_t& dim) {
iterator it = values_.find(j); iterator it = values_.find(j);
if (it == values_.end()) if (it == values_.end())
throw KeyDoesNotExist<J>("erase", j); throw KeyDoesNotExist<J>("erase", j);
@ -134,8 +134,8 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
// todo: insert for every element is inefficient // todo: insert for every element is inefficient
template<class J> template<class J>
Values<J> Values<J>::retract(const VectorValues& delta, const Ordering& ordering) const { ValuesOld<J> ValuesOld<J>::retract(const VectorValues& delta, const Ordering& ordering) const {
Values<J> newValues; ValuesOld<J> newValues;
typedef pair<J,typename J::Value> KeyValue; typedef pair<J,typename J::Value> KeyValue;
BOOST_FOREACH(const KeyValue& value, this->values_) { BOOST_FOREACH(const KeyValue& value, this->values_) {
const J& j = value.first; const J& j = value.first;
@ -151,7 +151,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class J> template<class J>
std::vector<size_t> Values<J>::dims(const Ordering& ordering) const { std::vector<size_t> ValuesOld<J>::dims(const Ordering& ordering) const {
_ValuesDimensionCollector dimCollector(ordering); _ValuesDimensionCollector dimCollector(ordering);
this->apply(dimCollector); this->apply(dimCollector);
return dimCollector.dimensions; return dimCollector.dimensions;
@ -159,7 +159,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class J> template<class J>
Ordering::shared_ptr Values<J>::orderingArbitrary(Index firstVar) const { Ordering::shared_ptr ValuesOld<J>::orderingArbitrary(Index firstVar) const {
// Generate an initial key ordering in iterator order // Generate an initial key ordering in iterator order
_ValuesKeyOrderer keyOrderer(firstVar); _ValuesKeyOrderer keyOrderer(firstVar);
this->apply(keyOrderer); this->apply(keyOrderer);
@ -169,12 +169,12 @@ namespace gtsam {
// /* ************************************************************************* */ // /* ************************************************************************* */
// // todo: insert for every element is inefficient // // todo: insert for every element is inefficient
// template<class J> // template<class J>
// Values<J> Values<J>::retract(const Vector& delta) const { // ValuesOld<J> ValuesOld<J>::retract(const Vector& delta) const {
// if(delta.size() != dim()) { // if(delta.size() != dim()) {
// cout << "Values::dim (" << dim() << ") <> delta.size (" << delta.size() << ")" << endl; // cout << "ValuesOld::dim (" << dim() << ") <> delta.size (" << delta.size() << ")" << endl;
// throw invalid_argument("Delta vector length does not match config dimensionality."); // throw invalid_argument("Delta vector length does not match config dimensionality.");
// } // }
// Values<J> newValues; // ValuesOld<J> newValues;
// int delta_offset = 0; // int delta_offset = 0;
// typedef pair<J,typename J::Value> KeyValue; // typedef pair<J,typename J::Value> KeyValue;
// BOOST_FOREACH(const KeyValue& value, this->values_) { // BOOST_FOREACH(const KeyValue& value, this->values_) {
@ -191,7 +191,7 @@ namespace gtsam {
// todo: insert for every element is inefficient // todo: insert for every element is inefficient
// todo: currently only logmaps elements in both configs // todo: currently only logmaps elements in both configs
template<class J> template<class J>
inline VectorValues Values<J>::localCoordinates(const Values<J>& cp, const Ordering& ordering) const { inline VectorValues ValuesOld<J>::localCoordinates(const ValuesOld<J>& cp, const Ordering& ordering) const {
VectorValues delta(this->dims(ordering)); VectorValues delta(this->dims(ordering));
localCoordinates(cp, ordering, delta); localCoordinates(cp, ordering, delta);
return delta; return delta;
@ -199,7 +199,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class J> template<class J>
void Values<J>::localCoordinates(const Values<J>& cp, const Ordering& ordering, VectorValues& delta) const { void ValuesOld<J>::localCoordinates(const ValuesOld<J>& cp, const Ordering& ordering, VectorValues& delta) const {
typedef pair<J,typename J::Value> KeyValue; typedef pair<J,typename J::Value> KeyValue;
BOOST_FOREACH(const KeyValue& value, cp) { BOOST_FOREACH(const KeyValue& value, cp) {
assert(this->exists(value.first)); assert(this->exists(value.first));
@ -213,7 +213,7 @@ namespace gtsam {
if(message_.empty()) if(message_.empty())
message_ = message_ =
"Attempting to " + std::string(operation_) + " the key \"" + "Attempting to " + std::string(operation_) + " the key \"" +
(std::string)key_ + "\", which does not exist in the Values."; (std::string)key_ + "\", which does not exist in the ValuesOld.";
return message_.c_str(); return message_.c_str();
} }

32
gtsam/nonlinear/Values.h → gtsam/nonlinear/ValuesOld.h
View File

@ -10,14 +10,14 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file Values.h * @file ValuesOld.h
* @author Richard Roberts * @author Richard Roberts
* *
* @brief A templated config for Manifold-group elements * @brief A templated config for Manifold-group elements
* *
* Detailed story: * Detailed story:
* A values structure is a map from keys to values. It is used to specify the value of a bunch * A values structure is a map from keys to values. It is used to specify the value of a bunch
* of variables in a factor graph. A Values is a values structure which can hold variables that * of variables in a factor graph. A ValuesOld is a values structure which can hold variables that
* are elements on manifolds, not just vectors. It then, as a whole, implements a aggregate type * are elements on manifolds, not just vectors. It then, as a whole, implements a aggregate type
* which is also a manifold element, and hence supports operations dim, retract, and localCoordinates. * which is also a manifold element, and hence supports operations dim, retract, and localCoordinates.
*/ */
@ -52,7 +52,7 @@ namespace gtsam {
* labels (example: Pose2, Point2, etc) * labels (example: Pose2, Point2, etc)
*/ */
template<class J> template<class J>
class Values { class ValuesOld {
public: public:
@ -77,12 +77,12 @@ namespace gtsam {
public: public:
Values() {} ValuesOld() {}
Values(const Values& config) : ValuesOld(const ValuesOld& config) :
values_(config.values_) {} values_(config.values_) {}
template<class J_ALT> template<class J_ALT>
Values(const Values<J_ALT>& other) {} // do nothing when initializing with wrong type ValuesOld(const ValuesOld<J_ALT>& other) {} // do nothing when initializing with wrong type
virtual ~Values() {} virtual ~ValuesOld() {}
/// @name Testable /// @name Testable
/// @{ /// @{
@ -91,7 +91,7 @@ namespace gtsam {
void print(const std::string &s="") const; void print(const std::string &s="") const;
/** Test whether configs are identical in keys and values */ /** Test whether configs are identical in keys and values */
bool equals(const Values& expected, double tol=1e-9) const; bool equals(const ValuesOld& expected, double tol=1e-9) const;
/// @} /// @}
@ -132,13 +132,13 @@ namespace gtsam {
size_t dim() const; size_t dim() const;
/** Add a delta config to current config and returns a new config */ /** Add a delta config to current config and returns a new config */
Values retract(const VectorValues& delta, const Ordering& ordering) const; ValuesOld retract(const VectorValues& delta, const Ordering& ordering) const;
/** Get a delta config about a linearization point c0 (*this) */ /** Get a delta config about a linearization point c0 (*this) */
VectorValues localCoordinates(const Values& cp, const Ordering& ordering) const; VectorValues localCoordinates(const ValuesOld& cp, const Ordering& ordering) const;
/** Get a delta config about a linearization point c0 (*this) */ /** Get a delta config about a linearization point c0 (*this) */
void localCoordinates(const Values& cp, const Ordering& ordering, VectorValues& delta) const; void localCoordinates(const ValuesOld& cp, const Ordering& ordering, VectorValues& delta) const;
/// @} /// @}
@ -148,10 +148,10 @@ namespace gtsam {
void insert(const J& j, const Value& val); void insert(const J& j, const Value& val);
/** Add a set of variables, throws KeyAlreadyExists<J> if a key is already present */ /** Add a set of variables, throws KeyAlreadyExists<J> if a key is already present */
void insert(const Values& cfg); void insert(const ValuesOld& cfg);
/** update the current available values without adding new ones */ /** update the current available values without adding new ones */
void update(const Values& cfg); void update(const ValuesOld& cfg);
/** single element change of existing element */ /** single element change of existing element */
void update(const J& j, const Value& val); void update(const J& j, const Value& val);
@ -172,7 +172,7 @@ namespace gtsam {
std::list<J> keys() const; std::list<J> keys() const;
/** Replace all keys and variables */ /** Replace all keys and variables */
Values& operator=(const Values& rhs) { ValuesOld& operator=(const ValuesOld& rhs) {
values_ = rhs.values_; values_ = rhs.values_;
return (*this); return (*this);
} }
@ -185,7 +185,7 @@ namespace gtsam {
/** /**
* Apply a class with an application operator() to a const_iterator over * Apply a class with an application operator() to a const_iterator over
* every <key,value> pair. The operator must be able to handle such an * every <key,value> pair. The operator must be able to handle such an
* iterator for every type in the Values, (i.e. through templating). * iterator for every type in the ValuesOld, (i.e. through templating).
*/ */
template<typename A> template<typename A>
void apply(A& operation) { void apply(A& operation) {
@ -297,5 +297,5 @@ public:
} // \namespace gtsam } // \namespace gtsam
#include <gtsam/nonlinear/Values-inl.h> #include <gtsam/nonlinear/ValuesOld-inl.h>

8
gtsam/slam/PartialPriorFactor.h
View File

@ -38,16 +38,16 @@ namespace gtsam {
* For practical use, it would be good to subclass this factor and have the class type * For practical use, it would be good to subclass this factor and have the class type
* construct the mask. * construct the mask.
*/ */
template<class VALUES, class KEY> template<class KEY>
class PartialPriorFactor: public NonlinearFactor1<VALUES, KEY> { class PartialPriorFactor: public NonlinearFactor1<KEY> {
public: public:
typedef typename KEY::Value T; typedef typename KEY::Value T;
protected: protected:
typedef NonlinearFactor1<VALUES, KEY> Base; typedef NonlinearFactor1<KEY> Base;
typedef PartialPriorFactor<VALUES, KEY> This; typedef PartialPriorFactor<KEY> This;
Vector prior_; ///< measurement on logmap parameters, in compressed form Vector prior_; ///< measurement on logmap parameters, in compressed form
std::vector<size_t> mask_; ///< indices of values to constrain in compressed prior vector std::vector<size_t> mask_; ///< indices of values to constrain in compressed prior vector

10
gtsam/slam/ProjectionFactor.h
View File

@ -29,8 +29,8 @@ namespace gtsam {
* Non-linear factor for a constraint derived from a 2D measurement. The calibration is known here. * Non-linear factor for a constraint derived from a 2D measurement. The calibration is known here.
* i.e. the main building block for visual SLAM. * i.e. the main building block for visual SLAM.
*/ */
template<class VALUES, class LMK, class POSK> template<class LMK, class POSK>
class GenericProjectionFactor: public NonlinearFactor2<VALUES, POSK, LMK> { class GenericProjectionFactor: public NonlinearFactor2<POSK, LMK> {
protected: protected:
// Keep a copy of measurement and calibration for I/O // Keep a copy of measurement and calibration for I/O
@ -40,10 +40,10 @@ namespace gtsam {
public: public:
/// shorthand for base class type /// shorthand for base class type
typedef NonlinearFactor2<VALUES, POSK, LMK> Base; typedef NonlinearFactor2<POSK, LMK> Base;
/// shorthand for a smart pointer to a factor /// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<GenericProjectionFactor<VALUES, LMK, POSK> > shared_ptr; typedef boost::shared_ptr<GenericProjectionFactor<LMK, POSK> > shared_ptr;
/// Default constructor /// Default constructor
GenericProjectionFactor() : GenericProjectionFactor() :
@ -73,7 +73,7 @@ namespace gtsam {
} }
/// equals /// equals
bool equals(const GenericProjectionFactor<VALUES, LMK, POSK>& p bool equals(const GenericProjectionFactor<LMK, POSK>& p
, double tol = 1e-9) const { , double tol = 1e-9) const {
return Base::equals(p, tol) && this->z_.equals(p.z_, tol) return Base::equals(p, tol) && this->z_.equals(p.z_, tol)
&& this->K_->equals(*p.K_, tol); && this->K_->equals(*p.K_, tol);

10
gtsam/slam/RangeFactor.h
View File

@ -25,14 +25,14 @@ namespace gtsam {
/** /**
* Binary factor for a range measurement * Binary factor for a range measurement
*/ */
template<class VALUES, class POSEKEY, class POINTKEY> template<class POSEKEY, class POINTKEY>
class RangeFactor: public NonlinearFactor2<VALUES, POSEKEY, POINTKEY> { class RangeFactor: public NonlinearFactor2<POSEKEY, POINTKEY> {
private: private:
double z_; /** measurement */ double z_; /** measurement */
typedef RangeFactor<VALUES, POSEKEY, POINTKEY> This; typedef RangeFactor<POSEKEY, POINTKEY> This;
typedef NonlinearFactor2<VALUES, POSEKEY, POINTKEY> Base; typedef NonlinearFactor2<POSEKEY, POINTKEY> Base;
typedef typename POSEKEY::Value Pose; typedef typename POSEKEY::Value Pose;
typedef typename POINTKEY::Value Point; typedef typename POINTKEY::Value Point;
@ -64,7 +64,7 @@ namespace gtsam {
} }
/** equals specialized to this factor */ /** equals specialized to this factor */
virtual bool equals(const NonlinearFactor<VALUES>& expected, double tol=1e-9) const { virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected); const This *e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol) && fabs(this->z_ - e->z_) < tol; return e != NULL && Base::equals(*e, tol) && fabs(this->z_ - e->z_) < tol;
} }

6
gtsam/slam/StereoFactor.h
View File

@ -22,8 +22,8 @@
namespace gtsam { namespace gtsam {
template<class VALUES, class KEY1, class KEY2> template<class KEY1, class KEY2>
class GenericStereoFactor: public NonlinearFactor2<VALUES, KEY1, KEY2> { class GenericStereoFactor: public NonlinearFactor2<KEY1, KEY2> {
private: private:
// Keep a copy of measurement and calibration for I/O // Keep a copy of measurement and calibration for I/O
@ -33,7 +33,7 @@ private:
public: public:
// shorthand for base class type // shorthand for base class type
typedef NonlinearFactor2<VALUES, KEY1, KEY2> Base; ///< typedef for base class typedef NonlinearFactor2<KEY1, KEY2> Base; ///< typedef for base class
typedef boost::shared_ptr<GenericStereoFactor> shared_ptr; ///< typedef for shared pointer to this object typedef boost::shared_ptr<GenericStereoFactor> shared_ptr; ///< typedef for shared pointer to this object
typedef typename KEY1::Value CamPose; ///< typedef for Pose Lie Value type typedef typename KEY1::Value CamPose; ///< typedef for Pose Lie Value type

10
gtsam/slam/dataset.cpp
View File

@ -156,10 +156,10 @@ void save2D(const Pose2Graph& graph, const DynamicValues& config, const SharedDi
fstream stream(filename.c_str(), fstream::out); fstream stream(filename.c_str(), fstream::out);
// save poses // save poses
pose2SLAM::Key key; for (DynamicValues::const_iterator it = config.begin(); it != config.end(); ++it) {
Pose2 pose; Pose2 pose = config.at<Pose2>(it->first);
BOOST_FOREACH(boost::tie(key, pose), config) stream << "VERTEX2 " << it->first.index() << " " << pose.x() << " " << pose.y() << " " << pose.theta() << endl;
stream << "VERTEX2 " << key.index() << " " << pose.x() << " " << pose.y() << " " << pose.theta() << endl; }
// save edges // save edges
Matrix R = model->R(); Matrix R = model->R();
@ -168,7 +168,7 @@ void save2D(const Pose2Graph& graph, const DynamicValues& config, const SharedDi
boost::shared_ptr<Pose2Factor> factor = boost::dynamic_pointer_cast<Pose2Factor>(factor_); boost::shared_ptr<Pose2Factor> factor = boost::dynamic_pointer_cast<Pose2Factor>(factor_);
if (!factor) continue; if (!factor) continue;
pose = factor->measured().inverse(); Pose2 pose = factor->measured().inverse();
stream << "EDGE2 " << factor->key2().index() << " " << factor->key1().index() stream << "EDGE2 " << factor->key2().index() << " " << factor->key1().index()
<< " " << pose.x() << " " << pose.y() << " " << pose.theta() << " " << pose.x() << " " << pose.y() << " " << pose.theta()
<< " " << RR(0, 0) << " " << RR(0, 1) << " " << RR(1, 1) << " " << RR(2, 2) << " " << RR(0, 0) << " " << RR(0, 1) << " " << RR(1, 1) << " " << RR(2, 2)

4
gtsam/slam/planarSLAM.cpp
View File

@ -25,8 +25,8 @@ namespace gtsam {
namespace planarSLAM { namespace planarSLAM {
Graph::Graph(const NonlinearFactorGraph<Values>& graph) : Graph::Graph(const NonlinearFactorGraph& graph) :
NonlinearFactorGraph<Values>(graph) {} NonlinearFactorGraph(graph) {}
void Graph::addPrior(const PoseKey& i, const Pose2& p, void Graph::addPrior(const PoseKey& i, const Pose2& p,
const SharedNoiseModel& model) { const SharedNoiseModel& model) {

46
gtsam/slam/planarSLAM.h
View File

@ -22,7 +22,6 @@
#include <gtsam/slam/RangeFactor.h> #include <gtsam/slam/RangeFactor.h>
#include <gtsam/slam/BearingFactor.h> #include <gtsam/slam/BearingFactor.h>
#include <gtsam/slam/BearingRangeFactor.h> #include <gtsam/slam/BearingRangeFactor.h>
#include <gtsam/nonlinear/TupleValues.h>
#include <gtsam/nonlinear/NonlinearEquality.h> #include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimization.h> #include <gtsam/nonlinear/NonlinearOptimization.h>
@ -41,32 +40,21 @@ namespace gtsam {
/// Typedef for a PointKey with Point2 data and 'l' symbol /// Typedef for a PointKey with Point2 data and 'l' symbol
typedef TypedSymbol<Point2, 'l'> PointKey; typedef TypedSymbol<Point2, 'l'> PointKey;
/// Typedef for Values structure with PoseKey type
typedef Values<PoseKey> PoseValues;
/// Typedef for Values structure with PointKey type
typedef Values<PointKey> PointValues;
/// Values class, inherited from TupleValues2, using PoseKeys and PointKeys /// Values class, inherited from TupleValues2, using PoseKeys and PointKeys
struct Values: public TupleValues2<PoseValues, PointValues> { struct Values: public DynamicValues {
/// Default constructor /// Default constructor
Values() {} Values() {}
/// Copy constructor /// Copy constructor
Values(const TupleValues2<PoseValues, PointValues>& values) : Values(const DynamicValues& values) :
TupleValues2<PoseValues, PointValues>(values) { DynamicValues(values) {
}
/// Copy constructor
Values(const TupleValues2<PoseValues, PointValues>::Base& values) :
TupleValues2<PoseValues, PointValues>(values) {
} }
/// From sub-values /// From sub-values
Values(const PoseValues& poses, const PointValues& points) : // Values(const DynamicValues& poses, const DynamicValues& points) :
TupleValues2<PoseValues, PointValues>(poses, points) { // DynamicValues(poses, points) {
} // }
// Convenience for MATLAB wrapper, which does not allow for identically named methods // Convenience for MATLAB wrapper, which does not allow for identically named methods
@ -88,26 +76,26 @@ namespace gtsam {
*/ */
/// A hard constraint for PoseKeys to enforce particular values /// A hard constraint for PoseKeys to enforce particular values
typedef NonlinearEquality<Values, PoseKey> Constraint; typedef NonlinearEquality<PoseKey> Constraint;
/// A prior factor to bias the value of a PoseKey /// A prior factor to bias the value of a PoseKey
typedef PriorFactor<Values, PoseKey> Prior; typedef PriorFactor<PoseKey> Prior;
/// A factor between two PoseKeys set with a Pose2 /// A factor between two PoseKeys set with a Pose2
typedef BetweenFactor<Values, PoseKey> Odometry; typedef BetweenFactor<PoseKey> Odometry;
/// A factor between a PoseKey and a PointKey to express difference in rotation (set with a Rot2) /// A factor between a PoseKey and a PointKey to express difference in rotation (set with a Rot2)
typedef BearingFactor<Values, PoseKey, PointKey> Bearing; typedef BearingFactor<PoseKey, PointKey> Bearing;
/// A factor between a PoseKey and a PointKey to express distance between them (set with a double) /// A factor between a PoseKey and a PointKey to express distance between them (set with a double)
typedef RangeFactor<Values, PoseKey, PointKey> Range; typedef RangeFactor<PoseKey, PointKey> Range;
/// A factor between a PoseKey and a PointKey to express difference in rotation and location /// A factor between a PoseKey and a PointKey to express difference in rotation and location
typedef BearingRangeFactor<Values, PoseKey, PointKey> BearingRange; typedef BearingRangeFactor<PoseKey, PointKey> BearingRange;
/// Creates a NonlinearFactorGraph with the Values type /// Creates a NonlinearFactorGraph with the Values type
struct Graph: public NonlinearFactorGraph<Values> { struct Graph: public NonlinearFactorGraph {
/// Default constructor for a NonlinearFactorGraph /// Default constructor for a NonlinearFactorGraph
Graph(){} Graph(){}
/// Creates a NonlinearFactorGraph based on another NonlinearFactorGraph /// Creates a NonlinearFactorGraph based on another NonlinearFactorGraph
Graph(const NonlinearFactorGraph<Values>& graph); Graph(const NonlinearFactorGraph& graph);
/// Biases the value of PoseKey key with Pose2 p given a noise model /// Biases the value of PoseKey key with Pose2 p given a noise model
void addPrior(const PoseKey& key, const Pose2& pose, const SharedNoiseModel& noiseModel); void addPrior(const PoseKey& key, const Pose2& pose, const SharedNoiseModel& noiseModel);
@ -132,15 +120,15 @@ namespace gtsam {
const Rot2& bearing, double range, const SharedNoiseModel& model); const Rot2& bearing, double range, const SharedNoiseModel& model);
/// Optimize /// Optimize
Values optimize(const Values& initialEstimate) { Values optimize(const DynamicValues& initialEstimate) {
typedef NonlinearOptimizer<Graph, Values> Optimizer; typedef NonlinearOptimizer<Graph> Optimizer;
return *Optimizer::optimizeLM(*this, initialEstimate, return *Optimizer::optimizeLM(*this, initialEstimate,
NonlinearOptimizationParameters::LAMBDA); NonlinearOptimizationParameters::LAMBDA);
} }
}; };
/// Optimizer /// Optimizer
typedef NonlinearOptimizer<Graph, Values> Optimizer; typedef NonlinearOptimizer<Graph> Optimizer;
} // planarSLAM } // planarSLAM

1
gtsam/slam/pose2SLAM.h
View File

@ -19,7 +19,6 @@
#include <gtsam/base/LieVector.h> #include <gtsam/base/LieVector.h>
#include <gtsam/geometry/Pose2.h> #include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/PriorFactor.h> #include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h> #include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/Key.h> #include <gtsam/nonlinear/Key.h>

6
gtsam/slam/pose3SLAM.cpp
View File

@ -25,8 +25,8 @@ namespace gtsam {
namespace pose3SLAM { namespace pose3SLAM {
/* ************************************************************************* */ /* ************************************************************************* */
Values circle(size_t n, double radius) { DynamicValues circle(size_t n, double radius) {
Values x; DynamicValues x;
double theta = 0, dtheta = 2 * M_PI / n; double theta = 0, dtheta = 2 * M_PI / n;
// We use aerospace/navlab convention, X forward, Y right, Z down // We use aerospace/navlab convention, X forward, Y right, Z down
// First pose will be at (R,0,0) // First pose will be at (R,0,0)
@ -39,7 +39,7 @@ namespace gtsam {
Point3 gti(radius*cos(theta), radius*sin(theta), 0); Point3 gti(radius*cos(theta), radius*sin(theta), 0);
Rot3 _0Ri = Rot3::yaw(-theta); // negative yaw goes counterclockwise, with Z down ! Rot3 _0Ri = Rot3::yaw(-theta); // negative yaw goes counterclockwise, with Z down !
Pose3 gTi(gR0 * _0Ri, gti); Pose3 gTi(gR0 * _0Ri, gti);
x.insert(i, gTi); x.insert(Key(i), gTi);
} }
return x; return x;
} }

16
gtsam/slam/pose3SLAM.h
View File

@ -18,7 +18,6 @@
#pragma once #pragma once
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/PriorFactor.h> #include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h> #include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/Key.h> #include <gtsam/nonlinear/Key.h>
@ -33,8 +32,6 @@ namespace gtsam {
/// Creates a Key with data Pose3 and symbol 'x' /// Creates a Key with data Pose3 and symbol 'x'
typedef TypedSymbol<Pose3, 'x'> Key; typedef TypedSymbol<Pose3, 'x'> Key;
/// Creates a Values structure with type 'Key'
typedef Values<Key> Values;
/** /**
* Create a circle of n 3D poses tangent to circle of radius R, first pose at (R,0) * Create a circle of n 3D poses tangent to circle of radius R, first pose at (R,0)
@ -42,17 +39,17 @@ namespace gtsam {
* @param R radius of circle * @param R radius of circle
* @return circle of n 3D poses * @return circle of n 3D poses
*/ */
Values circle(size_t n, double R); DynamicValues circle(size_t n, double R);
/// A prior factor on Key with Pose3 data type. /// A prior factor on Key with Pose3 data type.
typedef PriorFactor<Values, Key> Prior; typedef PriorFactor<Key> Prior;
/// A factor to put constraints between two factors. /// A factor to put constraints between two factors.
typedef BetweenFactor<Values, Key> Constraint; typedef BetweenFactor<Key> Constraint;
/// A hard constraint would enforce that the given key would have the input value in the results. /// A hard constraint would enforce that the given key would have the input value in the results.
typedef NonlinearEquality<Values, Key> HardConstraint; typedef NonlinearEquality<Key> HardConstraint;
/// Graph /// Graph
struct Graph: public NonlinearFactorGraph<Values> { struct Graph: public NonlinearFactorGraph {
/// Adds a factor between keys of the same type /// Adds a factor between keys of the same type
void addPrior(const Key& i, const Pose3& p, void addPrior(const Key& i, const Pose3& p,
@ -67,14 +64,13 @@ namespace gtsam {
}; };
/// Optimizer /// Optimizer
typedef NonlinearOptimizer<Graph, Values> Optimizer; typedef NonlinearOptimizer<Graph> Optimizer;
} // pose3SLAM } // pose3SLAM
/** /**
* Backwards compatibility * Backwards compatibility
*/ */
typedef pose3SLAM::Values Pose3Values; ///< Typedef for Values class for backwards compatibility
typedef pose3SLAM::Prior Pose3Prior; ///< Typedef for Prior class for backwards compatibility typedef pose3SLAM::Prior Pose3Prior; ///< Typedef for Prior class for backwards compatibility
typedef pose3SLAM::Constraint Pose3Factor; ///< Typedef for Constraint class for backwards compatibility typedef pose3SLAM::Constraint Pose3Factor; ///< Typedef for Constraint class for backwards compatibility
typedef pose3SLAM::Graph Pose3Graph; ///< Typedef for Graph class for backwards compatibility typedef pose3SLAM::Graph Pose3Graph; ///< Typedef for Graph class for backwards compatibility

1
gtsam/slam/simulated2D.h
View File

@ -19,7 +19,6 @@
#pragma once #pragma once
#include <gtsam/geometry/Point2.h> #include <gtsam/geometry/Point2.h>
#include <gtsam/nonlinear/TupleValues.h>
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>

14
gtsam/slam/simulated2DOriented.h
View File

@ -19,7 +19,6 @@
#pragma once #pragma once
#include <gtsam/geometry/Pose2.h> #include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/TupleValues.h>
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
@ -32,25 +31,26 @@ namespace gtsam {
// The types that take an oriented pose2 rather than point2 // The types that take an oriented pose2 rather than point2
typedef TypedSymbol<Point2, 'l'> PointKey; typedef TypedSymbol<Point2, 'l'> PointKey;
typedef TypedSymbol<Pose2, 'x'> PoseKey; typedef TypedSymbol<Pose2, 'x'> PoseKey;
typedef Values<PoseKey> PoseValues;
typedef Values<PointKey> PointValues;
/// Specialized Values structure with syntactic sugar for /// Specialized Values structure with syntactic sugar for
/// compatibility with matlab /// compatibility with matlab
class Values: public TupleValues2<PoseValues, PointValues> { class Values: public DynamicValues {
int nrPoses_, nrPoints_;
public: public:
Values() {} Values() : nrPoses_(0), nrPoints_(0) {}
void insertPose(const PoseKey& i, const Pose2& p) { void insertPose(const PoseKey& i, const Pose2& p) {
insert(i, p); insert(i, p);
nrPoses_++;
} }
void insertPoint(const PointKey& j, const Point2& p) { void insertPoint(const PointKey& j, const Point2& p) {
insert(j, p); insert(j, p);
nrPoints_++;
} }
int nrPoses() const { return this->first_.size(); } int nrPoses() const { return nrPoses_; }
int nrPoints() const { return this->second_.size(); } int nrPoints() const { return nrPoints_; }
Pose2 pose(const PoseKey& i) const { return (*this)[i]; } Pose2 pose(const PoseKey& i) const { return (*this)[i]; }
Point2 point(const PointKey& j) const { return (*this)[j]; } Point2 point(const PointKey& j) const { return (*this)[j]; }

1
gtsam/slam/simulated3D.h
View File

@ -24,7 +24,6 @@
#include <gtsam/linear/VectorValues.h> #include <gtsam/linear/VectorValues.h>
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/Key.h> #include <gtsam/nonlinear/Key.h>
#include <gtsam/nonlinear/TupleValues.h>
// \namespace // \namespace

28
gtsam/slam/visualSLAM.h
View File

@ -22,7 +22,6 @@
#include <gtsam/geometry/SimpleCamera.h> #include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/nonlinear/Key.h> #include <gtsam/nonlinear/Key.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/TupleValues.h>
#include <gtsam/nonlinear/NonlinearEquality.h> #include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h> #include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/slam/PriorFactor.h> #include <gtsam/slam/PriorFactor.h>
@ -39,25 +38,22 @@ namespace gtsam {
*/ */
typedef TypedSymbol<Pose3,'x'> PoseKey; ///< The key type used for poses typedef TypedSymbol<Pose3,'x'> PoseKey; ///< The key type used for poses
typedef TypedSymbol<Point3,'l'> PointKey; ///< The key type used for points typedef TypedSymbol<Point3,'l'> PointKey; ///< The key type used for points
typedef Values<PoseKey> PoseValues; ///< Values used for poses typedef boost::shared_ptr<DynamicValues> shared_values; ///< shared pointer to values data structure
typedef Values<PointKey> PointValues; ///< Values used for points
typedef TupleValues2<PoseValues, PointValues> Values; ///< Values data structure
typedef boost::shared_ptr<Values> shared_values; ///< shared pointer to values data structure
typedef NonlinearEquality<Values, PoseKey> PoseConstraint; ///< put a hard constraint on a pose typedef NonlinearEquality<PoseKey> PoseConstraint; ///< put a hard constraint on a pose
typedef NonlinearEquality<Values, PointKey> PointConstraint; ///< put a hard constraint on a point typedef NonlinearEquality<PointKey> PointConstraint; ///< put a hard constraint on a point
typedef PriorFactor<Values, PoseKey> PosePrior; ///< put a soft prior on a Pose typedef PriorFactor<PoseKey> PosePrior; ///< put a soft prior on a Pose
typedef PriorFactor<Values, PointKey> PointPrior; ///< put a soft prior on a point typedef PriorFactor<PointKey> PointPrior; ///< put a soft prior on a point
typedef RangeFactor<Values, PoseKey, PointKey> RangeFactor; ///< put a factor on the range from a pose to a point typedef RangeFactor<PoseKey, PointKey> RangeFactor; ///< put a factor on the range from a pose to a point
/// monocular and stereo camera typedefs for general use /// monocular and stereo camera typedefs for general use
typedef GenericProjectionFactor<Values, PointKey, PoseKey> ProjectionFactor; typedef GenericProjectionFactor<PointKey, PoseKey> ProjectionFactor;
typedef GenericStereoFactor<Values, PoseKey, PointKey> StereoFactor; typedef GenericStereoFactor<PoseKey, PointKey> StereoFactor;
/** /**
* Non-linear factor graph for vanilla visual SLAM * Non-linear factor graph for vanilla visual SLAM
*/ */
class Graph: public NonlinearFactorGraph<Values> { class Graph: public NonlinearFactorGraph {
public: public:
/// shared pointer to this type of graph /// shared pointer to this type of graph
@ -69,12 +65,12 @@ namespace gtsam {
/// print out graph /// print out graph
void print(const std::string& s = "") const { void print(const std::string& s = "") const {
NonlinearFactorGraph<Values>::print(s); NonlinearFactorGraph::print(s);
} }
/// check if two graphs are equal /// check if two graphs are equal
bool equals(const Graph& p, double tol = 1e-9) const { bool equals(const Graph& p, double tol = 1e-9) const {
return NonlinearFactorGraph<Values>::equals(p, tol); return NonlinearFactorGraph::equals(p, tol);
} }
/** /**
@ -140,6 +136,6 @@ namespace gtsam {
}; // Graph }; // Graph
/// typedef for Optimizer. The current default will use the multi-frontal solver /// typedef for Optimizer. The current default will use the multi-frontal solver
typedef NonlinearOptimizer<Graph, Values> Optimizer; typedef NonlinearOptimizer<Graph> Optimizer;
} } // namespaces } } // namespaces