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/GaussianISAM.h> // To get optimize(BayesTree<GaussianConditional>)
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
//#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Ordering.h>
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
headers += Key.h
headers += NonlinearFactorGraph.h NonlinearFactorGraph-inl.h
headers += NonlinearFactorGraph.h
headers += NonlinearOptimizer-inl.h NonlinearOptimization.h NonlinearOptimization-inl.h NonlinearOptimizationParameters.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
# 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
* @author Frank Dellaert
* @author Carlos Nieto
* @author Christian Potthast
*/
#pragma once
#include <cmath>
#include <boost/foreach.hpp>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/inference.h>
#include <boost/foreach.hpp>
#include <cmath>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
using namespace std;

1
gtsam/nonlinear/NonlinearFactorGraph.h
View File

@ -100,4 +100,3 @@ namespace gtsam {
} // 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
*/
@ -34,8 +34,8 @@ namespace gtsam {
/* ************************************************************************* */
template<class J>
void Values<J>::print(const string &s) const {
cout << "Values " << s << ", size " << values_.size() << "\n";
void ValuesOld<J>::print(const string &s) const {
cout << "ValuesOld " << s << ", size " << values_.size() << "\n";
BOOST_FOREACH(const KeyValuePair& v, values_) {
gtsam::print(v.second, (string)(v.first));
}
@ -43,7 +43,7 @@ namespace gtsam {
/* ************************************************************************* */
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;
BOOST_FOREACH(const KeyValuePair& v, values_) {
if (!expected.exists(v.first)) return false;
@ -55,7 +55,7 @@ namespace gtsam {
/* ************************************************************************* */
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);
if (it == values_.end()) throw KeyDoesNotExist<J>("retrieve", j);
else return it->second;
@ -63,7 +63,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class J>
size_t Values<J>::dim() const {
size_t ValuesOld<J>::dim() const {
size_t n = 0;
BOOST_FOREACH(const KeyValuePair& value, values_)
n += value.second.dim();
@ -72,27 +72,27 @@ namespace gtsam {
/* ************************************************************************* */
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));
}
/* ************************************************************************* */
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)
throw KeyAlreadyExists<J>(name, val);
}
/* ************************************************************************* */
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_)
insert(v.first, v.second);
}
/* ************************************************************************* */
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::optional<typename J::Value> t = cfg.exists_(v.first);
if (t) values_[v.first] = *t;
@ -101,13 +101,13 @@ namespace gtsam {
/* ************************************************************************* */
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;
}
/* ************************************************************************* */
template<class J>
std::list<J> Values<J>::keys() const {
std::list<J> ValuesOld<J>::keys() const {
std::list<J> ret;
BOOST_FOREACH(const KeyValuePair& v, values_)
ret.push_back(v.first);
@ -116,14 +116,14 @@ namespace gtsam {
/* ************************************************************************* */
template<class J>
void Values<J>::erase(const J& j) {
void ValuesOld<J>::erase(const J& j) {
size_t dim; // unused
erase(j, dim);
}
/* ************************************************************************* */
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);
if (it == values_.end())
throw KeyDoesNotExist<J>("erase", j);
@ -134,8 +134,8 @@ namespace gtsam {
/* ************************************************************************* */
// todo: insert for every element is inefficient
template<class J>
Values<J> Values<J>::retract(const VectorValues& delta, const Ordering& ordering) const {
Values<J> newValues;
ValuesOld<J> ValuesOld<J>::retract(const VectorValues& delta, const Ordering& ordering) const {
ValuesOld<J> newValues;
typedef pair<J,typename J::Value> KeyValue;
BOOST_FOREACH(const KeyValue& value, this->values_) {
const J& j = value.first;
@ -151,7 +151,7 @@ namespace gtsam {
/* ************************************************************************* */
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);
this->apply(dimCollector);
return dimCollector.dimensions;
@ -159,7 +159,7 @@ namespace gtsam {
/* ************************************************************************* */
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
_ValuesKeyOrderer keyOrderer(firstVar);
this->apply(keyOrderer);
@ -169,12 +169,12 @@ namespace gtsam {
// /* ************************************************************************* */
// // todo: insert for every element is inefficient
// 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()) {
// 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.");
// }
// Values<J> newValues;
// ValuesOld<J> newValues;
// int delta_offset = 0;
// typedef pair<J,typename J::Value> KeyValue;
// BOOST_FOREACH(const KeyValue& value, this->values_) {
@ -191,7 +191,7 @@ namespace gtsam {
// todo: insert for every element is inefficient
// todo: currently only logmaps elements in both configs
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));
localCoordinates(cp, ordering, delta);
return delta;
@ -199,7 +199,7 @@ namespace gtsam {
/* ************************************************************************* */
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;
BOOST_FOREACH(const KeyValue& value, cp) {
assert(this->exists(value.first));
@ -213,7 +213,7 @@ namespace gtsam {
if(message_.empty())
message_ =
"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();
}

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

@ -10,14 +10,14 @@
* -------------------------------------------------------------------------- */
/**
* @file Values.h
* @file ValuesOld.h
* @author Richard Roberts
*
* @brief A templated config for Manifold-group elements
*
* Detailed story:
* 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
* 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)
*/
template<class J>
class Values {
class ValuesOld {
public:
@ -77,12 +77,12 @@ namespace gtsam {
public:
Values() {}
Values(const Values& config) :
ValuesOld() {}
ValuesOld(const ValuesOld& config) :
values_(config.values_) {}
template<class J_ALT>
Values(const Values<J_ALT>& other) {} // do nothing when initializing with wrong type
virtual ~Values() {}
ValuesOld(const ValuesOld<J_ALT>& other) {} // do nothing when initializing with wrong type
virtual ~ValuesOld() {}
/// @name Testable
/// @{
@ -91,7 +91,7 @@ namespace gtsam {
void print(const std::string &s="") const;
/** 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;
/** 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) */
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) */
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);
/** 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 */
void update(const Values& cfg);
void update(const ValuesOld& cfg);
/** single element change of existing element */
void update(const J& j, const Value& val);
@ -172,7 +172,7 @@ namespace gtsam {
std::list<J> keys() const;
/** Replace all keys and variables */
Values& operator=(const Values& rhs) {
ValuesOld& operator=(const ValuesOld& rhs) {
values_ = rhs.values_;
return (*this);
}
@ -185,7 +185,7 @@ namespace gtsam {
/**
* 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
* 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>
void apply(A& operation) {
@ -297,5 +297,5 @@ public:
} // \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
* construct the mask.
*/
template<class VALUES, class KEY>
class PartialPriorFactor: public NonlinearFactor1<VALUES, KEY> {
template<class KEY>
class PartialPriorFactor: public NonlinearFactor1<KEY> {
public:
typedef typename KEY::Value T;
protected:
typedef NonlinearFactor1<VALUES, KEY> Base;
typedef PartialPriorFactor<VALUES, KEY> This;
typedef NonlinearFactor1<KEY> Base;
typedef PartialPriorFactor<KEY> This;
Vector prior_; ///< measurement on logmap parameters, in compressed form
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.
* i.e. the main building block for visual SLAM.
*/
template<class VALUES, class LMK, class POSK>
class GenericProjectionFactor: public NonlinearFactor2<VALUES, POSK, LMK> {
template<class LMK, class POSK>
class GenericProjectionFactor: public NonlinearFactor2<POSK, LMK> {
protected:
// Keep a copy of measurement and calibration for I/O
@ -40,10 +40,10 @@ namespace gtsam {
public:
/// shorthand for base class type
typedef NonlinearFactor2<VALUES, POSK, LMK> Base;
typedef NonlinearFactor2<POSK, LMK> Base;
/// 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
GenericProjectionFactor() :
@ -73,7 +73,7 @@ namespace gtsam {
}
/// equals
bool equals(const GenericProjectionFactor<VALUES, LMK, POSK>& p
bool equals(const GenericProjectionFactor<LMK, POSK>& p
, double tol = 1e-9) const {
return Base::equals(p, tol) && this->z_.equals(p.z_, 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
*/
template<class VALUES, class POSEKEY, class POINTKEY>
class RangeFactor: public NonlinearFactor2<VALUES, POSEKEY, POINTKEY> {
template<class POSEKEY, class POINTKEY>
class RangeFactor: public NonlinearFactor2<POSEKEY, POINTKEY> {
private:
double z_; /** measurement */
typedef RangeFactor<VALUES, POSEKEY, POINTKEY> This;
typedef NonlinearFactor2<VALUES, POSEKEY, POINTKEY> Base;
typedef RangeFactor<POSEKEY, POINTKEY> This;
typedef NonlinearFactor2<POSEKEY, POINTKEY> Base;
typedef typename POSEKEY::Value Pose;
typedef typename POINTKEY::Value Point;
@ -64,7 +64,7 @@ namespace gtsam {
}
/** 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);
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 {
template<class VALUES, class KEY1, class KEY2>
class GenericStereoFactor: public NonlinearFactor2<VALUES, KEY1, KEY2> {
template<class KEY1, class KEY2>
class GenericStereoFactor: public NonlinearFactor2<KEY1, KEY2> {
private:
// Keep a copy of measurement and calibration for I/O
@ -33,7 +33,7 @@ private:
public:
// 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 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);
// save poses
pose2SLAM::Key key;
Pose2 pose;
BOOST_FOREACH(boost::tie(key, pose), config)
stream << "VERTEX2 " << key.index() << " " << pose.x() << " " << pose.y() << " " << pose.theta() << endl;
for (DynamicValues::const_iterator it = config.begin(); it != config.end(); ++it) {
Pose2 pose = config.at<Pose2>(it->first);
stream << "VERTEX2 " << it->first.index() << " " << pose.x() << " " << pose.y() << " " << pose.theta() << endl;
}
// save edges
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_);
if (!factor) continue;
pose = factor->measured().inverse();
Pose2 pose = factor->measured().inverse();
stream << "EDGE2 " << factor->key2().index() << " " << factor->key1().index()
<< " " << pose.x() << " " << pose.y() << " " << pose.theta()
<< " " << 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 {
Graph::Graph(const NonlinearFactorGraph<Values>& graph) :
NonlinearFactorGraph<Values>(graph) {}
Graph::Graph(const NonlinearFactorGraph& graph) :
NonlinearFactorGraph(graph) {}
void Graph::addPrior(const PoseKey& i, const Pose2& p,
const SharedNoiseModel& model) {

46
gtsam/slam/planarSLAM.h
View File

@ -22,7 +22,6 @@
#include <gtsam/slam/RangeFactor.h>
#include <gtsam/slam/BearingFactor.h>
#include <gtsam/slam/BearingRangeFactor.h>
#include <gtsam/nonlinear/TupleValues.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimization.h>
@ -41,32 +40,21 @@ namespace gtsam {
/// Typedef for a PointKey with Point2 data and 'l' symbol
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
struct Values: public TupleValues2<PoseValues, PointValues> {
struct Values: public DynamicValues {
/// Default constructor
Values() {}
/// Copy constructor
Values(const TupleValues2<PoseValues, PointValues>& values) :
TupleValues2<PoseValues, PointValues>(values) {
}
/// Copy constructor
Values(const TupleValues2<PoseValues, PointValues>::Base& values) :
TupleValues2<PoseValues, PointValues>(values) {
Values(const DynamicValues& values) :
DynamicValues(values) {
}
/// From sub-values
Values(const PoseValues& poses, const PointValues& points) :
TupleValues2<PoseValues, PointValues>(poses, points) {
}
// Values(const DynamicValues& poses, const DynamicValues& points) :
// DynamicValues(poses, points) {
// }
// 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
typedef NonlinearEquality<Values, PoseKey> Constraint;
typedef NonlinearEquality<PoseKey> Constraint;
/// 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
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)
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)
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
typedef BearingRangeFactor<Values, PoseKey, PointKey> BearingRange;
typedef BearingRangeFactor<PoseKey, PointKey> BearingRange;
/// Creates a NonlinearFactorGraph with the Values type
struct Graph: public NonlinearFactorGraph<Values> {
struct Graph: public NonlinearFactorGraph {
/// Default constructor for a NonlinearFactorGraph
Graph(){}
/// 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
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);
/// Optimize
Values optimize(const Values& initialEstimate) {
typedef NonlinearOptimizer<Graph, Values> Optimizer;
Values optimize(const DynamicValues& initialEstimate) {
typedef NonlinearOptimizer<Graph> Optimizer;
return *Optimizer::optimizeLM(*this, initialEstimate,
NonlinearOptimizationParameters::LAMBDA);
}
};
/// Optimizer
typedef NonlinearOptimizer<Graph, Values> Optimizer;
typedef NonlinearOptimizer<Graph> Optimizer;
} // planarSLAM

1
gtsam/slam/pose2SLAM.h
View File

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

6
gtsam/slam/pose3SLAM.cpp
View File

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

16
gtsam/slam/pose3SLAM.h
View File

@ -18,7 +18,6 @@
#pragma once
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/Key.h>
@ -33,8 +32,6 @@ namespace gtsam {
/// Creates a Key with data Pose3 and symbol 'x'
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)
@ -42,17 +39,17 @@ namespace gtsam {
* @param R radius of circle
* @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.
typedef PriorFactor<Values, Key> Prior;
typedef PriorFactor<Key> Prior;
/// 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.
typedef NonlinearEquality<Values, Key> HardConstraint;
typedef NonlinearEquality<Key> HardConstraint;
/// Graph
struct Graph: public NonlinearFactorGraph<Values> {
struct Graph: public NonlinearFactorGraph {
/// Adds a factor between keys of the same type
void addPrior(const Key& i, const Pose3& p,
@ -67,14 +64,13 @@ namespace gtsam {
};
/// Optimizer
typedef NonlinearOptimizer<Graph, Values> Optimizer;
typedef NonlinearOptimizer<Graph> Optimizer;
} // pose3SLAM
/**
* 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::Constraint Pose3Factor; ///< Typedef for Constraint 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
#include <gtsam/geometry/Point2.h>
#include <gtsam/nonlinear/TupleValues.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>

14
gtsam/slam/simulated2DOriented.h
View File

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

28
gtsam/slam/visualSLAM.h
View File

@ -22,7 +22,6 @@
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/nonlinear/Key.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/TupleValues.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearOptimizer.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<Point3,'l'> PointKey; ///< The key type used for points
typedef Values<PoseKey> PoseValues; ///< Values used for poses
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 boost::shared_ptr<DynamicValues> shared_values; ///< shared pointer to values data structure
typedef NonlinearEquality<Values, PoseKey> PoseConstraint; ///< put a hard constraint on a pose
typedef NonlinearEquality<Values, PointKey> PointConstraint; ///< put a hard constraint on a point
typedef PriorFactor<Values, PoseKey> PosePrior; ///< put a soft prior on a Pose
typedef PriorFactor<Values, 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 NonlinearEquality<PoseKey> PoseConstraint; ///< put a hard constraint on a pose
typedef NonlinearEquality<PointKey> PointConstraint; ///< put a hard constraint on a point
typedef PriorFactor<PoseKey> PosePrior; ///< put a soft prior on a Pose
typedef PriorFactor<PointKey> PointPrior; ///< put a soft prior on 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
typedef GenericProjectionFactor<Values, PointKey, PoseKey> ProjectionFactor;
typedef GenericStereoFactor<Values, PoseKey, PointKey> StereoFactor;
typedef GenericProjectionFactor<PointKey, PoseKey> ProjectionFactor;
typedef GenericStereoFactor<PoseKey, PointKey> StereoFactor;
/**
* Non-linear factor graph for vanilla visual SLAM
*/
class Graph: public NonlinearFactorGraph<Values> {
class Graph: public NonlinearFactorGraph {
public:
/// shared pointer to this type of graph
@ -69,12 +65,12 @@ namespace gtsam {
/// print out graph
void print(const std::string& s = "") const {
NonlinearFactorGraph<Values>::print(s);
NonlinearFactorGraph::print(s);
}
/// check if two graphs are equal
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
/// typedef for Optimizer. The current default will use the multi-frontal solver
typedef NonlinearOptimizer<Graph, Values> Optimizer;
typedef NonlinearOptimizer<Graph> Optimizer;
} } // namespaces

8
tests/testNonlinearFactor.cpp
View File

@ -93,7 +93,7 @@ TEST( NonlinearFactor, NonlinearFactor )
CHECK(assert_equal(0.1*ones(2),actual_e));
// error = 0.5 * [1 1] * [1;1] = 1
double expected = 1.0;
double expected = 1.0;
// calculate the error from the factor "f1"
double actual = factor->error(cfg);
@ -179,14 +179,14 @@ TEST( NonlinearFactor, size )
{
// create a non linear factor graph
Graph fg = createNonlinearFactorGraph();
// create a values structure for the non linear factor graph
example::Values cfg = createNoisyValues();
// get some factors from the graph
Graph::sharedFactor factor1 = fg[0], factor2 = fg[1],
factor3 = fg[2];
CHECK(factor1->size() == 1);
CHECK(factor2->size() == 2);
CHECK(factor3->size() == 2);