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

Continuing changes from templating NonlinearFactors on value instead of key

release/4.3a0
Richard Roberts 2012-02-06 23:32:59 +00:00
parent 7c7c3e3836
commit eaa9e4d739
28 changed files with 358 additions and 290 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/base/DerivedValue.h
View File

@ -83,6 +83,16 @@ public:
return (static_cast<DERIVED*>(this))->operator=(derivedRhs); return (static_cast<DERIVED*>(this))->operator=(derivedRhs);
} }
/// Conversion to the derived class
operator const DERIVED& () const {
return static_cast<const DERIVED&>(*this);
}
/// Conversion to the derived class
operator DERIVED& () {
return static_cast<DERIVED&>(*this);
}
protected: protected:
/// Assignment operator, protected because only the Value or DERIVED /// Assignment operator, protected because only the Value or DERIVED
/// assignment operators should be used. /// assignment operators should be used.

63
gtsam/nonlinear/NonlinearEquality.h
View File

@ -47,11 +47,11 @@ namespace gtsam {
* *
* \nosubgrouping * \nosubgrouping
*/ */
template<class KEY> template<class VALUE>
class NonlinearEquality: public NonlinearFactor1<KEY> { class NonlinearEquality: public NonlinearFactor1<VALUE> {
public: public:
typedef typename KEY::Value T; typedef VALUE T;
private: private:
@ -64,6 +64,12 @@ namespace gtsam {
// error gain in allow error case // error gain in allow error case
double error_gain_; double error_gain_;
// typedef to this class
typedef NonlinearEquality<VALUE> This;
// typedef to base class
typedef NonlinearFactor1<VALUE> Base;
public: public:
/** /**
@ -71,7 +77,6 @@ namespace gtsam {
*/ */
bool (*compare_)(const T& a, const T& b); bool (*compare_)(const T& a, const T& b);
typedef NonlinearFactor1<KEY> Base;
/** default constructor - only for serialization */ /** default constructor - only for serialization */
NonlinearEquality() {} NonlinearEquality() {}
@ -84,7 +89,7 @@ namespace gtsam {
/** /**
* Constructor - forces exact evaluation * Constructor - forces exact evaluation
*/ */
NonlinearEquality(const KEY& j, const T& feasible, bool (*_compare)(const T&, const T&) = compare<T>) : NonlinearEquality(const Symbol& j, const T& feasible, bool (*_compare)(const T&, const T&) = compare<T>) :
Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_(feasible), Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_(feasible),
allow_error_(false), error_gain_(0.0), allow_error_(false), error_gain_(0.0),
compare_(_compare) { compare_(_compare) {
@ -93,7 +98,7 @@ namespace gtsam {
/** /**
* Constructor - allows inexact evaluation * Constructor - allows inexact evaluation
*/ */
NonlinearEquality(const KEY& j, const T& feasible, double error_gain, bool (*_compare)(const T&, const T&) = compare<T>) : NonlinearEquality(const Symbol& j, const T& feasible, double error_gain, bool (*_compare)(const T&, const T&) = compare<T>) :
Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_(feasible), Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_(feasible),
allow_error_(true), error_gain_(error_gain), allow_error_(true), error_gain_(error_gain),
compare_(_compare) { compare_(_compare) {
@ -103,17 +108,17 @@ namespace gtsam {
/// @name Testable /// @name Testable
/// @{ /// @{
void print(const std::string& s = "") const { virtual void print(const std::string& s = "") const {
std::cout << "Constraint: " << s << " on [" << (std::string)(this->key_) << "]\n"; std::cout << "Constraint: " << s << " on [" << (std::string)(this->key()) << "]\n";
gtsam::print(feasible_,"Feasible Point"); gtsam::print(feasible_,"Feasible Point");
std::cout << "Variable Dimension: " << feasible_.dim() << std::endl; std::cout << "Variable Dimension: " << feasible_.dim() << std::endl;
} }
/** Check if two factors are equal */ /** Check if two factors are equal */
bool equals(const NonlinearEquality<KEY>& f, double tol = 1e-9) const { virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const {
if (!Base::equals(f)) return false; const This* e = dynamic_cast<const This*>(&f);
return feasible_.equals(f.feasible_, tol) && return e && Base::equals(f) && feasible_.equals(e->feasible_, tol) &&
fabs(error_gain_ - f.error_gain_) < tol; fabs(error_gain_ - e->error_gain_) < tol;
} }
/// @} /// @}
@ -122,7 +127,7 @@ namespace gtsam {
/** actual error function calculation */ /** actual error function calculation */
virtual double error(const Values& c) const { virtual double error(const Values& c) const {
const T& xj = c[this->key_]; const T& xj = c.at<T>(this->key());
Vector e = this->unwhitenedError(c); Vector e = this->unwhitenedError(c);
if (allow_error_ || !compare_(xj, feasible_)) { if (allow_error_ || !compare_(xj, feasible_)) {
return error_gain_ * dot(e,e); return error_gain_ * dot(e,e);
@ -132,7 +137,7 @@ namespace gtsam {
} }
/** error function */ /** error function */
inline Vector evaluateError(const T& xj, boost::optional<Matrix&> H = boost::none) const { Vector evaluateError(const T& xj, boost::optional<Matrix&> H = boost::none) const {
size_t nj = feasible_.dim(); size_t nj = feasible_.dim();
if (allow_error_) { if (allow_error_) {
if (H) *H = eye(nj); // FIXME: this is not the right linearization for nonlinear compare if (H) *H = eye(nj); // FIXME: this is not the right linearization for nonlinear compare
@ -142,18 +147,18 @@ namespace gtsam {
return zero(nj); // set error to zero if equal return zero(nj); // set error to zero if equal
} else { } else {
if (H) throw std::invalid_argument( if (H) throw std::invalid_argument(
"Linearization point not feasible for " + (std::string)(this->key_) + "!"); "Linearization point not feasible for " + (std::string)(this->key()) + "!");
return repeat(nj, std::numeric_limits<double>::infinity()); // set error to infinity if not equal return repeat(nj, std::numeric_limits<double>::infinity()); // set error to infinity if not equal
} }
} }
// Linearize is over-written, because base linearization tries to whiten // Linearize is over-written, because base linearization tries to whiten
virtual GaussianFactor::shared_ptr linearize(const Values& x, const Ordering& ordering) const { virtual GaussianFactor::shared_ptr linearize(const Values& x, const Ordering& ordering) const {
const T& xj = x[this->key_]; const T& xj = x.at<T>(this->key());
Matrix A; Matrix A;
Vector b = evaluateError(xj, A); Vector b = evaluateError(xj, A);
SharedDiagonal model = noiseModel::Constrained::All(b.size()); SharedDiagonal model = noiseModel::Constrained::All(b.size());
return GaussianFactor::shared_ptr(new JacobianFactor(ordering[this->key_], A, b, model)); return GaussianFactor::shared_ptr(new JacobianFactor(ordering[this->key()], A, b, model));
} }
/// @} /// @}
@ -177,14 +182,14 @@ namespace gtsam {
/** /**
* Simple unary equality constraint - fixes a value for a variable * Simple unary equality constraint - fixes a value for a variable
*/ */
template<class KEY> template<class VALUE>
class NonlinearEquality1 : public NonlinearFactor1<KEY> { class NonlinearEquality1 : public NonlinearFactor1<VALUE> {
public: public:
typedef typename KEY::Value X; typedef VALUE X;
protected: protected:
typedef NonlinearFactor1<KEY> Base; typedef NonlinearFactor1<VALUE> Base;
/** default constructor to allow for serialization */ /** default constructor to allow for serialization */
NonlinearEquality1() {} NonlinearEquality1() {}
@ -196,10 +201,10 @@ namespace gtsam {
public: public:
typedef boost::shared_ptr<NonlinearEquality1<KEY> > shared_ptr; typedef boost::shared_ptr<NonlinearEquality1<VALUE> > shared_ptr;
///TODO: comment ///TODO: comment
NonlinearEquality1(const X& value, const KEY& key1, double mu = 1000.0) NonlinearEquality1(const X& value, const Symbol& key1, double mu = 1000.0)
: Base(noiseModel::Constrained::All(value.dim(), fabs(mu)), key1), value_(value) {} : Base(noiseModel::Constrained::All(value.dim(), fabs(mu)), key1), value_(value) {}
virtual ~NonlinearEquality1() {} virtual ~NonlinearEquality1() {}
@ -236,13 +241,13 @@ namespace gtsam {
* Simple binary equality constraint - this constraint forces two factors to * Simple binary equality constraint - this constraint forces two factors to
* be the same. * be the same.
*/ */
template<class KEY> template<class VALUE>
class NonlinearEquality2 : public NonlinearFactor2<KEY, KEY> { class NonlinearEquality2 : public NonlinearFactor2<VALUE, VALUE> {
public: public:
typedef typename KEY::Value X; typedef VALUE X;
protected: protected:
typedef NonlinearFactor2<KEY, KEY> Base; typedef NonlinearFactor2<VALUE, VALUE> Base;
GTSAM_CONCEPT_MANIFOLD_TYPE(X); GTSAM_CONCEPT_MANIFOLD_TYPE(X);
@ -251,10 +256,10 @@ namespace gtsam {
public: public:
typedef boost::shared_ptr<NonlinearEquality2<KEY> > shared_ptr; typedef boost::shared_ptr<NonlinearEquality2<VALUE> > shared_ptr;
///TODO: comment ///TODO: comment
NonlinearEquality2(const KEY& key1, const KEY& key2, double mu = 1000.0) NonlinearEquality2(const Symbol& key1, const Symbol& key2, double mu = 1000.0)
: Base(noiseModel::Constrained::All(X::Dim(), fabs(mu)), key1, key2) {} : Base(noiseModel::Constrained::All(X::Dim(), fabs(mu)), key1, key2) {}
virtual ~NonlinearEquality2() {} virtual ~NonlinearEquality2() {}

10
gtsam/nonlinear/NonlinearFactor.h
View File

@ -95,6 +95,11 @@ public:
std::cout << s << ": NonlinearFactor\n"; std::cout << s << ": NonlinearFactor\n";
} }
/** Check if two factors are equal */
virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const {
return Base::equals(f);
}
/// @} /// @}
/// @name Standard Interface /// @name Standard Interface
/// @{ /// @{
@ -202,8 +207,9 @@ public:
} }
/** Check if two factors are equal */ /** Check if two factors are equal */
virtual bool equals(const NoiseModelFactor& f, double tol = 1e-9) const { virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const {
return noiseModel_->equals(*f.noiseModel_, tol) && Base::equals(f, tol); const NoiseModelFactor* e = dynamic_cast<const NoiseModelFactor*>(&f);
return e && Base::equals(f, tol) && noiseModel_->equals(*e->noiseModel_, tol);
} }
/** get the dimension of the factor (number of rows on linearization) */ /** get the dimension of the factor (number of rows on linearization) */

44
gtsam/nonlinear/Values-inl.h
View File

@ -23,7 +23,10 @@
*/ */
#include <utility> #include <utility>
#include <boost/type_traits/conditional.hpp>
#include <boost/type_traits/is_base_of.hpp>
#include <gtsam/base/DerivedValue.h>
#include <gtsam/nonlinear/Values.h> // Only so Eclipse finds class definition #include <gtsam/nonlinear/Values.h> // Only so Eclipse finds class definition
namespace gtsam { namespace gtsam {
@ -37,6 +40,30 @@ namespace gtsam {
ValueCloneAllocator() {} ValueCloneAllocator() {}
}; };
/* ************************************************************************* */
class ValueAutomaticCasting {
const Symbol& key_;
const Value& value_;
public:
ValueAutomaticCasting(const Symbol& key, const Value& value) : key_(key), value_(value) {}
template<class ValueType>
operator const ValueType& () const {
// Check the type and throw exception if incorrect
if(typeid(ValueType) != typeid(value_))
throw ValuesIncorrectType(key_, typeid(ValueType), typeid(value_));
// We have already checked the type, so do a "blind" static_cast, not dynamic_cast
return static_cast<const ValueType&>(value_);
}
};
// /* ************************************************************************* */
// template<class ValueType>
// ValueType& operator=(ValueType& lhs, const ValueAutomaticCasting& rhs) {
// lhs = rhs;
// }
/* ************************************************************************* */ /* ************************************************************************* */
template<typename ValueType> template<typename ValueType>
const ValueType& Values::at(const Symbol& j) const { const ValueType& Values::at(const Symbol& j) const {
@ -55,6 +82,18 @@ namespace gtsam {
return static_cast<const ValueType&>(*item->second); return static_cast<const ValueType&>(*item->second);
} }
/* ************************************************************************* */
inline ValueAutomaticCasting Values::at(const Symbol& j) const {
// Find the item
KeyValueMap::const_iterator item = values_.find(j);
// Throw exception if it does not exist
if(item == values_.end())
throw ValuesKeyDoesNotExist("retrieve", j);
return ValueAutomaticCasting(item->first, *item->second);
}
/* ************************************************************************* */ /* ************************************************************************* */
template<typename TypedKey> template<typename TypedKey>
const typename TypedKey::Value& Values::at(const TypedKey& j) const { const typename TypedKey::Value& Values::at(const TypedKey& j) const {
@ -65,6 +104,11 @@ namespace gtsam {
return at<typename TypedKey::Value>(symbol); return at<typename TypedKey::Value>(symbol);
} }
/* ************************************************************************* */
inline ValueAutomaticCasting Values::operator[](const Symbol& j) const {
return at(j);
}
/* ************************************************************************* */ /* ************************************************************************* */
template<typename ValueType> template<typename ValueType>
boost::optional<const ValueType&> Values::exists(const Symbol& j) const { boost::optional<const ValueType&> Values::exists(const Symbol& j) const {

13
gtsam/nonlinear/Values.h
View File

@ -44,6 +44,7 @@ namespace gtsam {
// Forward declarations // Forward declarations
class ValueCloneAllocator; class ValueCloneAllocator;
class ValueAutomaticCasting;
/** /**
* A non-templated config holding any types of Manifold-group elements. A * A non-templated config holding any types of Manifold-group elements. A
@ -138,6 +139,15 @@ namespace gtsam {
template<typename ValueType> template<typename ValueType>
const ValueType& at(const Symbol& j) const; const ValueType& at(const Symbol& j) const;
/** Retrieve a variable by key \c j. This non-templated version returns a
* special ValueAutomaticCasting object that may be assigned to the proper
* value.
* @param j Retrieve the value associated with this key
* @return A ValueAutomaticCasting object that may be assignmed to a Value
* of the proper type.
*/
ValueAutomaticCasting at(const Symbol& j) const;
/** Retrieve a variable using a special key (typically TypedSymbol), which /** Retrieve a variable using a special key (typically TypedSymbol), which
* contains the type of the value associated with the key, and which must * contains the type of the value associated with the key, and which must
* be conversion constructible to a Symbol, e.g. * be conversion constructible to a Symbol, e.g.
@ -153,6 +163,9 @@ namespace gtsam {
const typename TypedKey::Value& operator[](const TypedKey& j) const { const typename TypedKey::Value& operator[](const TypedKey& j) const {
return at(j); } return at(j); }
/** operator[] syntax for at(const Symbol& j) */
ValueAutomaticCasting operator[](const Symbol& j) const;
/** Check if a value exists with key \c j. See exists<>(const Symbol& j) /** Check if a value exists with key \c j. See exists<>(const Symbol& j)
* and exists(const TypedKey& j) for versions that return the value if it * and exists(const TypedKey& j) for versions that return the value if it
* exists. */ * exists. */

4
gtsam/slam/BearingFactor.h
View File

@ -29,9 +29,9 @@ namespace gtsam {
class BearingFactor: public NonlinearFactor2<POSE, POINT> { class BearingFactor: public NonlinearFactor2<POSE, POINT> {
private: private:
typedef typename POSE Pose; typedef POSE Pose;
typedef typename Pose::Rotation Rot; typedef typename Pose::Rotation Rot;
typedef typename POINT Point; typedef POINT Point;
typedef BearingFactor<POSE, POINT> This; typedef BearingFactor<POSE, POINT> This;
typedef NonlinearFactor2<POSE, POINT> Base; typedef NonlinearFactor2<POSE, POINT> Base;

10
gtsam/slam/BearingRangeFactor.h
View File

@ -31,9 +31,9 @@ namespace gtsam {
class BearingRangeFactor: public NonlinearFactor2<POSE, POINT> { class BearingRangeFactor: public NonlinearFactor2<POSE, POINT> {
private: private:
typedef typename POSE Pose; typedef POSE Pose;
typedef typename POSE::Rotation Rot; typedef typename POSE::Rotation Rot;
typedef typename POINT Point; typedef POINT Point;
typedef BearingRangeFactor<POSE, POINT> This; typedef BearingRangeFactor<POSE, POINT> This;
typedef NonlinearFactor2<POSE, POINT> Base; typedef NonlinearFactor2<POSE, POINT> Base;
@ -60,9 +60,9 @@ namespace gtsam {
/** Print */ /** Print */
virtual void print(const std::string& s = "") const { virtual void print(const std::string& s = "") const {
std::cout << s << ": BearingRangeFactor(" std::cout << s << ": BearingRangeFactor("
<< (std::string) this->key1_ << "," << (std::string) this->key1() << ","
<< (std::string) this->key2_ << ")\n"; << (std::string) this->key2() << ")\n";
bearing_.print(" measured bearing"); measuredBearing_.print(" measured bearing");
std::cout << " measured range: " << measuredRange_ << std::endl; std::cout << " measured range: " << measuredRange_ << std::endl;
this->noiseModel_->print(" noise model"); this->noiseModel_->print(" noise model");
} }

15
gtsam/slam/BetweenFactor.h
View File

@ -26,12 +26,15 @@ namespace gtsam {
/** /**
* A class for a measurement predicted by "between(config[key1],config[key2])" * A class for a measurement predicted by "between(config[key1],config[key2])"
* KEY1::Value is the Lie Group type * @tparam VALUE the Value type
* T is the measurement type, by default the same
*/ */
template<class VALUE> template<class VALUE>
class BetweenFactor: public NonlinearFactor2<VALUE, VALUE> { class BetweenFactor: public NonlinearFactor2<VALUE, VALUE> {
public:
typedef VALUE T;
private: private:
typedef BetweenFactor<VALUE> This; typedef BetweenFactor<VALUE> This;
@ -64,8 +67,8 @@ namespace gtsam {
/** print */ /** print */
virtual void print(const std::string& s) const { virtual void print(const std::string& s) const {
std::cout << s << "BetweenFactor(" std::cout << s << "BetweenFactor("
<< (std::string) this->key1_ << "," << (std::string) this->key1() << ","
<< (std::string) this->key2_ << ")\n"; << (std::string) this->key2() << ")\n";
measured_.print(" measured"); measured_.print(" measured");
this->noiseModel_->print(" noise model"); this->noiseModel_->print(" noise model");
} }
@ -79,7 +82,7 @@ namespace gtsam {
/** implement functions needed to derive from Factor */ /** implement functions needed to derive from Factor */
/** vector of errors */ /** vector of errors */
Vector evaluateError(const typename KEY1::Value& p1, const typename KEY1::Value& p2, Vector evaluateError(const T& p1, const T& p2,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
boost::none) const { boost::none) const {
T hx = p1.between(p2, H1, H2); // h(x) T hx = p1.between(p2, H1, H2); // h(x)
@ -121,7 +124,7 @@ namespace gtsam {
/** Syntactic sugar for constrained version */ /** Syntactic sugar for constrained version */
BetweenConstraint(const VALUE& measured, const Symbol& key1, const Symbol& key2, double mu = 1000.0) : BetweenConstraint(const VALUE& measured, const Symbol& key1, const Symbol& key2, double mu = 1000.0) :
BetweenFactor<VALUE>(key1, key2, measured, noiseModel::Constrained::All(KEY::Value::Dim(), fabs(mu))) {} BetweenFactor<VALUE>(key1, key2, measured, noiseModel::Constrained::All(VALUE::Dim(), fabs(mu))) {}
private: private:

14
gtsam/slam/GeneralSFMFactor.h
View File

@ -37,8 +37,8 @@ namespace gtsam {
public: public:
typedef typename CAMERA Cam; ///< typedef for camera type typedef CAMERA Cam; ///< typedef for camera type
typedef GeneralSFMFactor<CAMERA, LANDMARK> This ; ///< typedef for this object typedef GeneralSFMFactor<CAMERA, LANDMARK> This; ///< typedef for this object
typedef NonlinearFactor2<CAMERA, LANDMARK> Base; ///< typedef for the base class typedef NonlinearFactor2<CAMERA, LANDMARK> Base; ///< typedef for the base class
typedef Point2 Measurement; ///< typedef for the measurement typedef Point2 Measurement; ///< typedef for the measurement
@ -52,7 +52,8 @@ namespace gtsam {
* @param i is basically the frame number * @param i is basically the frame number
* @param j is the index of the landmark * @param j is the index of the landmark
*/ */
GeneralSFMFactor(const Point2& measured, const SharedNoiseModel& model, const Symbol& cameraKey, const Landmark& landmarkKey) : Base(model, i, j), measured_(measured) {} GeneralSFMFactor(const Point2& measured, const SharedNoiseModel& model, const Symbol& cameraKey, const Symbol& landmarkKey) :
Base(model, cameraKey, cameraKey), measured_(measured) {}
GeneralSFMFactor():measured_(0.0,0.0) {} ///< default constructor GeneralSFMFactor():measured_(0.0,0.0) {} ///< default constructor
GeneralSFMFactor(const Point2 & p):measured_(p) {} ///< constructor that takes a Point2 GeneralSFMFactor(const Point2 & p):measured_(p) {} ///< constructor that takes a Point2
@ -66,14 +67,15 @@ namespace gtsam {
*/ */
void print(const std::string& s = "SFMFactor") const { void print(const std::string& s = "SFMFactor") const {
Base::print(s); Base::print(s);
z_.print(s + ".z"); measured_.print(s + ".z");
} }
/** /**
* equals * equals
*/ */
bool equals(const GeneralSFMFactor<CamK, LmK> &p, double tol = 1e-9) const { bool equals(const NonlinearFactor &p, double tol = 1e-9) const {
return Base::equals(p, tol) && this->measured_.equals(p.z_, tol) ; const This* e = dynamic_cast<const This*>(&p);
return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol) ;
} }
/** h(x)-z */ /** h(x)-z */

2
gtsam/slam/PriorFactor.h
View File

@ -60,7 +60,7 @@ namespace gtsam {
/** print */ /** print */
virtual void print(const std::string& s) const { virtual void print(const std::string& s) const {
std::cout << s << "PriorFactor(" << (std::string) this->key_ << ")\n"; std::cout << s << "PriorFactor(" << (std::string) this->key() << ")\n";
prior_.print(" prior"); prior_.print(" prior");
this->noiseModel_->print(" noise model"); this->noiseModel_->print(" noise model");
} }

8
gtsam/slam/ProjectionFactor.h
View File

@ -78,8 +78,8 @@ namespace gtsam {
/// equals /// equals
virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
const This *e = dynamic_cast<const This*> (&expected); const This *e = dynamic_cast<const This*>(&p);
return e && Base::equals(p, tol) && this->measured_.equals(e->z_, tol) && this->K_->equals(*e->K_, tol); return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol) && this->K_->equals(*e->K_, tol);
} }
/// Evaluate error h(x)-z and optionally derivatives /// Evaluate error h(x)-z and optionally derivatives
@ -92,8 +92,8 @@ namespace gtsam {
} catch( CheiralityException& e) { } catch( CheiralityException& e) {
if (H1) *H1 = zeros(2,6); if (H1) *H1 = zeros(2,6);
if (H2) *H2 = zeros(2,3); if (H2) *H2 = zeros(2,3);
cout << e.what() << ": Landmark "<< this->key2_.index() << cout << e.what() << ": Landmark "<< this->key2().index() <<
" moved behind camera " << this->key1_.index() << endl; " moved behind camera " << this->key1().index() << endl;
return ones(2) * 2.0 * K_->fx(); return ones(2) * 2.0 * K_->fx();
} }
} }

8
gtsam/slam/RangeFactor.h
View File

@ -34,8 +34,8 @@ namespace gtsam {
typedef RangeFactor<POSE, POINT> This; typedef RangeFactor<POSE, POINT> This;
typedef NonlinearFactor2<POSE, POINT> Base; typedef NonlinearFactor2<POSE, POINT> Base;
typedef typename POSE Pose; typedef POSE Pose;
typedef typename POINT Point; typedef POINT Point;
// Concept requirements for this factor // Concept requirements for this factor
GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(Pose, Point) GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(Pose, Point)
@ -46,7 +46,7 @@ namespace gtsam {
RangeFactor(const Symbol& poseKey, const Symbol& pointKey, double measured, RangeFactor(const Symbol& poseKey, const Symbol& pointKey, double measured,
const SharedNoiseModel& model) : const SharedNoiseModel& model) :
Base(model, poseKey, PointKey), measured_(measured) { Base(model, poseKey, pointKey), measured_(measured) {
} }
virtual ~RangeFactor() {} virtual ~RangeFactor() {}
@ -65,7 +65,7 @@ namespace gtsam {
/** equals specialized to this factor */ /** equals specialized to this factor */
virtual bool equals(const NonlinearFactor& 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->measured_ - e->z_) < tol; return e != NULL && Base::equals(*e, tol) && fabs(this->measured_ - e->measured_) < tol;
} }
/** print contents */ /** print contents */

51
gtsam/slam/StereoFactor.h
View File

@ -22,20 +22,20 @@
namespace gtsam { namespace gtsam {
template<class KEY1, class KEY2> template<class POSE, class LANDMARK>
class GenericStereoFactor: public NonlinearFactor2<KEY1, KEY2> { class GenericStereoFactor: public NonlinearFactor2<POSE, LANDMARK> {
private: private:
// Keep a copy of measurement and calibration for I/O // Keep a copy of measurement and calibration for I/O
StereoPoint2 z_; ///< the measurement StereoPoint2 measured_; ///< the measurement
boost::shared_ptr<Cal3_S2Stereo> K_; ///< shared pointer to calibration boost::shared_ptr<Cal3_S2Stereo> K_; ///< shared pointer to calibration
public: public:
// shorthand for base class type // shorthand for base class type
typedef NonlinearFactor2<KEY1, KEY2> Base; ///< typedef for base class typedef NonlinearFactor2<POSE, LANDMARK> 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 POSE CamPose; ///< typedef for Pose Lie Value type
/** /**
* Default constructor * Default constructor
@ -44,18 +44,17 @@ public:
/** /**
* Constructor * Constructor
* @param z is the Stereo Point measurement (u_l, u_r, v). v will be identical for left & right for rectified stereo pair * @param measured is the Stereo Point measurement (u_l, u_r, v). v will be identical for left & right for rectified stereo pair
* @param model is the noise model in on the measurement * @param model is the noise model in on the measurement
* @param j_pose the pose index * @param poseKey the pose variable key
* @param j_landmark the landmark index * @param landmarkKey the landmark variable key
* @param K the constant calibration * @param K the constant calibration
*/ */
GenericStereoFactor(const StereoPoint2& z, const SharedNoiseModel& model, KEY1 j_pose, GenericStereoFactor(const StereoPoint2& measured, const SharedNoiseModel& model, const Symbol& poseKey, const Symbol& landmarkKey, const shared_ptrKStereo& K) :
KEY2 j_landmark, const shared_ptrKStereo& K) : Base(model, poseKey, landmarkKey), measured_(measured), K_(K) {
Base(model, j_pose, j_landmark), z_(z), K_(K) {
} }
~GenericStereoFactor() {} ///< desctructor ~GenericStereoFactor() {} ///< destructor
/** /**
* print * print
@ -63,41 +62,27 @@ public:
*/ */
void print(const std::string& s) const { void print(const std::string& s) const {
Base::print(s); Base::print(s);
z_.print(s + ".z"); measured_.print(s + ".z");
} }
/** /**
* equals * equals
*/ */
bool equals(const GenericStereoFactor& f, double tol) const { virtual bool equals(const NonlinearFactor& f, double tol) const {
const GenericStereoFactor* p = dynamic_cast<const GenericStereoFactor*> (&f); const GenericStereoFactor* p = dynamic_cast<const GenericStereoFactor*> (&f);
if (p == NULL) return p && Base::equals(f) && measured_.equals(p->measured_, tol);
goto fail;
//if (cameraFrameNumber_ != p->cameraFrameNumber_ || landmarkNumber_ != p->landmarkNumber_) goto fail;
if (!z_.equals(p->z_, tol))
goto fail;
return true;
fail: print("actual");
p->print("expected");
return false;
} }
/** h(x)-z */ /** h(x)-z */
Vector evaluateError(const Pose3& pose, const Point3& point, Vector evaluateError(const Pose3& pose, const Point3& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
StereoCamera stereoCam(pose, K_); StereoCamera stereoCam(pose, K_);
return (stereoCam.project(point, H1, H2) - z_).vector(); return (stereoCam.project(point, H1, H2) - measured_).vector();
}
/// get the measurement z
StereoPoint2 z() {
return z_;
} }
/** return the measured */ /** return the measured */
inline const StereoPoint2 measured() const { const StereoPoint2& measured() const {
return z_; return measured_;
} }
private: private:
@ -105,7 +90,7 @@ private:
friend class boost::serialization::access; friend class boost::serialization::access;
template<class Archive> template<class Archive>
void serialize(Archive & ar, const unsigned int version) { void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(z_); ar & BOOST_SERIALIZATION_NVP(measured_);
ar & BOOST_SERIALIZATION_NVP(K_); ar & BOOST_SERIALIZATION_NVP(K_);
} }
}; };

4
gtsam/slam/dataset.cpp
View File

@ -133,9 +133,9 @@ pair<sharedPose2Graph, Values::shared_ptr> load2D(const string& filename,
// Insert vertices if pure odometry file // Insert vertices if pure odometry file
if (!poses->exists(pose2SLAM::PoseKey(id1))) poses->insert(pose2SLAM::PoseKey(id1), Pose2()); if (!poses->exists(pose2SLAM::PoseKey(id1))) poses->insert(pose2SLAM::PoseKey(id1), Pose2());
if (!poses->exists(pose2SLAM::PoseKey(id2))) poses->insert(pose2SLAM::PoseKey(id2), poses->at(pose2SLAM::PoseKey(id1)) * l1Xl2); if (!poses->exists(pose2SLAM::PoseKey(id2))) poses->insert(pose2SLAM::PoseKey(id2), poses->at<Pose2>(pose2SLAM::PoseKey(id1)) * l1Xl2);
pose2SLAM::Graph::sharedFactor factor(new Pose2Factor(id1, id2, l1Xl2, *model)); pose2SLAM::Graph::sharedFactor factor(new Pose2Factor(pose2SLAM::PoseKey(id1), pose2SLAM::PoseKey(id2), l1Xl2, *model));
graph->push_back(factor); graph->push_back(factor);
} }
is.ignore(LINESIZE, '\n'); is.ignore(LINESIZE, '\n');

2
gtsam/slam/pose3SLAM.cpp
View File

@ -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(Key(i), gTi); x.insert(PoseKey(i), gTi);
} }
return x; return x;
} }

20
gtsam/slam/simulated2D.h
View File

@ -57,14 +57,14 @@ namespace simulated2D {
} }
/// Insert a pose /// Insert a pose
void insertPose(const simulated2D::PoseKey& i, const Point2& p) { void insertPose(Index j, const Point2& p) {
insert(i, p); insert(PoseKey(j), p);
nrPoses_++; nrPoses_++;
} }
/// Insert a point /// Insert a point
void insertPoint(const simulated2D::PointKey& j, const Point2& p) { void insertPoint(Index j, const Point2& p) {
insert(j, p); insert(PointKey(j), p);
nrPoints_++; nrPoints_++;
} }
@ -79,13 +79,13 @@ namespace simulated2D {
} }
/// Return pose i /// Return pose i
Point2 pose(const simulated2D::PoseKey& i) const { Point2 pose(Index j) const {
return (*this)[i]; return at<Point2>(PoseKey(j));
} }
/// Return point j /// Return point j
Point2 point(const simulated2D::PointKey& j) const { Point2 point(Index j) const {
return (*this)[j]; return at<Point2>(PointKey(j));
} }
}; };
@ -156,7 +156,7 @@ namespace simulated2D {
/** /**
* Binary factor simulating "odometry" between two Vectors * Binary factor simulating "odometry" between two Vectors
*/ */
template<class VALUE = Pose2> template<class VALUE = Point2>
class GenericOdometry: public NonlinearFactor2<VALUE, VALUE> { class GenericOdometry: public NonlinearFactor2<VALUE, VALUE> {
public: public:
typedef NonlinearFactor2<VALUE, VALUE> Base; ///< base class typedef NonlinearFactor2<VALUE, VALUE> Base; ///< base class
@ -203,7 +203,7 @@ namespace simulated2D {
typedef POSE Pose; ///< shortcut to Pose type typedef POSE Pose; ///< shortcut to Pose type
typedef LANDMARK Landmark; ///< shortcut to Landmark type typedef LANDMARK Landmark; ///< shortcut to Landmark type
Landmark z_; ///< Measurement Landmark measured_; ///< Measurement
/// Create measurement factor /// Create measurement factor
GenericMeasurement(const Landmark& measured, const SharedNoiseModel& model, const Symbol& poseKey, const Symbol& landmarkKey) : GenericMeasurement(const Landmark& measured, const SharedNoiseModel& model, const Symbol& poseKey, const Symbol& landmarkKey) :

14
gtsam/slam/simulated2DOriented.h
View File

@ -40,21 +40,21 @@ namespace simulated2DOriented {
public: public:
Values() : nrPoses_(0), nrPoints_(0) {} Values() : nrPoses_(0), nrPoints_(0) {}
void insertPose(const PoseKey& i, const Pose2& p) { void insertPose(Index j, const Pose2& p) {
insert(i, p); insert(PoseKey(j), p);
nrPoses_++; nrPoses_++;
} }
void insertPoint(const PointKey& j, const Point2& p) { void insertPoint(Index j, const Point2& p) {
insert(j, p); insert(PointKey(j), p);
nrPoints_++; nrPoints_++;
} }
int nrPoses() const { return nrPoses_; } int nrPoses() const { return nrPoses_; }
int nrPoints() const { return nrPoints_; } int nrPoints() const { return nrPoints_; }
Pose2 pose(const PoseKey& i) const { return (*this)[i]; } Pose2 pose(Index j) const { return at<Pose2>(PoseKey(j)); }
Point2 point(const PointKey& j) const { return (*this)[j]; } Point2 point(Index j) const { return at<Point2>(PointKey(j)); }
}; };
//TODO:: point prior is not implemented right now //TODO:: point prior is not implemented right now
@ -111,7 +111,7 @@ namespace simulated2DOriented {
} }
/// Evaluate error and optionally derivative /// Evaluate error and optionally derivative
Vector evaluateError(const Pose2& x1, const Pose2& x2, Vector evaluateError(const VALUE& x1, const VALUE& x2,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const { boost::optional<Matrix&> H2 = boost::none) const {
return measured_.localCoordinates(odo(x1, x2, H1, H2)); return measured_.localCoordinates(odo(x1, x2, H1, H2));

46
gtsam/slam/simulated3D.h
View File

@ -36,8 +36,11 @@ namespace simulated3D {
* the simulated2D domain. * the simulated2D domain.
*/ */
typedef gtsam::TypedSymbol<Point3, 'x'> PoseKey; /// Convenience function for constructing a pose key
typedef gtsam::TypedSymbol<Point3, 'l'> PointKey; inline Symbol PoseKey(Index j) { return Symbol('x', j); }
/// Convenience function for constructing a pose key
inline Symbol PointKey(Index j) { return Symbol('l', j); }
/** /**
* Prior on a single pose * Prior on a single pose
@ -61,19 +64,18 @@ Point3 mea(const Point3& x, const Point3& l,
/** /**
* A prior factor on a single linear robot pose * A prior factor on a single linear robot pose
*/ */
struct PointPrior3D: public NonlinearFactor1<PoseKey> { struct PointPrior3D: public NonlinearFactor1<Point3> {
Point3 z_; ///< The prior pose value for the variable attached to this factor Point3 measured_; ///< The prior pose value for the variable attached to this factor
/** /**
* Constructor for a prior factor * Constructor for a prior factor
* @param z is the measured/prior position for the pose * @param measured is the measured/prior position for the pose
* @param model is the measurement model for the factor (Dimension: 3) * @param model is the measurement model for the factor (Dimension: 3)
* @param j is the key for the pose * @param key is the key for the pose
*/ */
PointPrior3D(const Point3& z, PointPrior3D(const Point3& measured, const SharedNoiseModel& model, const Symbol& key) :
const SharedNoiseModel& model, const PoseKey& j) : NonlinearFactor1<Point3> (model, key), measured_(measured) {
NonlinearFactor1<PoseKey> (model, j), z_(z) {
} }
/** /**
@ -85,29 +87,27 @@ struct PointPrior3D: public NonlinearFactor1<PoseKey> {
*/ */
Vector evaluateError(const Point3& x, boost::optional<Matrix&> H = Vector evaluateError(const Point3& x, boost::optional<Matrix&> H =
boost::none) { boost::none) {
return (prior(x, H) - z_).vector(); return (prior(x, H) - measured_).vector();
} }
}; };
/** /**
* Models a linear 3D measurement between 3D points * Models a linear 3D measurement between 3D points
*/ */
struct Simulated3DMeasurement: public NonlinearFactor2<PoseKey, PointKey> { struct Simulated3DMeasurement: public NonlinearFactor2<Point3, Point3> {
Point3 z_; ///< Linear displacement between a pose and landmark Point3 measured_; ///< Linear displacement between a pose and landmark
/** /**
* Creates a measurement factor with a given measurement * Creates a measurement factor with a given measurement
* @param z is the measurement, a linear displacement between poses and landmarks * @param measured is the measurement, a linear displacement between poses and landmarks
* @param model is a measurement model for the factor (Dimension: 3) * @param model is a measurement model for the factor (Dimension: 3)
* @param j1 is the pose key of the robot * @param poseKey is the pose key of the robot
* @param j2 is the point key for the landmark * @param pointKey is the point key for the landmark
*/ */
Simulated3DMeasurement(const Point3& z, Simulated3DMeasurement(const Point3& measured, const SharedNoiseModel& model,
const SharedNoiseModel& model, PoseKey& j1, PointKey j2) : const Symbol& poseKey, const Symbol& pointKey) :
NonlinearFactor2<PoseKey, PointKey> ( NonlinearFactor2<Point3, Point3>(model, poseKey, pointKey), measured_(measured) {}
model, j1, j2), z_(z) {
}
/** /**
* Error function with optional derivatives * Error function with optional derivatives
@ -117,9 +117,9 @@ struct Simulated3DMeasurement: public NonlinearFactor2<PoseKey, PointKey> {
* @param H2 is an optional Jacobian matrix in terms of x2 (Dimension: 3x3) * @param H2 is an optional Jacobian matrix in terms of x2 (Dimension: 3x3)
* @return vector error between measurement and prediction (Dimension: 3) * @return vector error between measurement and prediction (Dimension: 3)
*/ */
Vector evaluateError(const Point3& x1, const Point3& x2, boost::optional< Vector evaluateError(const Point3& x1, const Point3& x2,
Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) { boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) {
return (mea(x1, x2, H1, H2) - z_).vector(); return (mea(x1, x2, H1, H2) - measured_).vector();
} }
}; };

49
gtsam/slam/smallExample.cpp
View File

@ -39,7 +39,10 @@ using namespace std;
namespace gtsam { namespace gtsam {
namespace example { namespace example {
typedef boost::shared_ptr<NonlinearFactor > shared; using simulated2D::PoseKey;
using simulated2D::PointKey;
typedef boost::shared_ptr<NonlinearFactor> shared;
static SharedDiagonal sigma1_0 = noiseModel::Isotropic::Sigma(2,1.0); static SharedDiagonal sigma1_0 = noiseModel::Isotropic::Sigma(2,1.0);
static SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1); static SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1);
@ -57,22 +60,22 @@ namespace example {
// prior on x1 // prior on x1
Point2 mu; Point2 mu;
shared f1(new simulated2D::Prior(mu, sigma0_1, 1)); shared f1(new simulated2D::Prior(mu, sigma0_1, PoseKey(1)));
nlfg->push_back(f1); nlfg->push_back(f1);
// odometry between x1 and x2 // odometry between x1 and x2
Point2 z2(1.5, 0); Point2 z2(1.5, 0);
shared f2(new simulated2D::Odometry(z2, sigma0_1, 1, 2)); shared f2(new simulated2D::Odometry(z2, sigma0_1, PoseKey(1), PoseKey(2)));
nlfg->push_back(f2); nlfg->push_back(f2);
// measurement between x1 and l1 // measurement between x1 and l1
Point2 z3(0, -1); Point2 z3(0, -1);
shared f3(new simulated2D::Measurement(z3, sigma0_2, 1, 1)); shared f3(new simulated2D::Measurement(z3, sigma0_2, PoseKey(1), PointKey(1)));
nlfg->push_back(f3); nlfg->push_back(f3);
// measurement between x2 and l1 // measurement between x2 and l1
Point2 z4(-1.5, -1.); Point2 z4(-1.5, -1.);
shared f4(new simulated2D::Measurement(z4, sigma0_2, 2, 1)); shared f4(new simulated2D::Measurement(z4, sigma0_2, PoseKey(2), PointKey(1)));
nlfg->push_back(f4); nlfg->push_back(f4);
return nlfg; return nlfg;
@ -86,9 +89,9 @@ namespace example {
/* ************************************************************************* */ /* ************************************************************************* */
Values createValues() { Values createValues() {
Values c; Values c;
c.insert(simulated2D::PoseKey(1), Point2(0.0, 0.0)); c.insert(PoseKey(1), Point2(0.0, 0.0));
c.insert(simulated2D::PoseKey(2), Point2(1.5, 0.0)); c.insert(PoseKey(2), Point2(1.5, 0.0));
c.insert(simulated2D::PointKey(1), Point2(0.0, -1.0)); c.insert(PointKey(1), Point2(0.0, -1.0));
return c; return c;
} }
@ -104,9 +107,9 @@ namespace example {
/* ************************************************************************* */ /* ************************************************************************* */
boost::shared_ptr<const Values> sharedNoisyValues() { boost::shared_ptr<const Values> sharedNoisyValues() {
boost::shared_ptr<Values> c(new Values); boost::shared_ptr<Values> c(new Values);
c->insert(simulated2D::PoseKey(1), Point2(0.1, 0.1)); c->insert(PoseKey(1), Point2(0.1, 0.1));
c->insert(simulated2D::PoseKey(2), Point2(1.4, 0.2)); c->insert(PoseKey(2), Point2(1.4, 0.2));
c->insert(simulated2D::PointKey(1), Point2(0.1, -1.1)); c->insert(PointKey(1), Point2(0.1, -1.1));
return c; return c;
} }
@ -195,17 +198,15 @@ namespace example {
0.0, cos(v.y())); 0.0, cos(v.y()));
} }
struct UnaryFactor: public gtsam::NonlinearFactor1<simulated2D::PoseKey> { struct UnaryFactor: public gtsam::NonlinearFactor1<Point2> {
Point2 z_; Point2 z_;
UnaryFactor(const Point2& z, const SharedNoiseModel& model, UnaryFactor(const Point2& z, const SharedNoiseModel& model, const Symbol& key) :
const simulated2D::PoseKey& key) : gtsam::NonlinearFactor1<Point2>(model, key), z_(z) {
gtsam::NonlinearFactor1<simulated2D::PoseKey>(model, key), z_(z) {
} }
Vector evaluateError(const Point2& x, boost::optional<Matrix&> A = Vector evaluateError(const Point2& x, boost::optional<Matrix&> A = boost::none) const {
boost::none) const {
if (A) *A = H(x); if (A) *A = H(x);
return (h(x) - z_).vector(); return (h(x) - z_).vector();
} }
@ -220,7 +221,7 @@ namespace example {
Vector z = Vector_(2, 1.0, 0.0); Vector z = Vector_(2, 1.0, 0.0);
double sigma = 0.1; double sigma = 0.1;
boost::shared_ptr<smallOptimize::UnaryFactor> factor( boost::shared_ptr<smallOptimize::UnaryFactor> factor(
new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(2,sigma), 1)); new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(2,sigma), PoseKey(1)));
fg->push_back(factor); fg->push_back(factor);
return fg; return fg;
} }
@ -238,23 +239,23 @@ namespace example {
// prior on x1 // prior on x1
Point2 x1(1.0, 0.0); Point2 x1(1.0, 0.0);
shared prior(new simulated2D::Prior(x1, sigma1_0, 1)); shared prior(new simulated2D::Prior(x1, sigma1_0, PoseKey(1)));
nlfg.push_back(prior); nlfg.push_back(prior);
poses.insert(simulated2D::PoseKey(1), x1); poses.insert(simulated2D::PoseKey(1), x1);
for (int t = 2; t <= T; t++) { for (int t = 2; t <= T; t++) {
// odometry between x_t and x_{t-1} // odometry between x_t and x_{t-1}
Point2 odo(1.0, 0.0); Point2 odo(1.0, 0.0);
shared odometry(new simulated2D::Odometry(odo, sigma1_0, t - 1, t)); shared odometry(new simulated2D::Odometry(odo, sigma1_0, PoseKey(t - 1), PoseKey(t)));
nlfg.push_back(odometry); nlfg.push_back(odometry);
// measurement on x_t is like perfect GPS // measurement on x_t is like perfect GPS
Point2 xt(t, 0); Point2 xt(t, 0);
shared measurement(new simulated2D::Prior(xt, sigma1_0, t)); shared measurement(new simulated2D::Prior(xt, sigma1_0, PoseKey(t)));
nlfg.push_back(measurement); nlfg.push_back(measurement);
// initial estimate // initial estimate
poses.insert(simulated2D::PoseKey(t), xt); poses.insert(PoseKey(t), xt);
} }
return make_pair(nlfg, poses); return make_pair(nlfg, poses);
@ -414,8 +415,8 @@ namespace example {
/* ************************************************************************* */ /* ************************************************************************* */
// Create key for simulated planar graph // Create key for simulated planar graph
simulated2D::PoseKey key(int x, int y) { Symbol key(int x, int y) {
return simulated2D::PoseKey(1000*x+y); return PoseKey(1000*x+y);
} }
/* ************************************************************************* */ /* ************************************************************************* */

18
gtsam/slam/tests/testAntiFactor.cpp
View File

@ -27,6 +27,8 @@
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
using pose3SLAM::PoseKey;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( AntiFactor, NegativeHessian) TEST( AntiFactor, NegativeHessian)
{ {
@ -40,17 +42,17 @@ TEST( AntiFactor, NegativeHessian)
// Create a configuration corresponding to the ground truth // Create a configuration corresponding to the ground truth
boost::shared_ptr<Values> values(new Values()); boost::shared_ptr<Values> values(new Values());
values->insert(pose3SLAM::Key(1), pose1); values->insert(PoseKey(1), pose1);
values->insert(pose3SLAM::Key(2), pose2); values->insert(PoseKey(2), pose2);
// Define an elimination ordering // Define an elimination ordering
Ordering::shared_ptr ordering(new Ordering()); Ordering::shared_ptr ordering(new Ordering());
ordering->insert(pose3SLAM::Key(1), 0); ordering->insert(PoseKey(1), 0);
ordering->insert(pose3SLAM::Key(2), 1); ordering->insert(PoseKey(2), 1);
// Create a "standard" factor // Create a "standard" factor
BetweenFactor<pose3SLAM::Key>::shared_ptr originalFactor(new BetweenFactor<pose3SLAM::Key>(1, 2, z, sigma)); BetweenFactor<Pose3>::shared_ptr originalFactor(new BetweenFactor<Pose3>(PoseKey(1), PoseKey(2), z, sigma));
// Linearize it into a Jacobian Factor // Linearize it into a Jacobian Factor
GaussianFactor::shared_ptr originalJacobian = originalFactor->linearize(*values, *ordering); GaussianFactor::shared_ptr originalJacobian = originalFactor->linearize(*values, *ordering);
@ -101,8 +103,8 @@ TEST( AntiFactor, EquivalentBayesNet)
// Create a configuration corresponding to the ground truth // Create a configuration corresponding to the ground truth
boost::shared_ptr<Values> values(new Values()); boost::shared_ptr<Values> values(new Values());
values->insert(pose3SLAM::Key(1), pose1); values->insert(PoseKey(1), pose1);
values->insert(pose3SLAM::Key(2), pose2); values->insert(PoseKey(2), pose2);
// Define an elimination ordering // Define an elimination ordering
Ordering::shared_ptr ordering = graph->orderingCOLAMD(*values); Ordering::shared_ptr ordering = graph->orderingCOLAMD(*values);
@ -115,7 +117,7 @@ TEST( AntiFactor, EquivalentBayesNet)
VectorValues expectedDeltas = optimize(*expectedBayesNet); VectorValues expectedDeltas = optimize(*expectedBayesNet);
// Add an additional factor between Pose1 and Pose2 // Add an additional factor between Pose1 and Pose2
pose3SLAM::Constraint::shared_ptr f1(new pose3SLAM::Constraint(1, 2, z, sigma)); pose3SLAM::Constraint::shared_ptr f1(new pose3SLAM::Constraint(PoseKey(1), PoseKey(2), z, sigma));
graph->push_back(f1); graph->push_back(f1);
// Add the corresponding AntiFactor between Pose1 and Pose2 // Add the corresponding AntiFactor between Pose1 and Pose2

25
gtsam/slam/tests/testGeneralSFMFactor.cpp
View File

@ -28,25 +28,23 @@ using namespace std;
using namespace gtsam; using namespace gtsam;
typedef PinholeCamera<Cal3_S2> GeneralCamera; typedef PinholeCamera<Cal3_S2> GeneralCamera;
typedef TypedSymbol<GeneralCamera, 'x'> CameraKey; typedef GeneralSFMFactor<GeneralCamera, Point3> Projection;
typedef TypedSymbol<Point3, 'l'> PointKey; typedef NonlinearEquality<GeneralCamera> CameraConstraint;
typedef GeneralSFMFactor<CameraKey, PointKey> Projection; typedef NonlinearEquality<Point3> Point3Constraint;
typedef NonlinearEquality<CameraKey> CameraConstraint;
typedef NonlinearEquality<PointKey> Point3Constraint;
class Graph: public NonlinearFactorGraph { class Graph: public NonlinearFactorGraph {
public: public:
void addMeasurement(const CameraKey& i, const PointKey& j, const Point2& z, const SharedNoiseModel& model) { void addMeasurement(int i, int j, const Point2& z, const SharedNoiseModel& model) {
push_back(boost::make_shared<Projection>(z, model, i, j)); push_back(boost::make_shared<Projection>(z, model, Symbol('x',i), Symbol('l',j)));
} }
void addCameraConstraint(int j, const GeneralCamera& p) { void addCameraConstraint(int j, const GeneralCamera& p) {
boost::shared_ptr<CameraConstraint> factor(new CameraConstraint(j, p)); boost::shared_ptr<CameraConstraint> factor(new CameraConstraint(Symbol('x',j), p));
push_back(factor); push_back(factor);
} }
void addPoint3Constraint(int j, const Point3& p) { void addPoint3Constraint(int j, const Point3& p) {
boost::shared_ptr<Point3Constraint> factor(new Point3Constraint(j, p)); boost::shared_ptr<Point3Constraint> factor(new Point3Constraint(Symbol('x',j), p));
push_back(factor); push_back(factor);
} }
@ -76,7 +74,7 @@ TEST( GeneralSFMFactor, 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.);
const int cameraFrameNumber=1, landmarkNumber=1; const Symbol cameraFrameNumber="x1", landmarkNumber="l1";
const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
boost::shared_ptr<Projection> boost::shared_ptr<Projection>
factor1(new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); factor1(new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
@ -90,17 +88,16 @@ TEST( GeneralSFMFactor, equals )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( GeneralSFMFactor, error ) { TEST( GeneralSFMFactor, error ) {
Point2 z(3.,0.); Point2 z(3.,0.);
const int cameraFrameNumber=1, landmarkNumber=1;
const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
boost::shared_ptr<Projection> boost::shared_ptr<Projection>
factor(new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); factor(new Projection(z, sigma, "x1", "l1"));
// For the following configuration, the factor predicts 320,240 // For the following configuration, the factor predicts 320,240
Values values; Values values;
Rot3 R; Rot3 R;
Point3 t1(0,0,-6); Point3 t1(0,0,-6);
Pose3 x1(R,t1); Pose3 x1(R,t1);
values.insert(CameraKey(1), GeneralCamera(x1)); values.insert("x1", GeneralCamera(x1));
Point3 l1; values.insert(PointKey(1), l1); Point3 l1; values.insert("l1", l1);
EXPECT(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(values))); EXPECT(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(values)));
} }

29
gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp
View File

@ -28,26 +28,24 @@ using namespace std;
using namespace gtsam; using namespace gtsam;
typedef PinholeCamera<Cal3Bundler> GeneralCamera; typedef PinholeCamera<Cal3Bundler> GeneralCamera;
typedef TypedSymbol<GeneralCamera, 'x'> CameraKey; typedef GeneralSFMFactor<GeneralCamera, Point3> Projection;
typedef TypedSymbol<Point3, 'l'> PointKey; typedef NonlinearEquality<GeneralCamera> CameraConstraint;
typedef GeneralSFMFactor<CameraKey, PointKey> Projection; typedef NonlinearEquality<Point3> Point3Constraint;
typedef NonlinearEquality<CameraKey> CameraConstraint;
typedef NonlinearEquality<PointKey> Point3Constraint;
/* ************************************************************************* */ /* ************************************************************************* */
class Graph: public NonlinearFactorGraph { class Graph: public NonlinearFactorGraph {
public: public:
void addMeasurement(const CameraKey& i, const PointKey& j, const Point2& z, const SharedNoiseModel& model) { void addMeasurement(const int& i, const int& j, const Point2& z, const SharedNoiseModel& model) {
push_back(boost::make_shared<Projection>(z, model, i, j)); push_back(boost::make_shared<Projection>(z, model, Symbol('x',i), Symbol('l',j)));
} }
void addCameraConstraint(int j, const GeneralCamera& p) { void addCameraConstraint(int j, const GeneralCamera& p) {
boost::shared_ptr<CameraConstraint> factor(new CameraConstraint(j, p)); boost::shared_ptr<CameraConstraint> factor(new CameraConstraint(Symbol('x', j), p));
push_back(factor); push_back(factor);
} }
void addPoint3Constraint(int j, const Point3& p) { void addPoint3Constraint(int j, const Point3& p) {
boost::shared_ptr<Point3Constraint> factor(new Point3Constraint(j, p)); boost::shared_ptr<Point3Constraint> factor(new Point3Constraint(Symbol('l', j), p));
push_back(factor); push_back(factor);
} }
@ -77,7 +75,7 @@ TEST( GeneralSFMFactor, 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.);
const int cameraFrameNumber=1, landmarkNumber=1; const Symbol cameraFrameNumber="x1", landmarkNumber="l1";
const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
boost::shared_ptr<Projection> boost::shared_ptr<Projection>
factor1(new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); factor1(new Projection(z, sigma, cameraFrameNumber, landmarkNumber));
@ -91,17 +89,16 @@ TEST( GeneralSFMFactor, equals )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( GeneralSFMFactor, error ) { TEST( GeneralSFMFactor, error ) {
Point2 z(3.,0.); Point2 z(3.,0.);
const int cameraFrameNumber=1, landmarkNumber=1;
const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); const SharedNoiseModel sigma(noiseModel::Unit::Create(1));
boost::shared_ptr<Projection> boost::shared_ptr<Projection>
factor(new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); factor(new Projection(z, sigma, "x1", "l1"));
// For the following configuration, the factor predicts 320,240 // For the following configuration, the factor predicts 320,240
Values values; Values values;
Rot3 R; Rot3 R;
Point3 t1(0,0,-6); Point3 t1(0,0,-6);
Pose3 x1(R,t1); Pose3 x1(R,t1);
values.insert(CameraKey(1), GeneralCamera(x1)); values.insert("x1", GeneralCamera(x1));
Point3 l1; values.insert(PointKey(1), l1); Point3 l1; values.insert("l1", l1);
EXPECT(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(values))); EXPECT(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(values)));
} }
@ -171,13 +168,13 @@ TEST( GeneralSFMFactor, optimize_defaultK ) {
const double noise = baseline*0.1; const double noise = baseline*0.1;
boost::shared_ptr<Values> values(new Values); boost::shared_ptr<Values> values(new Values);
for ( size_t i = 0 ; i < X.size() ; ++i ) for ( size_t i = 0 ; i < X.size() ; ++i )
values->insert(CameraKey((int)i), X[i]) ; values->insert(Symbol('x',i), X[i]) ;
for ( size_t i = 0 ; i < L.size() ; ++i ) { for ( size_t i = 0 ; i < L.size() ; ++i ) {
Point3 pt(L[i].x()+noise*getGaussian(), Point3 pt(L[i].x()+noise*getGaussian(),
L[i].y()+noise*getGaussian(), L[i].y()+noise*getGaussian(),
L[i].z()+noise*getGaussian()); L[i].z()+noise*getGaussian());
values->insert(PointKey(i), pt) ; values->insert(Symbol('l',i), pt) ;
} }
graph->addCameraConstraint(0, X[0]); graph->addCameraConstraint(0, X[0]);

20
gtsam/slam/tests/testPlanarSLAM.cpp
View File

@ -37,7 +37,7 @@ SharedNoiseModel
/* ************************************************************************* */ /* ************************************************************************* */
TEST( planarSLAM, PriorFactor_equals ) TEST( planarSLAM, PriorFactor_equals )
{ {
planarSLAM::Prior factor1(2, x1, I3), factor2(2, x2, I3); planarSLAM::Prior factor1(PoseKey(2), x1, I3), factor2(PoseKey(2), x2, I3);
EXPECT(assert_equal(factor1, factor1, 1e-5)); EXPECT(assert_equal(factor1, factor1, 1e-5));
EXPECT(assert_equal(factor2, factor2, 1e-5)); EXPECT(assert_equal(factor2, factor2, 1e-5));
EXPECT(assert_inequal(factor1, factor2, 1e-5)); EXPECT(assert_inequal(factor1, factor2, 1e-5));
@ -48,7 +48,7 @@ TEST( planarSLAM, BearingFactor )
{ {
// Create factor // Create factor
Rot2 z = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1 Rot2 z = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1
planarSLAM::Bearing factor(2, 3, z, sigma); planarSLAM::Bearing factor(PoseKey(2), PointKey(3), z, sigma);
// create config // create config
planarSLAM::Values c; planarSLAM::Values c;
@ -64,8 +64,8 @@ TEST( planarSLAM, BearingFactor )
TEST( planarSLAM, BearingFactor_equals ) TEST( planarSLAM, BearingFactor_equals )
{ {
planarSLAM::Bearing planarSLAM::Bearing
factor1(2, 3, Rot2::fromAngle(0.1), sigma), factor1(PoseKey(2), PointKey(3), Rot2::fromAngle(0.1), sigma),
factor2(2, 3, Rot2::fromAngle(2.3), sigma); factor2(PoseKey(2), PointKey(3), Rot2::fromAngle(2.3), sigma);
EXPECT(assert_equal(factor1, factor1, 1e-5)); EXPECT(assert_equal(factor1, factor1, 1e-5));
EXPECT(assert_equal(factor2, factor2, 1e-5)); EXPECT(assert_equal(factor2, factor2, 1e-5));
EXPECT(assert_inequal(factor1, factor2, 1e-5)); EXPECT(assert_inequal(factor1, factor2, 1e-5));
@ -76,7 +76,7 @@ TEST( planarSLAM, RangeFactor )
{ {
// Create factor // Create factor
double z(sqrt(2) - 0.22); // h(x) - z = 0.22 double z(sqrt(2) - 0.22); // h(x) - z = 0.22
planarSLAM::Range factor(2, 3, z, sigma); planarSLAM::Range factor(PoseKey(2), PointKey(3), z, sigma);
// create config // create config
planarSLAM::Values c; planarSLAM::Values c;
@ -91,7 +91,7 @@ TEST( planarSLAM, RangeFactor )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( planarSLAM, RangeFactor_equals ) TEST( planarSLAM, RangeFactor_equals )
{ {
planarSLAM::Range factor1(2, 3, 1.2, sigma), factor2(2, 3, 7.2, sigma); planarSLAM::Range factor1(PoseKey(2), PointKey(3), 1.2, sigma), factor2(PoseKey(2), PointKey(3), 7.2, sigma);
EXPECT(assert_equal(factor1, factor1, 1e-5)); EXPECT(assert_equal(factor1, factor1, 1e-5));
EXPECT(assert_equal(factor2, factor2, 1e-5)); EXPECT(assert_equal(factor2, factor2, 1e-5));
EXPECT(assert_inequal(factor1, factor2, 1e-5)); EXPECT(assert_inequal(factor1, factor2, 1e-5));
@ -103,7 +103,7 @@ TEST( planarSLAM, BearingRangeFactor )
// Create factor // Create factor
Rot2 r = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1 Rot2 r = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1
double b(sqrt(2) - 0.22); // h(x) - z = 0.22 double b(sqrt(2) - 0.22); // h(x) - z = 0.22
planarSLAM::BearingRange factor(2, 3, r, b, sigma2); planarSLAM::BearingRange factor(PoseKey(2), PointKey(3), r, b, sigma2);
// create config // create config
planarSLAM::Values c; planarSLAM::Values c;
@ -119,8 +119,8 @@ TEST( planarSLAM, BearingRangeFactor )
TEST( planarSLAM, BearingRangeFactor_equals ) TEST( planarSLAM, BearingRangeFactor_equals )
{ {
planarSLAM::BearingRange planarSLAM::BearingRange
factor1(2, 3, Rot2::fromAngle(0.1), 7.3, sigma2), factor1(PoseKey(2), PointKey(3), Rot2::fromAngle(0.1), 7.3, sigma2),
factor2(2, 3, Rot2::fromAngle(3), 2.0, sigma2); factor2(PoseKey(2), PointKey(3), Rot2::fromAngle(3), 2.0, sigma2);
EXPECT(assert_equal(factor1, factor1, 1e-5)); EXPECT(assert_equal(factor1, factor1, 1e-5));
EXPECT(assert_equal(factor2, factor2, 1e-5)); EXPECT(assert_equal(factor2, factor2, 1e-5));
EXPECT(assert_inequal(factor1, factor2, 1e-5)); EXPECT(assert_inequal(factor1, factor2, 1e-5));
@ -129,7 +129,7 @@ TEST( planarSLAM, BearingRangeFactor_equals )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( planarSLAM, PoseConstraint_equals ) TEST( planarSLAM, PoseConstraint_equals )
{ {
planarSLAM::Constraint factor1(2, x2), factor2(2, x3); planarSLAM::Constraint factor1(PoseKey(2), x2), factor2(PoseKey(2), x3);
EXPECT(assert_equal(factor1, factor1, 1e-5)); EXPECT(assert_equal(factor1, factor1, 1e-5));
EXPECT(assert_equal(factor2, factor2, 1e-5)); EXPECT(assert_equal(factor2, factor2, 1e-5));
EXPECT(assert_inequal(factor1, factor2, 1e-5)); EXPECT(assert_inequal(factor1, factor2, 1e-5));

63
gtsam/slam/tests/testPose2SLAM.cpp
View File

@ -34,6 +34,7 @@ using namespace boost::assign;
using namespace std; using namespace std;
typedef pose2SLAM::Odometry Pose2Factor; typedef pose2SLAM::Odometry Pose2Factor;
using pose2SLAM::PoseKey;
// common measurement covariance // common measurement covariance
static double sx=0.5, sy=0.5,st=0.1; static double sx=0.5, sy=0.5,st=0.1;
@ -49,7 +50,7 @@ static noiseModel::Gaussian::shared_ptr covariance(
// Test constraint, small displacement // Test constraint, small displacement
Vector f1(const Pose2& pose1, const Pose2& pose2) { Vector f1(const Pose2& pose1, const Pose2& pose2) {
Pose2 z(2.1130913087, 0.468461064817, 0.436332312999); Pose2 z(2.1130913087, 0.468461064817, 0.436332312999);
Pose2Factor constraint(1, 2, z, covariance); Pose2Factor constraint(PoseKey(1), PoseKey(2), z, covariance);
return constraint.evaluateError(pose1, pose2); return constraint.evaluateError(pose1, pose2);
} }
@ -57,7 +58,7 @@ TEST( Pose2SLAM, constraint1 )
{ {
// create a factor between unknown poses p1 and p2 // create a factor between unknown poses p1 and p2
Pose2 pose1, pose2(2.1130913087, 0.468461064817, 0.436332312999); Pose2 pose1, pose2(2.1130913087, 0.468461064817, 0.436332312999);
Pose2Factor constraint(1, 2, pose2, covariance); Pose2Factor constraint(PoseKey(1), PoseKey(2), pose2, covariance);
Matrix H1, H2; Matrix H1, H2;
Vector actual = constraint.evaluateError(pose1, pose2, H1, H2); Vector actual = constraint.evaluateError(pose1, pose2, H1, H2);
@ -72,7 +73,7 @@ TEST( Pose2SLAM, constraint1 )
// Test constraint, large displacement // Test constraint, large displacement
Vector f2(const Pose2& pose1, const Pose2& pose2) { Vector f2(const Pose2& pose1, const Pose2& pose2) {
Pose2 z(2,2,M_PI_2); Pose2 z(2,2,M_PI_2);
Pose2Factor constraint(1, 2, z, covariance); Pose2Factor constraint(PoseKey(1), PoseKey(2), z, covariance);
return constraint.evaluateError(pose1, pose2); return constraint.evaluateError(pose1, pose2);
} }
@ -80,7 +81,7 @@ TEST( Pose2SLAM, constraint2 )
{ {
// create a factor between unknown poses p1 and p2 // create a factor between unknown poses p1 and p2
Pose2 pose1, pose2(2,2,M_PI_2); Pose2 pose1, pose2(2,2,M_PI_2);
Pose2Factor constraint(1, 2, pose2, covariance); Pose2Factor constraint(PoseKey(1), PoseKey(2), pose2, covariance);
Matrix H1, H2; Matrix H1, H2;
Vector actual = constraint.evaluateError(pose1, pose2, H1, H2); Vector actual = constraint.evaluateError(pose1, pose2, H1, H2);
@ -110,7 +111,7 @@ TEST( Pose2SLAM, linearization )
{ {
// create a factor between unknown poses p1 and p2 // create a factor between unknown poses p1 and p2
Pose2 measured(2,2,M_PI_2); Pose2 measured(2,2,M_PI_2);
Pose2Factor constraint(1,2,measured, covariance); Pose2Factor constraint(PoseKey(1), PoseKey(2), measured, covariance);
pose2SLAM::Graph graph; pose2SLAM::Graph graph;
graph.addOdometry(1,2,measured, covariance); graph.addOdometry(1,2,measured, covariance);
@ -178,8 +179,8 @@ TEST(Pose2Graph, optimize) {
TEST(Pose2Graph, optimizeThreePoses) { TEST(Pose2Graph, optimizeThreePoses) {
// Create a hexagon of poses // Create a hexagon of poses
Values hexagon = pose2SLAM::circle(3,1.0); pose2SLAM::Values hexagon = pose2SLAM::circle(3,1.0);
Pose2 p0 = hexagon[pose2SLAM::PoseKey(0)], p1 = hexagon[pose2SLAM::PoseKey(1)]; Pose2 p0 = hexagon.pose(0), p1 = hexagon.pose(1);
// create a Pose graph with one equality constraint and one measurement // create a Pose graph with one equality constraint and one measurement
shared_ptr<pose2SLAM::Graph> fg(new pose2SLAM::Graph); shared_ptr<pose2SLAM::Graph> fg(new pose2SLAM::Graph);
@ -190,10 +191,10 @@ TEST(Pose2Graph, optimizeThreePoses) {
fg->addOdometry(2, 0, delta, covariance); fg->addOdometry(2, 0, delta, covariance);
// Create initial config // Create initial config
boost::shared_ptr<Values> initial(new Values()); boost::shared_ptr<pose2SLAM::Values> initial(new pose2SLAM::Values());
initial->insert(pose2SLAM::PoseKey(0), p0); initial->insertPose(0, p0);
initial->insert(pose2SLAM::PoseKey(1), hexagon[pose2SLAM::PoseKey(1)].retract(Vector_(3,-0.1, 0.1,-0.1))); initial->insertPose(1, hexagon.pose(1).retract(Vector_(3,-0.1, 0.1,-0.1)));
initial->insert(pose2SLAM::PoseKey(2), hexagon[pose2SLAM::PoseKey(2)].retract(Vector_(3, 0.1,-0.1, 0.1))); initial->insertPose(2, hexagon.pose(2).retract(Vector_(3, 0.1,-0.1, 0.1)));
// Choose an ordering // Choose an ordering
shared_ptr<Ordering> ordering(new Ordering); shared_ptr<Ordering> ordering(new Ordering);
@ -207,7 +208,7 @@ TEST(Pose2Graph, optimizeThreePoses) {
Values actual = *optimizer.values(); Values actual = *optimizer.values();
// Check with ground truth // Check with ground truth
CHECK(assert_equal(hexagon, actual)); CHECK(assert_equal((const Values&)hexagon, actual));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -215,8 +216,8 @@ TEST(Pose2Graph, optimizeThreePoses) {
TEST_UNSAFE(Pose2SLAM, optimizeCircle) { TEST_UNSAFE(Pose2SLAM, optimizeCircle) {
// Create a hexagon of poses // Create a hexagon of poses
Values hexagon = pose2SLAM::circle(6,1.0); pose2SLAM::Values hexagon = pose2SLAM::circle(6,1.0);
Pose2 p0 = hexagon[pose2SLAM::PoseKey(0)], p1 = hexagon[pose2SLAM::PoseKey(1)]; Pose2 p0 = hexagon.pose(0), p1 = hexagon.pose(1);
// create a Pose graph with one equality constraint and one measurement // create a Pose graph with one equality constraint and one measurement
shared_ptr<pose2SLAM::Graph> fg(new pose2SLAM::Graph); shared_ptr<pose2SLAM::Graph> fg(new pose2SLAM::Graph);
@ -230,13 +231,13 @@ TEST_UNSAFE(Pose2SLAM, optimizeCircle) {
fg->addOdometry(5, 0, delta, covariance); fg->addOdometry(5, 0, delta, covariance);
// Create initial config // Create initial config
boost::shared_ptr<Values> initial(new Values()); boost::shared_ptr<pose2SLAM::Values> initial(new pose2SLAM::Values());
initial->insert(pose2SLAM::PoseKey(0), p0); initial->insertPose(0, p0);
initial->insert(pose2SLAM::PoseKey(1), hexagon[pose2SLAM::PoseKey(1)].retract(Vector_(3,-0.1, 0.1,-0.1))); initial->insertPose(1, hexagon.pose(1).retract(Vector_(3,-0.1, 0.1,-0.1)));
initial->insert(pose2SLAM::PoseKey(2), hexagon[pose2SLAM::PoseKey(2)].retract(Vector_(3, 0.1,-0.1, 0.1))); initial->insertPose(2, hexagon.pose(2).retract(Vector_(3, 0.1,-0.1, 0.1)));
initial->insert(pose2SLAM::PoseKey(3), hexagon[pose2SLAM::PoseKey(3)].retract(Vector_(3,-0.1, 0.1,-0.1))); initial->insertPose(3, hexagon.pose(3).retract(Vector_(3,-0.1, 0.1,-0.1)));
initial->insert(pose2SLAM::PoseKey(4), hexagon[pose2SLAM::PoseKey(4)].retract(Vector_(3, 0.1,-0.1, 0.1))); initial->insertPose(4, hexagon.pose(4).retract(Vector_(3, 0.1,-0.1, 0.1)));
initial->insert(pose2SLAM::PoseKey(5), hexagon[pose2SLAM::PoseKey(5)].retract(Vector_(3,-0.1, 0.1,-0.1))); initial->insertPose(5, hexagon.pose(5).retract(Vector_(3,-0.1, 0.1,-0.1)));
// Choose an ordering // Choose an ordering
shared_ptr<Ordering> ordering(new Ordering); shared_ptr<Ordering> ordering(new Ordering);
@ -250,10 +251,10 @@ TEST_UNSAFE(Pose2SLAM, optimizeCircle) {
Values actual = *optimizer.values(); Values actual = *optimizer.values();
// Check with ground truth // Check with ground truth
CHECK(assert_equal(hexagon, actual)); CHECK(assert_equal((const Values&)hexagon, actual));
// Check loop closure // Check loop closure
CHECK(assert_equal(delta,actual[pose2SLAM::PoseKey(5)].between(actual[pose2SLAM::PoseKey(0)]))); CHECK(assert_equal(delta, actual.at<Pose2>(PoseKey(5)).between(actual.at<Pose2>(PoseKey(0)))));
// Pose2SLAMOptimizer myOptimizer("3"); // Pose2SLAMOptimizer myOptimizer("3");
@ -387,10 +388,10 @@ TEST( Pose2Prior, error )
// Choose a linearization point // Choose a linearization point
Pose2 p1(1, 0, 0); // robot at (1,0) Pose2 p1(1, 0, 0); // robot at (1,0)
pose2SLAM::Values x0; pose2SLAM::Values x0;
x0.insert(pose2SLAM::PoseKey(1), p1); x0.insert(PoseKey(1), p1);
// Create factor // Create factor
pose2SLAM::Prior factor(1, p1, sigmas); pose2SLAM::Prior factor(PoseKey(1), p1, sigmas);
// Actual linearization // Actual linearization
Ordering ordering(*x0.orderingArbitrary()); Ordering ordering(*x0.orderingArbitrary());
@ -417,7 +418,7 @@ TEST( Pose2Prior, error )
/* ************************************************************************* */ /* ************************************************************************* */
// common Pose2Prior for tests below // common Pose2Prior for tests below
static gtsam::Pose2 priorVal(2,2,M_PI_2); static gtsam::Pose2 priorVal(2,2,M_PI_2);
static pose2SLAM::Prior priorFactor(1,priorVal, sigmas); static pose2SLAM::Prior priorFactor(PoseKey(1), priorVal, sigmas);
/* ************************************************************************* */ /* ************************************************************************* */
// The error |A*dx-b| approximates (h(x0+dx)-z) = -error_vector // The error |A*dx-b| approximates (h(x0+dx)-z) = -error_vector
@ -431,7 +432,7 @@ TEST( Pose2Prior, linearize )
{ {
// Choose a linearization point at ground truth // Choose a linearization point at ground truth
pose2SLAM::Values x0; pose2SLAM::Values x0;
x0.insert(pose2SLAM::PoseKey(1),priorVal); x0.insertPose(1, priorVal);
// Actual linearization // Actual linearization
Ordering ordering(*x0.orderingArbitrary()); Ordering ordering(*x0.orderingArbitrary());
@ -451,12 +452,12 @@ TEST( Pose2Factor, error )
Pose2 p1; // robot at origin Pose2 p1; // robot at origin
Pose2 p2(1, 0, 0); // robot at (1,0) Pose2 p2(1, 0, 0); // robot at (1,0)
pose2SLAM::Values x0; pose2SLAM::Values x0;
x0.insert(pose2SLAM::PoseKey(1), p1); x0.insertPose(1, p1);
x0.insert(pose2SLAM::PoseKey(2), p2); x0.insertPose(2, p2);
// Create factor // Create factor
Pose2 z = p1.between(p2); Pose2 z = p1.between(p2);
Pose2Factor factor(1, 2, z, covariance); Pose2Factor factor(PoseKey(1), PoseKey(2), z, covariance);
// Actual linearization // Actual linearization
Ordering ordering(*x0.orderingArbitrary()); Ordering ordering(*x0.orderingArbitrary());
@ -483,7 +484,7 @@ TEST( Pose2Factor, error )
/* ************************************************************************* */ /* ************************************************************************* */
// common Pose2Factor for tests below // common Pose2Factor for tests below
static Pose2 measured(2,2,M_PI_2); static Pose2 measured(2,2,M_PI_2);
static Pose2Factor factor(1,2,measured, covariance); static Pose2Factor factor(PoseKey(1),PoseKey(2),measured, covariance);
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Pose2Factor, rhs ) TEST( Pose2Factor, rhs )

22
gtsam/slam/tests/testPose3SLAM.cpp
View File

@ -50,7 +50,7 @@ TEST(Pose3Graph, optimizeCircle) {
// Create a hexagon of poses // Create a hexagon of poses
double radius = 10; double radius = 10;
Values hexagon = pose3SLAM::circle(6,radius); Values hexagon = pose3SLAM::circle(6,radius);
Pose3 gT0 = hexagon[Key(0)], gT1 = hexagon[Key(1)]; Pose3 gT0 = hexagon[PoseKey(0)], gT1 = hexagon[PoseKey(1)];
// create a Pose graph with one equality constraint and one measurement // create a Pose graph with one equality constraint and one measurement
shared_ptr<Pose3Graph> fg(new Pose3Graph); shared_ptr<Pose3Graph> fg(new Pose3Graph);
@ -67,12 +67,12 @@ TEST(Pose3Graph, optimizeCircle) {
// Create initial config // Create initial config
boost::shared_ptr<Values> initial(new Values()); boost::shared_ptr<Values> initial(new Values());
initial->insert(Key(0), gT0); initial->insert(PoseKey(0), gT0);
initial->insert(Key(1), hexagon[Key(1)].retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); initial->insert(PoseKey(1), hexagon.at<Pose3>(PoseKey(1)).retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
initial->insert(Key(2), hexagon[Key(2)].retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1))); initial->insert(PoseKey(2), hexagon.at<Pose3>(PoseKey(2)).retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
initial->insert(Key(3), hexagon[Key(3)].retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); initial->insert(PoseKey(3), hexagon.at<Pose3>(PoseKey(3)).retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
initial->insert(Key(4), hexagon[Key(4)].retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1))); initial->insert(PoseKey(4), hexagon.at<Pose3>(PoseKey(4)).retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
initial->insert(Key(5), hexagon[Key(5)].retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); initial->insert(PoseKey(5), hexagon.at<Pose3>(PoseKey(5)).retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
// Choose an ordering and optimize // Choose an ordering and optimize
shared_ptr<Ordering> ordering(new Ordering); shared_ptr<Ordering> ordering(new Ordering);
@ -87,17 +87,17 @@ TEST(Pose3Graph, optimizeCircle) {
CHECK(assert_equal(hexagon, actual,1e-4)); CHECK(assert_equal(hexagon, actual,1e-4));
// Check loop closure // Check loop closure
CHECK(assert_equal(_0T1,actual[Key(5)].between(actual[Key(0)]),1e-5)); CHECK(assert_equal(_0T1, actual.at<Pose3>(PoseKey(5)).between(actual.at<Pose3>(PoseKey(0))),1e-5));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Pose3Graph, partial_prior_height) { TEST(Pose3Graph, partial_prior_height) {
typedef PartialPriorFactor<pose3SLAM::Key> Partial; typedef PartialPriorFactor<Pose3> Partial;
// reference: Pose3 Expmap - (0-2: Rot3) (3-5: Point3) // reference: Pose3 Expmap - (0-2: Rot3) (3-5: Point3)
// height prior - single element interface // height prior - single element interface
pose3SLAM::Key key(1); Symbol key = PoseKey(1);
double exp_height = 5.0; double exp_height = 5.0;
SharedDiagonal model = noiseModel::Unit::Create(1); SharedDiagonal model = noiseModel::Unit::Create(1);
Pose3 init(Rot3(), Point3(1.0, 2.0, 3.0)), expected(Rot3(), Point3(1.0, 2.0, exp_height)); Pose3 init(Rot3(), Point3(1.0, 2.0, 3.0)), expected(Rot3(), Point3(1.0, 2.0, exp_height));
@ -117,7 +117,7 @@ TEST(Pose3Graph, partial_prior_height) {
EXPECT_DOUBLES_EQUAL(2.0, height.error(values), tol); EXPECT_DOUBLES_EQUAL(2.0, height.error(values), tol);
Values actual = *pose3SLAM::Optimizer::optimizeLM(graph, values); Values actual = *pose3SLAM::Optimizer::optimizeLM(graph, values);
EXPECT(assert_equal(expected, actual[key], tol)); EXPECT(assert_equal(expected, actual.at<Pose3>(key), tol));
EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol); EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol);
} }

6
gtsam/slam/tests/testProjectionFactor.cpp
View File

@ -49,7 +49,7 @@ TEST( ProjectionFactor, error )
Point2 z(323.,240.); Point2 z(323.,240.);
int cameraFrameNumber=1, landmarkNumber=1; int cameraFrameNumber=1, landmarkNumber=1;
boost::shared_ptr<visualSLAM::ProjectionFactor> boost::shared_ptr<visualSLAM::ProjectionFactor>
factor(new visualSLAM::ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK)); factor(new visualSLAM::ProjectionFactor(z, sigma, PoseKey(cameraFrameNumber), PointKey(landmarkNumber), sK));
// For the following values structure, the factor predicts 320,240 // For the following values structure, the factor predicts 320,240
Values config; Values config;
@ -98,10 +98,10 @@ TEST( ProjectionFactor, equals )
Vector z = Vector_(2,323.,240.); Vector z = Vector_(2,323.,240.);
int cameraFrameNumber=1, landmarkNumber=1; int cameraFrameNumber=1, landmarkNumber=1;
boost::shared_ptr<visualSLAM::ProjectionFactor> boost::shared_ptr<visualSLAM::ProjectionFactor>
factor1(new visualSLAM::ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK)); factor1(new visualSLAM::ProjectionFactor(z, sigma, PoseKey(cameraFrameNumber), PointKey(landmarkNumber), sK));
boost::shared_ptr<visualSLAM::ProjectionFactor> boost::shared_ptr<visualSLAM::ProjectionFactor>
factor2(new visualSLAM::ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK)); factor2(new visualSLAM::ProjectionFactor(z, sigma, PoseKey(cameraFrameNumber), PointKey(landmarkNumber), sK));
CHECK(assert_equal(*factor1, *factor2)); CHECK(assert_equal(*factor1, *factor2));
} }

4
gtsam/slam/tests/testStereoFactor.cpp
View File

@ -52,11 +52,11 @@ TEST( StereoFactor, singlePoint)
boost::shared_ptr<Cal3_S2Stereo> K(new Cal3_S2Stereo(625, 625, 0, 320, 240, 0.5)); boost::shared_ptr<Cal3_S2Stereo> K(new Cal3_S2Stereo(625, 625, 0, 320, 240, 0.5));
boost::shared_ptr<visualSLAM::Graph> graph(new visualSLAM::Graph()); boost::shared_ptr<visualSLAM::Graph> graph(new visualSLAM::Graph());
graph->add(visualSLAM::PoseConstraint(1,camera1)); graph->add(visualSLAM::PoseConstraint(PoseKey(1),camera1));
StereoPoint2 z14(320,320.0-50, 240); StereoPoint2 z14(320,320.0-50, 240);
// arguments: measurement, sigma, cam#, measurement #, K, baseline (m) // arguments: measurement, sigma, cam#, measurement #, K, baseline (m)
graph->add(visualSLAM::StereoFactor(z14,sigma, 1, 1, K)); graph->add(visualSLAM::StereoFactor(z14,sigma, PoseKey(1), PointKey(1), K));
// Create a configuration corresponding to the ground truth // Create a configuration corresponding to the ground truth
boost::shared_ptr<Values> values(new Values()); boost::shared_ptr<Values> values(new Values());

68
gtsam/slam/visualSLAM.h
View File

@ -34,22 +34,24 @@ namespace visualSLAM {
using namespace gtsam; using namespace gtsam;
/// Convenience function for constructing a pose key
inline Symbol PoseKey(Index j) { return Symbol('x', j); }
/// Convenience function for constructing a pose key
inline Symbol PointKey(Index j) { return Symbol('l', j); }
/** /**
* Typedefs that make up the visualSLAM namespace. * Typedefs that make up the visualSLAM namespace.
*/ */
typedef TypedSymbol<Pose3,'x'> PoseKey; ///< The key type used for poses typedef NonlinearEquality<Pose3> PoseConstraint; ///< put a hard constraint on a pose
typedef TypedSymbol<Point3,'l'> PointKey; ///< The key type used for points typedef NonlinearEquality<Point3> PointConstraint; ///< put a hard constraint on a point
typedef boost::shared_ptr<Values> shared_values; ///< shared pointer to values data structure typedef PriorFactor<Pose3> PosePrior; ///< put a soft prior on a Pose
typedef PriorFactor<Point3> PointPrior; ///< put a soft prior on a point
typedef NonlinearEquality<PoseKey> PoseConstraint; ///< put a hard constraint on a pose typedef RangeFactor<Pose3, Point3> RangeFactor; ///< put a factor on the range from a pose to a point
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 /// monocular and stereo camera typedefs for general use
typedef GenericProjectionFactor<PointKey, PoseKey> ProjectionFactor; typedef GenericProjectionFactor<Pose3, Point3> ProjectionFactor;
typedef GenericStereoFactor<PoseKey, PointKey> StereoFactor; typedef GenericStereoFactor<Pose3, Point3> StereoFactor;
/** /**
* Non-linear factor graph for vanilla visual SLAM * Non-linear factor graph for vanilla visual SLAM
@ -76,69 +78,69 @@ namespace visualSLAM {
/** /**
* Add a projection factor measurement (monocular) * Add a projection factor measurement (monocular)
* @param z the measurement * @param measured the measurement
* @param model the noise model for the measurement * @param model the noise model for the measurement
* @param i index of camera * @param poseKey variable key for the camera pose
* @param j index of point * @param pointKey variable key for the landmark
* @param K shared pointer to calibration object * @param K shared pointer to calibration object
*/ */
void addMeasurement(const Point2& z, const SharedNoiseModel& model, void addMeasurement(const Point2& measured, const SharedNoiseModel& model,
PoseKey i, PointKey j, const shared_ptrK& K) { const Symbol& poseKey, const Symbol& pointKey, const shared_ptrK& K) {
boost::shared_ptr<ProjectionFactor> factor(new ProjectionFactor(z, model, i, j, K)); boost::shared_ptr<ProjectionFactor> factor(new ProjectionFactor(measured, model, poseKey, pointKey, K));
push_back(factor); push_back(factor);
} }
/** /**
* Add a constraint on a pose (for now, *must* be satisfied in any Values) * Add a constraint on a pose (for now, *must* be satisfied in any Values)
* @param j index of camera * @param key variable key of the camera pose
* @param p to which pose to constrain it to * @param p to which pose to constrain it to
*/ */
void addPoseConstraint(int j, const Pose3& p = Pose3()) { void addPoseConstraint(const Symbol& key, const Pose3& p = Pose3()) {
boost::shared_ptr<PoseConstraint> factor(new PoseConstraint(j, p)); boost::shared_ptr<PoseConstraint> factor(new PoseConstraint(key, p));
push_back(factor); push_back(factor);
} }
/** /**
* Add a constraint on a point (for now, *must* be satisfied in any Values) * Add a constraint on a point (for now, *must* be satisfied in any Values)
* @param j index of landmark * @param key variable key of the landmark
* @param p point around which soft prior is defined * @param p point around which soft prior is defined
*/ */
void addPointConstraint(int j, const Point3& p = Point3()) { void addPointConstraint(const Symbol& key, const Point3& p = Point3()) {
boost::shared_ptr<PointConstraint> factor(new PointConstraint(j, p)); boost::shared_ptr<PointConstraint> factor(new PointConstraint(key, p));
push_back(factor); push_back(factor);
} }
/** /**
* Add a prior on a pose * Add a prior on a pose
* @param j index of camera * @param key variable key of the camera pose
* @param p around which soft prior is defined * @param p around which soft prior is defined
* @param model uncertainty model of this prior * @param model uncertainty model of this prior
*/ */
void addPosePrior(int j, const Pose3& p = Pose3(), const SharedNoiseModel& model = noiseModel::Unit::Create(6)) { void addPosePrior(const Symbol& key, const Pose3& p = Pose3(), const SharedNoiseModel& model = noiseModel::Unit::Create(6)) {
boost::shared_ptr<PosePrior> factor(new PosePrior(j, p, model)); boost::shared_ptr<PosePrior> factor(new PosePrior(key, p, model));
push_back(factor); push_back(factor);
} }
/** /**
* Add a prior on a landmark * Add a prior on a landmark
* @param j index of landmark * @param key variable key of the landmark
* @param p to which point to constrain it to * @param p to which point to constrain it to
* @param model uncertainty model of this prior * @param model uncertainty model of this prior
*/ */
void addPointPrior(int j, const Point3& p = Point3(), const SharedNoiseModel& model = noiseModel::Unit::Create(3)) { void addPointPrior(const Symbol& key, const Point3& p = Point3(), const SharedNoiseModel& model = noiseModel::Unit::Create(3)) {
boost::shared_ptr<PointPrior> factor(new PointPrior(j, p, model)); boost::shared_ptr<PointPrior> factor(new PointPrior(key, p, model));
push_back(factor); push_back(factor);
} }
/** /**
* Add a range prior to a landmark * Add a range prior to a landmark
* @param i index of pose * @param poseKey variable key of the camera pose
* @param j index of landmark * @param pointKey variable key of the landmark
* @param range approximate range to landmark * @param range approximate range to landmark
* @param model uncertainty model of this prior * @param model uncertainty model of this prior
*/ */
void addRangeFactor(PoseKey i, PointKey j, double range, const SharedNoiseModel& model = noiseModel::Unit::Create(1)) { void addRangeFactor(const Symbol& poseKey, const Symbol& pointKey, double range, const SharedNoiseModel& model = noiseModel::Unit::Create(1)) {
push_back(boost::shared_ptr<RangeFactor>(new RangeFactor(i, j, range, model))); push_back(boost::shared_ptr<RangeFactor>(new RangeFactor(poseKey, pointKey, range, model)));
} }
}; // Graph }; // Graph