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

Working on templating factors on Value type instead of key type

release/4.3a0
Richard Roberts 2012-02-06 00:44:25 +00:00
parent cd42dea2c3
commit 811be62ed3
20 changed files with 467 additions and 528 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

304
gtsam/nonlinear/NonlinearFactor.h
View File

@ -77,18 +77,6 @@ public:
NonlinearFactor() { NonlinearFactor() {
} }
/**
* Constructor
* @param keys A boost::tuple containing the variables involved in this factor,
* example: <tt>NonlinearFactor(make_tuple(symbol1, symbol2, symbol3))</tt>
*/
template<class U1, class U2>
NonlinearFactor(const boost::tuples::cons<U1,U2>& keys) {
this->keys_.resize(boost::tuples::length<boost::tuples::cons<U1,U2> >::value);
// Use helper function to fill key vector, using 'cons' representation of tuple
__fill_from_tuple(this->keys(), 0, keys);
}
/** /**
* Constructor * Constructor
* @param keys The variables involved in this factor * @param keys The variables involved in this factor
@ -186,16 +174,6 @@ public:
/** Destructor */ /** Destructor */
virtual ~NoiseModelFactor() {} virtual ~NoiseModelFactor() {}
/**
* Constructor
* @param keys A boost::tuple containing the variables involved in this factor,
* example: <tt>NoiseModelFactor(noiseModel, make_tuple(symbol1, symbol2, symbol3)</tt>
*/
template<class U1, class U2>
NoiseModelFactor(const SharedNoiseModel& noiseModel, const boost::tuples::cons<U1,U2>& keys)
: Base(keys), noiseModel_(noiseModel) {
}
/** /**
* Constructor * Constructor
* @param keys The variables involved in this factor * @param keys The variables involved in this factor
@ -320,21 +298,18 @@ private:
/* ************************************************************************* */ /* ************************************************************************* */
/** A convenient base class for creating your own NoiseModelFactor with 1 /** A convenient base class for creating your own NoiseModelFactor with 1
* variable. To derive from this class, implement evaluateError(). */ * variable. To derive from this class, implement evaluateError(). */
template<class KEY> template<class VALUE>
class NonlinearFactor1: public NoiseModelFactor { class NonlinearFactor1: public NoiseModelFactor {
public: public:
// typedefs for value types pulled from keys // typedefs for value types pulled from keys
typedef typename KEY::Value X; typedef VALUE X;
protected: protected:
// The value of the key. Not const to allow serialization
KEY key_;
typedef NoiseModelFactor Base; typedef NoiseModelFactor Base;
typedef NonlinearFactor1<KEY> This; typedef NonlinearFactor1<VALUE> This;
public: public:
@ -343,22 +318,24 @@ public:
virtual ~NonlinearFactor1() {} virtual ~NonlinearFactor1() {}
inline const KEY& key() const { return key_; } inline const Symbol& key() const { return keys_[0]; }
/** /**
* Constructor * Constructor
* @param z measurement * @param z measurement
* @param key by which to look up X value in Values * @param key by which to look up X value in Values
*/ */
NonlinearFactor1(const SharedNoiseModel& noiseModel, const KEY& key1) : NonlinearFactor1(const SharedNoiseModel& noiseModel, const Symbol& key1) :
Base(noiseModel, make_tuple(key1)), key_(key1) { Base(noiseModel) {
keys_.resize(1);
keys_[0] = key1;
} }
/** Calls the 1-key specific version of evaluateError, which is pure virtual /** Calls the 1-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */ * so must be implemented in the derived class. */
virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const { virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
if(this->active(x)) { if(this->active(x)) {
const X& x1 = x[key_]; const X& x1 = x.at<X>(keys_[0]);
if(H) { if(H) {
return evaluateError(x1, (*H)[0]); return evaluateError(x1, (*H)[0]);
} else { } else {
@ -371,7 +348,7 @@ public:
/** Print */ /** Print */
virtual void print(const std::string& s = "") const { virtual void print(const std::string& s = "") const {
std::cout << s << ": NonlinearFactor1(" << (std::string) this->key_ << ")\n"; std::cout << s << ": NonlinearFactor1(" << (std::string) keys_[0] << ")\n";
this->noiseModel_->print(" noise model: "); this->noiseModel_->print(" noise model: ");
} }
@ -391,7 +368,6 @@ private:
void serialize(ARCHIVE & ar, const unsigned int version) { void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NoiseModelFactor", ar & boost::serialization::make_nvp("NoiseModelFactor",
boost::serialization::base_object<Base>(*this)); boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(key_);
} }
};// \class NonlinearFactor1 };// \class NonlinearFactor1
@ -399,23 +375,19 @@ private:
/* ************************************************************************* */ /* ************************************************************************* */
/** A convenient base class for creating your own NoiseModelFactor with 2 /** A convenient base class for creating your own NoiseModelFactor with 2
* variables. To derive from this class, implement evaluateError(). */ * variables. To derive from this class, implement evaluateError(). */
template<class KEY1, class KEY2> template<class VALUE1, class VALUE2>
class NonlinearFactor2: public NoiseModelFactor { class NonlinearFactor2: public NoiseModelFactor {
public: public:
// typedefs for value types pulled from keys // typedefs for value types pulled from keys
typedef typename KEY1::Value X1; typedef VALUE1 X1;
typedef typename KEY2::Value X2; typedef VALUE2 X2;
protected: protected:
// The values of the keys. Not const to allow serialization
KEY1 key1_;
KEY2 key2_;
typedef NoiseModelFactor Base; typedef NoiseModelFactor Base;
typedef NonlinearFactor2<KEY1, KEY2> This; typedef NonlinearFactor2<VALUE1, VALUE2> This;
public: public:
@ -429,21 +401,25 @@ public:
* @param j1 key of the first variable * @param j1 key of the first variable
* @param j2 key of the second variable * @param j2 key of the second variable
*/ */
NonlinearFactor2(const SharedNoiseModel& noiseModel, const KEY1& j1, const KEY2& j2) : NonlinearFactor2(const SharedNoiseModel& noiseModel, const Symbol& j1, const Symbol& j2) :
Base(noiseModel, make_tuple(j1,j2)), key1_(j1), key2_(j2) {} Base(noiseModel) {
keys_.resize(2);
keys_[0] = j1;
keys_[1] = j2;
}
virtual ~NonlinearFactor2() {} virtual ~NonlinearFactor2() {}
/** methods to retrieve both keys */ /** methods to retrieve both keys */
inline const KEY1& key1() const { return key1_; } inline const Symbol& key1() const { return keys_[0]; }
inline const KEY2& key2() const { return key2_; } inline const Symbol& key2() const { return keys_[1]; }
/** Calls the 2-key specific version of evaluateError, which is pure virtual /** Calls the 2-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */ * so must be implemented in the derived class. */
virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const { virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
if(this->active(x)) { if(this->active(x)) {
const X1& x1 = x[key1_]; const X1& x1 = x.at<X1>(keys_[0]);
const X2& x2 = x[key2_]; const X2& x2 = x.at<X2>(keys_[1]);
if(H) { if(H) {
return evaluateError(x1, x2, (*H)[0], (*H)[1]); return evaluateError(x1, x2, (*H)[0], (*H)[1]);
} else { } else {
@ -457,8 +433,8 @@ public:
/** Print */ /** Print */
virtual void print(const std::string& s = "") const { virtual void print(const std::string& s = "") const {
std::cout << s << ": NonlinearFactor2(" std::cout << s << ": NonlinearFactor2("
<< (std::string) this->key1_ << "," << (std::string) keys_[0] << ","
<< (std::string) this->key2_ << ")\n"; << (std::string) keys_[1] << ")\n";
this->noiseModel_->print(" noise model: "); this->noiseModel_->print(" noise model: ");
} }
@ -479,33 +455,26 @@ private:
void serialize(ARCHIVE & ar, const unsigned int version) { void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NoiseModelFactor", ar & boost::serialization::make_nvp("NoiseModelFactor",
boost::serialization::base_object<Base>(*this)); boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(key1_);
ar & BOOST_SERIALIZATION_NVP(key2_);
} }
}; // \class NonlinearFactor2 }; // \class NonlinearFactor2
/* ************************************************************************* */ /* ************************************************************************* */
/** A convenient base class for creating your own NoiseModelFactor with 3 /** A convenient base class for creating your own NoiseModelFactor with 3
* variables. To derive from this class, implement evaluateError(). */ * variables. To derive from this class, implement evaluateError(). */
template<class KEY1, class KEY2, class KEY3> template<class VALUE1, class VALUE2, class VALUE3>
class NonlinearFactor3: public NoiseModelFactor { class NonlinearFactor3: public NoiseModelFactor {
public: public:
// typedefs for value types pulled from keys // typedefs for value types pulled from keys
typedef typename KEY1::Value X1; typedef VALUE1 X1;
typedef typename KEY2::Value X2; typedef VALUE2 X2;
typedef typename KEY3::Value X3; typedef VALUE3 X3;
protected: protected:
// The values of the keys. Not const to allow serialization
KEY1 key1_;
KEY2 key2_;
KEY3 key3_;
typedef NoiseModelFactor Base; typedef NoiseModelFactor Base;
typedef NonlinearFactor3<KEY1, KEY2, KEY3> This; typedef NonlinearFactor3<VALUE1, VALUE2, VALUE3> This;
public: public:
@ -520,24 +489,29 @@ public:
* @param j2 key of the second variable * @param j2 key of the second variable
* @param j3 key of the third variable * @param j3 key of the third variable
*/ */
NonlinearFactor3(const SharedNoiseModel& noiseModel, const KEY1& j1, const KEY2& j2, const KEY3& j3) : NonlinearFactor3(const SharedNoiseModel& noiseModel, const Symbol& j1, const Symbol& j2, const Symbol& j3) :
Base(noiseModel, make_tuple(j1,j2,j3)), key1_(j1), key2_(j2), key3_(j3) {} Base(noiseModel) {
keys_.resize(3);
keys_[0] = j1;
keys_[1] = j2;
keys_[2] = j3;
}
virtual ~NonlinearFactor3() {} virtual ~NonlinearFactor3() {}
/** methods to retrieve keys */ /** methods to retrieve keys */
inline const KEY1& key1() const { return key1_; } inline const Symbol& key1() const { return keys_[0]; }
inline const KEY2& key2() const { return key2_; } inline const Symbol& key2() const { return keys_[1]; }
inline const KEY3& key3() const { return key3_; } inline const Symbol& key3() const { return keys_[2]; }
/** Calls the 3-key specific version of evaluateError, which is pure virtual /** Calls the 3-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */ * so must be implemented in the derived class. */
virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const { virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
if(this->active(x)) { if(this->active(x)) {
if(H) if(H)
return evaluateError(x[key1_], x[key2_], x[key3_], (*H)[0], (*H)[1], (*H)[2]); return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), (*H)[0], (*H)[1], (*H)[2]);
else else
return evaluateError(x[key1_], x[key2_], x[key3_]); return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]));
} else { } else {
return zero(this->dim()); return zero(this->dim());
} }
@ -546,9 +520,9 @@ public:
/** Print */ /** Print */
virtual void print(const std::string& s = "") const { virtual void print(const std::string& s = "") const {
std::cout << s << ": NonlinearFactor3(" std::cout << s << ": NonlinearFactor3("
<< (std::string) this->key1_ << "," << (std::string) this->keys_[0] << ","
<< (std::string) this->key2_ << "," << (std::string) this->keys_[1] << ","
<< (std::string) this->key3_ << ")\n"; << (std::string) this->keys_[2] << ")\n";
this->noiseModel_->print(" noise model: "); this->noiseModel_->print(" noise model: ");
} }
@ -572,36 +546,27 @@ private:
void serialize(ARCHIVE & ar, const unsigned int version) { void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NoiseModelFactor", ar & boost::serialization::make_nvp("NoiseModelFactor",
boost::serialization::base_object<Base>(*this)); boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(key1_);
ar & BOOST_SERIALIZATION_NVP(key2_);
ar & BOOST_SERIALIZATION_NVP(key3_);
} }
}; // \class NonlinearFactor3 }; // \class NonlinearFactor3
/* ************************************************************************* */ /* ************************************************************************* */
/** A convenient base class for creating your own NoiseModelFactor with 4 /** A convenient base class for creating your own NoiseModelFactor with 4
* variables. To derive from this class, implement evaluateError(). */ * variables. To derive from this class, implement evaluateError(). */
template<class KEY1, class KEY2, class KEY3, class KEY4> template<class VALUE1, class VALUE2, class VALUE3, class VALUE4>
class NonlinearFactor4: public NoiseModelFactor { class NonlinearFactor4: public NoiseModelFactor {
public: public:
// typedefs for value types pulled from keys // typedefs for value types pulled from keys
typedef typename KEY1::Value X1; typedef VALUE1 X1;
typedef typename KEY2::Value X2; typedef VALUE2 X2;
typedef typename KEY3::Value X3; typedef VALUE3 X3;
typedef typename KEY4::Value X4; typedef VALUE4 X4;
protected: protected:
// The values of the keys. Not const to allow serialization
KEY1 key1_;
KEY2 key2_;
KEY3 key3_;
KEY4 key4_;
typedef NoiseModelFactor Base; typedef NoiseModelFactor Base;
typedef NonlinearFactor4<KEY1, KEY2, KEY3, KEY4> This; typedef NonlinearFactor4<VALUE1, VALUE2, VALUE3, VALUE4> This;
public: public:
@ -617,25 +582,31 @@ public:
* @param j3 key of the third variable * @param j3 key of the third variable
* @param j4 key of the fourth variable * @param j4 key of the fourth variable
*/ */
NonlinearFactor4(const SharedNoiseModel& noiseModel, const KEY1& j1, const KEY2& j2, const KEY3& j3, const KEY4& j4) : NonlinearFactor4(const SharedNoiseModel& noiseModel, const Symbol& j1, const Symbol& j2, const Symbol& j3, const Symbol& j4) :
Base(noiseModel, make_tuple(j1,j2,j3,j4)), key1_(j1), key2_(j2), key3_(j3), key4_(j4) {} Base(noiseModel) {
keys_.resize(4);
keys_[0] = j1;
keys_[1] = j2;
keys_[2] = j3;
keys_[3] = j4;
}
virtual ~NonlinearFactor4() {} virtual ~NonlinearFactor4() {}
/** methods to retrieve keys */ /** methods to retrieve keys */
inline const KEY1& key1() const { return key1_; } inline const Symbol& key1() const { return keys_[0]; }
inline const KEY2& key2() const { return key2_; } inline const Symbol& key2() const { return keys_[1]; }
inline const KEY3& key3() const { return key3_; } inline const Symbol& key3() const { return keys_[2]; }
inline const KEY4& key4() const { return key4_; } inline const Symbol& key4() const { return keys_[3]; }
/** Calls the 4-key specific version of evaluateError, which is pure virtual /** Calls the 4-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */ * so must be implemented in the derived class. */
virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const { virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
if(this->active(x)) { if(this->active(x)) {
if(H) if(H)
return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], (*H)[0], (*H)[1], (*H)[2], (*H)[3]); return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), (*H)[0], (*H)[1], (*H)[2], (*H)[3]);
else else
return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_]); return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]));
} else { } else {
return zero(this->dim()); return zero(this->dim());
} }
@ -644,10 +615,10 @@ public:
/** Print */ /** Print */
virtual void print(const std::string& s = "") const { virtual void print(const std::string& s = "") const {
std::cout << s << ": NonlinearFactor4(" std::cout << s << ": NonlinearFactor4("
<< (std::string) this->key1_ << "," << (std::string) this->keys_[0] << ","
<< (std::string) this->key2_ << "," << (std::string) this->keys_[1] << ","
<< (std::string) this->key3_ << "," << (std::string) this->keys_[2] << ","
<< (std::string) this->key4_ << ")\n"; << (std::string) this->keys_[3] << ")\n";
this->noiseModel_->print(" noise model: "); this->noiseModel_->print(" noise model: ");
} }
@ -671,39 +642,28 @@ private:
void serialize(ARCHIVE & ar, const unsigned int version) { void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NoiseModelFactor", ar & boost::serialization::make_nvp("NoiseModelFactor",
boost::serialization::base_object<Base>(*this)); boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(key1_);
ar & BOOST_SERIALIZATION_NVP(key2_);
ar & BOOST_SERIALIZATION_NVP(key3_);
ar & BOOST_SERIALIZATION_NVP(key4_);
} }
}; // \class NonlinearFactor4 }; // \class NonlinearFactor4
/* ************************************************************************* */ /* ************************************************************************* */
/** A convenient base class for creating your own NoiseModelFactor with 5 /** A convenient base class for creating your own NoiseModelFactor with 5
* variables. To derive from this class, implement evaluateError(). */ * variables. To derive from this class, implement evaluateError(). */
template<class KEY1, class KEY2, class KEY3, class KEY4, class KEY5> template<class VALUE1, class VALUE2, class VALUE3, class VALUE4, class VALUE5>
class NonlinearFactor5: public NoiseModelFactor { class NonlinearFactor5: public NoiseModelFactor {
public: public:
// typedefs for value types pulled from keys // typedefs for value types pulled from keys
typedef typename KEY1::Value X1; typedef VALUE1 X1;
typedef typename KEY2::Value X2; typedef VALUE2 X2;
typedef typename KEY3::Value X3; typedef VALUE3 X3;
typedef typename KEY4::Value X4; typedef VALUE4 X4;
typedef typename KEY5::Value X5; typedef VALUE5 X5;
protected: protected:
// The values of the keys. Not const to allow serialization
KEY1 key1_;
KEY2 key2_;
KEY3 key3_;
KEY4 key4_;
KEY5 key5_;
typedef NoiseModelFactor Base; typedef NoiseModelFactor Base;
typedef NonlinearFactor5<KEY1, KEY2, KEY3, KEY4, KEY5> This; typedef NonlinearFactor5<VALUE1, VALUE2, VALUE3, VALUE4, VALUE5> This;
public: public:
@ -720,26 +680,33 @@ public:
* @param j4 key of the fourth variable * @param j4 key of the fourth variable
* @param j5 key of the fifth variable * @param j5 key of the fifth variable
*/ */
NonlinearFactor5(const SharedNoiseModel& noiseModel, const KEY1& j1, const KEY2& j2, const KEY3& j3, const KEY4& j4, const KEY5& j5) : NonlinearFactor5(const SharedNoiseModel& noiseModel, const Symbol& j1, const Symbol& j2, const Symbol& j3, const Symbol& j4, const Symbol& j5) :
Base(noiseModel, make_tuple(j1,j2,j3,j4,j5)), key1_(j1), key2_(j2), key3_(j3), key4_(j4), key5_(j5) {} Base(noiseModel) {
keys_.resize(5);
keys_[0] = j1;
keys_[1] = j2;
keys_[2] = j3;
keys_[3] = j4;
keys_[4] = j5;
}
virtual ~NonlinearFactor5() {} virtual ~NonlinearFactor5() {}
/** methods to retrieve keys */ /** methods to retrieve keys */
inline const KEY1& key1() const { return key1_; } inline const Symbol& key1() const { return keys_[0]; }
inline const KEY2& key2() const { return key2_; } inline const Symbol& key2() const { return keys_[1]; }
inline const KEY3& key3() const { return key3_; } inline const Symbol& key3() const { return keys_[2]; }
inline const KEY4& key4() const { return key4_; } inline const Symbol& key4() const { return keys_[3]; }
inline const KEY5& key5() const { return key5_; } inline const Symbol& key5() const { return keys_[4]; }
/** Calls the 5-key specific version of evaluateError, which is pure virtual /** Calls the 5-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */ * so must be implemented in the derived class. */
virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const { virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
if(this->active(x)) { if(this->active(x)) {
if(H) if(H)
return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_], (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4]); return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]), (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4]);
else else
return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_]); return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]));
} else { } else {
return zero(this->dim()); return zero(this->dim());
} }
@ -748,11 +715,11 @@ public:
/** Print */ /** Print */
virtual void print(const std::string& s = "") const { virtual void print(const std::string& s = "") const {
std::cout << s << ": NonlinearFactor5(" std::cout << s << ": NonlinearFactor5("
<< (std::string) this->key1_ << "," << (std::string) this->keys_[0] << ","
<< (std::string) this->key2_ << "," << (std::string) this->keys_[1] << ","
<< (std::string) this->key3_ << "," << (std::string) this->keys_[2] << ","
<< (std::string) this->key4_ << "," << (std::string) this->keys_[3] << ","
<< (std::string) this->key5_ << ")\n"; << (std::string) this->keys_[4] << ")\n";
this->noiseModel_->print(" noise model: "); this->noiseModel_->print(" noise model: ");
} }
@ -777,42 +744,29 @@ private:
void serialize(ARCHIVE & ar, const unsigned int version) { void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NoiseModelFactor", ar & boost::serialization::make_nvp("NoiseModelFactor",
boost::serialization::base_object<Base>(*this)); boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(key1_);
ar & BOOST_SERIALIZATION_NVP(key2_);
ar & BOOST_SERIALIZATION_NVP(key3_);
ar & BOOST_SERIALIZATION_NVP(key4_);
ar & BOOST_SERIALIZATION_NVP(key5_);
} }
}; // \class NonlinearFactor5 }; // \class NonlinearFactor5
/* ************************************************************************* */ /* ************************************************************************* */
/** A convenient base class for creating your own NoiseModelFactor with 6 /** A convenient base class for creating your own NoiseModelFactor with 6
* variables. To derive from this class, implement evaluateError(). */ * variables. To derive from this class, implement evaluateError(). */
template<class KEY1, class KEY2, class KEY3, class KEY4, class KEY5, class KEY6> template<class VALUE1, class VALUE2, class VALUE3, class VALUE4, class VALUE5, class VALUE6>
class NonlinearFactor6: public NoiseModelFactor { class NonlinearFactor6: public NoiseModelFactor {
public: public:
// typedefs for value types pulled from keys // typedefs for value types pulled from keys
typedef typename KEY1::Value X1; typedef VALUE1 X1;
typedef typename KEY2::Value X2; typedef VALUE2 X2;
typedef typename KEY3::Value X3; typedef VALUE3 X3;
typedef typename KEY4::Value X4; typedef VALUE4 X4;
typedef typename KEY5::Value X5; typedef VALUE5 X5;
typedef typename KEY6::Value X6; typedef VALUE6 X6;
protected: protected:
// The values of the keys. Not const to allow serialization
KEY1 key1_;
KEY2 key2_;
KEY3 key3_;
KEY4 key4_;
KEY5 key5_;
KEY6 key6_;
typedef NoiseModelFactor Base; typedef NoiseModelFactor Base;
typedef NonlinearFactor6<KEY1, KEY2, KEY3, KEY4, KEY5, KEY6> This; typedef NonlinearFactor6<VALUE1, VALUE2, VALUE3, VALUE4, VALUE5, VALUE6> This;
public: public:
@ -830,27 +784,35 @@ public:
* @param j5 key of the fifth variable * @param j5 key of the fifth variable
* @param j6 key of the fifth variable * @param j6 key of the fifth variable
*/ */
NonlinearFactor6(const SharedNoiseModel& noiseModel, const KEY1& j1, const KEY2& j2, const KEY3& j3, const KEY4& j4, const KEY5& j5, const KEY6& j6) : NonlinearFactor6(const SharedNoiseModel& noiseModel, const Symbol& j1, const Symbol& j2, const Symbol& j3, const Symbol& j4, const Symbol& j5, const Symbol& j6) :
Base(noiseModel, make_tuple(j1,j2,j3,j4,j5,j6)), key1_(j1), key2_(j2), key3_(j3), key4_(j4), key5_(j5), key6_(j6) {} Base(noiseModel) {
keys_.resize(6);
keys_[0] = j1;
keys_[1] = j2;
keys_[2] = j3;
keys_[3] = j4;
keys_[4] = j5;
keys_[5] = j6;
}
virtual ~NonlinearFactor6() {} virtual ~NonlinearFactor6() {}
/** methods to retrieve keys */ /** methods to retrieve keys */
inline const KEY1& key1() const { return key1_; } inline const Symbol& key1() const { return keys_[0]; }
inline const KEY2& key2() const { return key2_; } inline const Symbol& key2() const { return keys_[1]; }
inline const KEY3& key3() const { return key3_; } inline const Symbol& key3() const { return keys_[2]; }
inline const KEY4& key4() const { return key4_; } inline const Symbol& key4() const { return keys_[3]; }
inline const KEY5& key5() const { return key5_; } inline const Symbol& key5() const { return keys_[4]; }
inline const KEY6& key6() const { return key6_; } inline const Symbol& key6() const { return keys_[5]; }
/** Calls the 6-key specific version of evaluateError, which is pure virtual /** Calls the 6-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */ * so must be implemented in the derived class. */
virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const { virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
if(this->active(x)) { if(this->active(x)) {
if(H) if(H)
return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_], x[key6_], (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4], (*H)[5]); return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]), x.at<X6>(keys_[5]), (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4], (*H)[5]);
else else
return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_], x[key6_]); return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]), x.at<X6>(keys_[5]));
} else { } else {
return zero(this->dim()); return zero(this->dim());
} }
@ -859,12 +821,12 @@ public:
/** Print */ /** Print */
virtual void print(const std::string& s = "") const { virtual void print(const std::string& s = "") const {
std::cout << s << ": NonlinearFactor6(" std::cout << s << ": NonlinearFactor6("
<< (std::string) this->key1_ << "," << (std::string) this->keys_[0] << ","
<< (std::string) this->key2_ << "," << (std::string) this->keys_[1] << ","
<< (std::string) this->key3_ << "," << (std::string) this->keys_[2] << ","
<< (std::string) this->key4_ << "," << (std::string) this->keys_[3] << ","
<< (std::string) this->key5_ << "," << (std::string) this->keys_[4] << ","
<< (std::string) this->key6_ << ")\n"; << (std::string) this->keys_[5] << ")\n";
this->noiseModel_->print(" noise model: "); this->noiseModel_->print(" noise model: ");
} }
@ -890,12 +852,6 @@ private:
void serialize(ARCHIVE & ar, const unsigned int version) { void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NoiseModelFactor", ar & boost::serialization::make_nvp("NoiseModelFactor",
boost::serialization::base_object<Base>(*this)); boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(key1_);
ar & BOOST_SERIALIZATION_NVP(key2_);
ar & BOOST_SERIALIZATION_NVP(key3_);
ar & BOOST_SERIALIZATION_NVP(key4_);
ar & BOOST_SERIALIZATION_NVP(key5_);
ar & BOOST_SERIALIZATION_NVP(key6_);
} }
}; // \class NonlinearFactor6 }; // \class NonlinearFactor6

30
gtsam/slam/BearingFactor.h
View File

@ -25,18 +25,18 @@ namespace gtsam {
/** /**
* Binary factor for a bearing measurement * Binary factor for a bearing measurement
*/ */
template<class POSEKEY, class POINTKEY> template<class POSE, class POINT>
class BearingFactor: public NonlinearFactor2<POSEKEY, POINTKEY> { class BearingFactor: public NonlinearFactor2<POSE, POINT> {
private: private:
typedef typename POSEKEY::Value Pose; typedef typename POSE Pose;
typedef typename POSEKEY::Value::Rotation Rot; typedef typename Pose::Rotation Rot;
typedef typename POINTKEY::Value Point; typedef typename POINT Point;
typedef BearingFactor<POSEKEY, POINTKEY> This; typedef BearingFactor<POSE, POINT> This;
typedef NonlinearFactor2<POSEKEY, POINTKEY> Base; typedef NonlinearFactor2<POSE, POINT> Base;
Rot z_; /** measurement */ Rot measured_; /** measurement */
/** concept check by type */ /** concept check by type */
GTSAM_CONCEPT_TESTABLE_TYPE(Rot) GTSAM_CONCEPT_TESTABLE_TYPE(Rot)
@ -48,9 +48,9 @@ namespace gtsam {
BearingFactor() {} BearingFactor() {}
/** primary constructor */ /** primary constructor */
BearingFactor(const POSEKEY& i, const POINTKEY& j, const Rot& z, BearingFactor(const Symbol& poseKey, const Symbol& pointKey, const Rot& measured,
const SharedNoiseModel& model) : const SharedNoiseModel& model) :
Base(model, i, j), z_(z) { Base(model, poseKey, pointKey), measured_(measured) {
} }
virtual ~BearingFactor() {} virtual ~BearingFactor() {}
@ -59,18 +59,18 @@ namespace gtsam {
Vector evaluateError(const Pose& pose, const Point& point, Vector evaluateError(const Pose& pose, const Point& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
Rot hx = pose.bearing(point, H1, H2); Rot hx = pose.bearing(point, H1, H2);
return Rot::Logmap(z_.between(hx)); return Rot::Logmap(measured_.between(hx));
} }
/** return the measured */ /** return the measured */
inline const Rot measured() const { const Rot& measured() const {
return z_; return measured_;
} }
/** equals */ /** equals */
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) && this->z_.equals(e->z_, tol); return e != NULL && Base::equals(*e, tol) && this->measured_.equals(e->measured_, tol);
} }
private: private:
@ -81,7 +81,7 @@ namespace gtsam {
void serialize(ARCHIVE & ar, const unsigned int version) { void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NonlinearFactor2", ar & boost::serialization::make_nvp("NonlinearFactor2",
boost::serialization::base_object<Base>(*this)); boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(z_); ar & BOOST_SERIALIZATION_NVP(measured_);
} }
}; // BearingFactor }; // BearingFactor

40
gtsam/slam/BearingRangeFactor.h
View File

@ -27,20 +27,20 @@ namespace gtsam {
/** /**
* Binary factor for a bearing measurement * Binary factor for a bearing measurement
*/ */
template<class POSEKEY, class POINTKEY> template<class POSE, class POINT>
class BearingRangeFactor: public NonlinearFactor2<POSEKEY, POINTKEY> { class BearingRangeFactor: public NonlinearFactor2<POSE, POINT> {
private: private:
typedef typename POSEKEY::Value Pose; typedef typename POSE Pose;
typedef typename POSEKEY::Value::Rotation Rot; typedef typename POSE::Rotation Rot;
typedef typename POINTKEY::Value Point; typedef typename POINT Point;
typedef BearingRangeFactor<POSEKEY, POINTKEY> This; typedef BearingRangeFactor<POSE, POINT> This;
typedef NonlinearFactor2<POSEKEY, POINTKEY> Base; typedef NonlinearFactor2<POSE, POINT> Base;
// the measurement // the measurement
Rot bearing_; Rot measuredBearing_;
double range_; double measuredRange_;
/** concept check by type */ /** concept check by type */
GTSAM_CONCEPT_TESTABLE_TYPE(Rot) GTSAM_CONCEPT_TESTABLE_TYPE(Rot)
@ -50,9 +50,9 @@ namespace gtsam {
public: public:
BearingRangeFactor() {} /* Default constructor */ BearingRangeFactor() {} /* Default constructor */
BearingRangeFactor(const POSEKEY& i, const POINTKEY& j, const Rot& bearing, const double range, BearingRangeFactor(const Symbol& poseKey, const Symbol& pointKey, const Rot& measuredBearing, const double measuredRange,
const SharedNoiseModel& model) : const SharedNoiseModel& model) :
Base(model, i, j), bearing_(bearing), range_(range) { Base(model, poseKey, pointKey), measuredBearing_(measuredBearing), measuredRange_(measuredRange) {
} }
virtual ~BearingRangeFactor() {} virtual ~BearingRangeFactor() {}
@ -63,7 +63,7 @@ namespace gtsam {
<< (std::string) this->key1_ << "," << (std::string) this->key1_ << ","
<< (std::string) this->key2_ << ")\n"; << (std::string) this->key2_ << ")\n";
bearing_.print(" measured bearing"); bearing_.print(" measured bearing");
std::cout << " measured range: " << range_ << std::endl; std::cout << " measured range: " << measuredRange_ << std::endl;
this->noiseModel_->print(" noise model"); this->noiseModel_->print(" noise model");
} }
@ -71,8 +71,8 @@ namespace gtsam {
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) && return e != NULL && Base::equals(*e, tol) &&
fabs(this->range_ - e->range_) < tol && fabs(this->measuredRange_ - e->measuredRange_) < tol &&
this->bearing_.equals(e->bearing_, tol); this->measuredBearing_.equals(e->measuredBearing_, tol);
} }
/** h(x)-z -> between(z,h(x)) for Rot manifold */ /** h(x)-z -> between(z,h(x)) for Rot manifold */
@ -85,10 +85,10 @@ namespace gtsam {
boost::optional<Matrix&> H22_ = H2 ? boost::optional<Matrix&>(H22) : boost::optional<Matrix&>(); boost::optional<Matrix&> H22_ = H2 ? boost::optional<Matrix&>(H22) : boost::optional<Matrix&>();
Rot y1 = pose.bearing(point, H11_, H12_); Rot y1 = pose.bearing(point, H11_, H12_);
Vector e1 = Rot::Logmap(bearing_.between(y1)); Vector e1 = Rot::Logmap(measuredBearing_.between(y1));
double y2 = pose.range(point, H21_, H22_); double y2 = pose.range(point, H21_, H22_);
Vector e2 = Vector_(1, y2 - range_); Vector e2 = Vector_(1, y2 - measuredRange_);
if (H1) *H1 = gtsam::stack(2, &H11, &H21); if (H1) *H1 = gtsam::stack(2, &H11, &H21);
if (H2) *H2 = gtsam::stack(2, &H12, &H22); if (H2) *H2 = gtsam::stack(2, &H12, &H22);
@ -96,8 +96,8 @@ namespace gtsam {
} }
/** return the measured */ /** return the measured */
inline const std::pair<Rot, double> measured() const { const std::pair<Rot, double> measured() const {
return std::make_pair(bearing_, range_); return std::make_pair(measuredBearing_, measuredRange_);
} }
private: private:
@ -108,8 +108,8 @@ namespace gtsam {
void serialize(ARCHIVE & ar, const unsigned int version) { void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NonlinearFactor2", ar & boost::serialization::make_nvp("NonlinearFactor2",
boost::serialization::base_object<Base>(*this)); boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(bearing_); ar & BOOST_SERIALIZATION_NVP(measuredBearing_);
ar & BOOST_SERIALIZATION_NVP(range_); ar & BOOST_SERIALIZATION_NVP(measuredRange_);
} }
}; // BearingRangeFactor }; // BearingRangeFactor

30
gtsam/slam/BetweenFactor.h
View File

@ -29,15 +29,15 @@ namespace gtsam {
* KEY1::Value is the Lie Group type * KEY1::Value is the Lie Group type
* T is the measurement type, by default the same * T is the measurement type, by default the same
*/ */
template<class KEY1, class T = typename KEY1::Value> template<class VALUE>
class BetweenFactor: public NonlinearFactor2<KEY1, KEY1> { class BetweenFactor: public NonlinearFactor2<VALUE, VALUE> {
private: private:
typedef BetweenFactor<KEY1, T> This; typedef BetweenFactor<VALUE> This;
typedef NonlinearFactor2<KEY1, KEY1> Base; typedef NonlinearFactor2<VALUE, VALUE> Base;
T measured_; /** The measurement */ VALUE measured_; /** The measurement */
/** concept check by type */ /** concept check by type */
GTSAM_CONCEPT_LIE_TYPE(T) GTSAM_CONCEPT_LIE_TYPE(T)
@ -52,7 +52,7 @@ namespace gtsam {
BetweenFactor() {} BetweenFactor() {}
/** Constructor */ /** Constructor */
BetweenFactor(const KEY1& key1, const KEY1& key2, const T& measured, BetweenFactor(const Symbol& key1, const Symbol& key2, const VALUE& measured,
const SharedNoiseModel& model) : const SharedNoiseModel& model) :
Base(model, key1, key2), measured_(measured) { Base(model, key1, key2), measured_(measured) {
} }
@ -88,12 +88,12 @@ namespace gtsam {
} }
/** return the measured */ /** return the measured */
inline const T measured() const { const VALUE& measured() const {
return measured_; return measured_;
} }
/** number of variables attached to this factor */ /** number of variables attached to this factor */
inline std::size_t size() const { std::size_t size() const {
return 2; return 2;
} }
@ -114,16 +114,14 @@ namespace gtsam {
* This constraint requires the underlying type to a Lie type * This constraint requires the underlying type to a Lie type
* *
*/ */
template<class KEY> template<class VALUE>
class BetweenConstraint : public BetweenFactor<KEY> { class BetweenConstraint : public BetweenFactor<VALUE> {
public: public:
typedef boost::shared_ptr<BetweenConstraint<KEY> > shared_ptr; typedef boost::shared_ptr<BetweenConstraint<VALUE> > shared_ptr;
/** Syntactic sugar for constrained version */ /** Syntactic sugar for constrained version */
BetweenConstraint(const typename KEY::Value& measured, BetweenConstraint(const VALUE& measured, const Symbol& key1, const Symbol& key2, double mu = 1000.0) :
const KEY& key1, const KEY& key2, double mu = 1000.0) BetweenFactor<VALUE>(key1, key2, measured, noiseModel::Constrained::All(KEY::Value::Dim(), fabs(mu))) {}
: BetweenFactor<KEY>(key1, key2, measured,
noiseModel::Constrained::All(KEY::Value::Dim(), fabs(mu))) {}
private: private:
@ -132,7 +130,7 @@ namespace gtsam {
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::make_nvp("BetweenFactor", ar & boost::serialization::make_nvp("BetweenFactor",
boost::serialization::base_object<BetweenFactor<KEY> >(*this)); boost::serialization::base_object<BetweenFactor<VALUE> >(*this));
} }
}; // \class BetweenConstraint }; // \class BetweenConstraint

14
gtsam/slam/BoundingConstraint.h
View File

@ -28,16 +28,16 @@ namespace gtsam {
* will need to have its value function implemented to return * will need to have its value function implemented to return
* a scalar for comparison. * a scalar for comparison.
*/ */
template<class KEY> template<class VALUE>
struct BoundingConstraint1: public NonlinearFactor1<KEY> { struct BoundingConstraint1: public NonlinearFactor1<VALUE> {
typedef typename KEY::Value X; typedef VALUE X;
typedef NonlinearFactor1<KEY> Base; typedef NonlinearFactor1<VALUE> Base;
typedef boost::shared_ptr<BoundingConstraint1<KEY> > shared_ptr; typedef boost::shared_ptr<BoundingConstraint1<VALUE> > shared_ptr;
double threshold_; double threshold_;
bool isGreaterThan_; /// flag for greater/less than bool isGreaterThan_; /// flag for greater/less than
BoundingConstraint1(const KEY& key, double threshold, BoundingConstraint1(const Symbol& key, double threshold,
bool isGreaterThan, double mu = 1000.0) : bool isGreaterThan, double mu = 1000.0) :
Base(noiseModel::Constrained::All(1, mu), key), Base(noiseModel::Constrained::All(1, mu), key),
threshold_(threshold), isGreaterThan_(isGreaterThan) { threshold_(threshold), isGreaterThan_(isGreaterThan) {
@ -59,7 +59,7 @@ struct BoundingConstraint1: public NonlinearFactor1<KEY> {
/** active when constraint *NOT* met */ /** active when constraint *NOT* met */
bool active(const Values& c) const { bool active(const Values& c) const {
// note: still active at equality to avoid zigzagging // note: still active at equality to avoid zigzagging
double x = value(c[this->key_]); double x = value(c[this->key()]);
return (isGreaterThan_) ? x <= threshold_ : x >= threshold_; return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
} }

30
gtsam/slam/GeneralSFMFactor.h
View File

@ -29,21 +29,21 @@ namespace gtsam {
/** /**
* Non-linear factor for a constraint derived from a 2D measurement. The calibration is unknown here compared to GenericProjectionFactor * Non-linear factor for a constraint derived from a 2D measurement. The calibration is unknown here compared to GenericProjectionFactor
*/ */
template <class CamK, class LmK> template <class CAMERA, class LANDMARK>
class GeneralSFMFactor: class GeneralSFMFactor:
public NonlinearFactor2<CamK, LmK> { public NonlinearFactor2<CAMERA, LANDMARK> {
protected: protected:
Point2 z_; ///< the 2D measurement Point2 measured_; ///< the 2D measurement
public: public:
typedef typename CamK::Value Cam; ///< typedef for camera type typedef typename CAMERA Cam; ///< typedef for camera type
typedef GeneralSFMFactor<CamK, LmK> Self ; ///< typedef for this object typedef GeneralSFMFactor<CAMERA, LANDMARK> This ; ///< typedef for this object
typedef NonlinearFactor2<CamK, LmK> 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
// shorthand for a smart pointer to a factor // shorthand for a smart pointer to a factor
typedef boost::shared_ptr<GeneralSFMFactor<LmK, CamK> > shared_ptr; typedef boost::shared_ptr<This> shared_ptr;
/** /**
* Constructor * Constructor
@ -52,11 +52,11 @@ 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& z, const SharedNoiseModel& model, const CamK& i, const LmK& j) : Base(model, i, j), z_(z) {} GeneralSFMFactor(const Point2& measured, const SharedNoiseModel& model, const Symbol& cameraKey, const Landmark& landmarkKey) : Base(model, i, j), measured_(measured) {}
GeneralSFMFactor():z_(0.0,0.0) {} ///< default constructor GeneralSFMFactor():measured_(0.0,0.0) {} ///< default constructor
GeneralSFMFactor(const Point2 & p):z_(p) {} ///< constructor that takes a Point2 GeneralSFMFactor(const Point2 & p):measured_(p) {} ///< constructor that takes a Point2
GeneralSFMFactor(double x, double y):z_(x,y) {} ///< constructor that takes doubles x,y to make a Point2 GeneralSFMFactor(double x, double y):measured_(x,y) {} ///< constructor that takes doubles x,y to make a Point2
virtual ~GeneralSFMFactor() {} ///< destructor virtual ~GeneralSFMFactor() {} ///< destructor
@ -73,7 +73,7 @@ namespace gtsam {
* equals * equals
*/ */
bool equals(const GeneralSFMFactor<CamK, LmK> &p, double tol = 1e-9) const { bool equals(const GeneralSFMFactor<CamK, LmK> &p, double tol = 1e-9) const {
return Base::equals(p, tol) && this->z_.equals(p.z_, tol) ; return Base::equals(p, tol) && this->measured_.equals(p.z_, tol) ;
} }
/** h(x)-z */ /** h(x)-z */
@ -83,14 +83,14 @@ namespace gtsam {
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 {
Vector error = z_.localCoordinates(camera.project2(point,H1,H2)); Vector error = measured_.localCoordinates(camera.project2(point,H1,H2));
// gtsam::print(error, "error"); // gtsam::print(error, "error");
return error; return error;
} }
/** return the measured */ /** return the measured */
inline const Point2 measured() const { inline const Point2 measured() const {
return z_; return measured_;
} }
private: private:
@ -98,7 +98,7 @@ namespace gtsam {
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_);
} }
}; };

16
gtsam/slam/PartialPriorFactor.h
View File

@ -38,16 +38,16 @@ namespace gtsam {
* For practical use, it would be good to subclass this factor and have the class type * For practical use, it would be good to subclass this factor and have the class type
* construct the mask. * construct the mask.
*/ */
template<class KEY> template<class VALUE>
class PartialPriorFactor: public NonlinearFactor1<KEY> { class PartialPriorFactor: public NonlinearFactor1<VALUE> {
public: public:
typedef typename KEY::Value T; typedef VALUE T;
protected: protected:
typedef NonlinearFactor1<KEY> Base; typedef NonlinearFactor1<VALUE> Base;
typedef PartialPriorFactor<KEY> This; typedef PartialPriorFactor<VALUE> This;
Vector prior_; ///< measurement on logmap parameters, in compressed form Vector prior_; ///< measurement on logmap parameters, in compressed form
std::vector<size_t> mask_; ///< indices of values to constrain in compressed prior vector std::vector<size_t> mask_; ///< indices of values to constrain in compressed prior vector
@ -60,7 +60,7 @@ namespace gtsam {
* constructor with just minimum requirements for a factor - allows more * constructor with just minimum requirements for a factor - allows more
* computation in the constructor. This should only be used by subclasses * computation in the constructor. This should only be used by subclasses
*/ */
PartialPriorFactor(const KEY& key, const SharedNoiseModel& model) PartialPriorFactor(const Symbol& key, const SharedNoiseModel& model)
: Base(model, key) {} : Base(model, key) {}
public: public:
@ -71,14 +71,14 @@ namespace gtsam {
virtual ~PartialPriorFactor() {} virtual ~PartialPriorFactor() {}
/** Single Element Constructor: acts on a single parameter specified by idx */ /** Single Element Constructor: acts on a single parameter specified by idx */
PartialPriorFactor(const KEY& key, size_t idx, double prior, const SharedNoiseModel& model) : PartialPriorFactor(const Symbol& key, size_t idx, double prior, const SharedNoiseModel& model) :
Base(model, key), prior_(Vector_(1, prior)), mask_(1, idx), H_(zeros(1, T::Dim())) { Base(model, key), prior_(Vector_(1, prior)), mask_(1, idx), H_(zeros(1, T::Dim())) {
assert(model->dim() == 1); assert(model->dim() == 1);
this->fillH(); this->fillH();
} }
/** Indices Constructor: specify the mask with a set of indices */ /** Indices Constructor: specify the mask with a set of indices */
PartialPriorFactor(const KEY& key, const std::vector<size_t>& mask, const Vector& prior, PartialPriorFactor(const Symbol& key, const std::vector<size_t>& mask, const Vector& prior,
const SharedNoiseModel& model) : const SharedNoiseModel& model) :
Base(model, key), prior_(prior), mask_(mask), H_(zeros(mask.size(), T::Dim())) { Base(model, key), prior_(prior), mask_(mask), H_(zeros(mask.size(), T::Dim())) {
assert((size_t)prior_.size() == mask.size()); assert((size_t)prior_.size() == mask.size());

29
gtsam/slam/PriorFactor.h
View File

@ -21,32 +21,30 @@
namespace gtsam { namespace gtsam {
/** /**
* A class for a soft prior on any Lie type * A class for a soft prior on any Value type
* It takes two template parameters:
* Key (typically TypedSymbol) is used to look up T's in a Values
* Values where the T's are stored, typically Values<Key> or a TupleValues<...>
* The Key type is not arbitrary: we need to cast to a Symbol at linearize, so
* a simple type like int will not work
*/ */
template<class KEY> template<class VALUE>
class PriorFactor: public NonlinearFactor1<KEY> { class PriorFactor: public NonlinearFactor1<VALUE> {
public: public:
typedef typename KEY::Value T; typedef VALUE T;
private: private:
typedef NonlinearFactor1<KEY> Base; typedef NonlinearFactor1<VALUE> Base;
T prior_; /** The measurement */ VALUE prior_; /** The measurement */
/** concept check by type */ /** concept check by type */
GTSAM_CONCEPT_TESTABLE_TYPE(T) GTSAM_CONCEPT_TESTABLE_TYPE(T)
public: public:
// shorthand for a smart pointer to a factor /// shorthand for a smart pointer to a factor
typedef typename boost::shared_ptr<PriorFactor> shared_ptr; typedef typename boost::shared_ptr<PriorFactor<VALUE> > shared_ptr;
/// Typedef to this class
typedef PriorFactor<VALUE> This;
/** default constructor - only use for serialization */ /** default constructor - only use for serialization */
PriorFactor() {} PriorFactor() {}
@ -54,8 +52,7 @@ namespace gtsam {
virtual ~PriorFactor() {} virtual ~PriorFactor() {}
/** Constructor */ /** Constructor */
PriorFactor(const KEY& key, const T& prior, PriorFactor(const Symbol& key, const VALUE& prior, const SharedNoiseModel& model) :
const SharedNoiseModel& model) :
Base(model, key), prior_(prior) { Base(model, key), prior_(prior) {
} }
@ -70,7 +67,7 @@ namespace gtsam {
/** equals */ /** equals */
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const PriorFactor<KEY> *e = dynamic_cast<const PriorFactor<KEY>*> (&expected); const This* e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol) && this->prior_.equals(e->prior_, tol); return e != NULL && Base::equals(*e, tol) && this->prior_.equals(e->prior_, tol);
} }

40
gtsam/slam/ProjectionFactor.h
View File

@ -29,21 +29,24 @@ namespace gtsam {
* Non-linear factor for a constraint derived from a 2D measurement. The calibration is known here. * Non-linear factor for a constraint derived from a 2D measurement. The calibration is known here.
* i.e. the main building block for visual SLAM. * i.e. the main building block for visual SLAM.
*/ */
template<class LMK, class POSK> template<class POSE, class LANDMARK>
class GenericProjectionFactor: public NonlinearFactor2<POSK, LMK> { class GenericProjectionFactor: public NonlinearFactor2<POSE, LANDMARK> {
protected: protected:
// Keep a copy of measurement and calibration for I/O // Keep a copy of measurement and calibration for I/O
Point2 z_; ///< 2D measurement Point2 measured_; ///< 2D measurement
boost::shared_ptr<Cal3_S2> K_; ///< shared pointer to calibration object boost::shared_ptr<Cal3_S2> K_; ///< shared pointer to calibration object
public: public:
/// shorthand for base class type /// shorthand for base class type
typedef NonlinearFactor2<POSK, LMK> Base; typedef NonlinearFactor2<POSE, LANDMARK> Base;
/// shorthand for this class
typedef GenericProjectionFactor<POSE, LANDMARK> This;
/// shorthand for a smart pointer to a factor /// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<GenericProjectionFactor<LMK, POSK> > shared_ptr; typedef boost::shared_ptr<This> shared_ptr;
/// Default constructor /// Default constructor
GenericProjectionFactor() : GenericProjectionFactor() :
@ -52,15 +55,16 @@ namespace gtsam {
/** /**
* Constructor * Constructor
* TODO: Mark argument order standard (keys, measurement, parameters)
* @param z is the 2 dimensional location of point in image (the measurement) * @param z is the 2 dimensional location of point in image (the measurement)
* @param model is the standard deviation * @param model is the standard deviation
* @param j_pose is basically the frame number * @param j_pose is basically the frame number
* @param j_landmark is the index of the landmark * @param j_landmark is the index of the landmark
* @param K shared pointer to the constant calibration * @param K shared pointer to the constant calibration
*/ */
GenericProjectionFactor(const Point2& z, const SharedNoiseModel& model, GenericProjectionFactor(const Point2& measured, const SharedNoiseModel& model,
POSK j_pose, LMK j_landmark, const shared_ptrK& K) : const Symbol poseKey, const Symbol& pointKey, const shared_ptrK& K) :
Base(model, j_pose, j_landmark), z_(z), K_(K) { Base(model, poseKey, pointKey), measured_(measured), K_(K) {
} }
/** /**
@ -69,14 +73,13 @@ namespace gtsam {
*/ */
void print(const std::string& s = "ProjectionFactor") const { void print(const std::string& s = "ProjectionFactor") const {
Base::print(s); Base::print(s);
z_.print(s + ".z"); measured_.print(s + ".z");
} }
/// equals /// equals
bool equals(const GenericProjectionFactor<LMK, POSK>& p virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
, double tol = 1e-9) const { const This *e = dynamic_cast<const This*> (&expected);
return Base::equals(p, tol) && this->z_.equals(p.z_, tol) return e && Base::equals(p, tol) && this->measured_.equals(e->z_, tol) && this->K_->equals(*e->K_, tol);
&& this->K_->equals(*p.K_, tol);
} }
/// Evaluate error h(x)-z and optionally derivatives /// Evaluate error h(x)-z and optionally derivatives
@ -84,10 +87,9 @@ namespace gtsam {
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
try { try {
SimpleCamera camera(*K_, pose); SimpleCamera camera(*K_, pose);
Point2 reprojectionError(camera.project(point, H1, H2) - z_); Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
return reprojectionError.vector(); return reprojectionError.vector();
} } 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() <<
@ -97,8 +99,8 @@ namespace gtsam {
} }
/** return the measurement */ /** return the measurement */
inline const Point2 measured() const { const Point2& measured() const {
return z_; return measured_;
} }
private: private:
@ -108,7 +110,7 @@ namespace gtsam {
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_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(z_); ar & BOOST_SERIALIZATION_NVP(measured_);
ar & BOOST_SERIALIZATION_NVP(K_); ar & BOOST_SERIALIZATION_NVP(K_);
} }
}; };

33
gtsam/slam/RangeFactor.h
View File

@ -25,17 +25,17 @@ namespace gtsam {
/** /**
* Binary factor for a range measurement * Binary factor for a range measurement
*/ */
template<class POSEKEY, class POINTKEY> template<class POSE, class POINT>
class RangeFactor: public NonlinearFactor2<POSEKEY, POINTKEY> { class RangeFactor: public NonlinearFactor2<POSE, POINT> {
private: private:
double z_; /** measurement */ double measured_; /** measurement */
typedef RangeFactor<POSEKEY, POINTKEY> This; typedef RangeFactor<POSE, POINT> This;
typedef NonlinearFactor2<POSEKEY, POINTKEY> Base; typedef NonlinearFactor2<POSE, POINT> Base;
typedef typename POSEKEY::Value Pose; typedef typename POSE Pose;
typedef typename POINTKEY::Value Point; typedef typename 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)
@ -44,34 +44,33 @@ namespace gtsam {
RangeFactor() {} /* Default constructor */ RangeFactor() {} /* Default constructor */
RangeFactor(const POSEKEY& i, const POINTKEY& j, double z, RangeFactor(const Symbol& poseKey, const Symbol& pointKey, double measured,
const SharedNoiseModel& model) : const SharedNoiseModel& model) :
Base(model, i, j), z_(z) { Base(model, poseKey, PointKey), measured_(measured) {
} }
virtual ~RangeFactor() {} virtual ~RangeFactor() {}
/** h(x)-z */ /** h(x)-z */
Vector evaluateError(const typename POSEKEY::Value& pose, const typename POINTKEY::Value& point, Vector evaluateError(const Pose& pose, const Point& point, boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
double hx = pose.range(point, H1, H2); double hx = pose.range(point, H1, H2);
return Vector_(1, hx - z_); return Vector_(1, hx - measured_);
} }
/** return the measured */ /** return the measured */
inline double measured() const { double measured() const {
return z_; return measured_;
} }
/** 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->z_ - e->z_) < tol; return e != NULL && Base::equals(*e, tol) && fabs(this->measured_ - e->z_) < tol;
} }
/** print contents */ /** print contents */
void print(const std::string& s="") const { void print(const std::string& s="") const {
Base::print(s + std::string(" range: ") + boost::lexical_cast<std::string>(z_)); Base::print(s + std::string(" range: ") + boost::lexical_cast<std::string>(measured_));
} }
private: private:
@ -82,7 +81,7 @@ namespace gtsam {
void serialize(ARCHIVE & ar, const unsigned int version) { void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NonlinearFactor2", ar & boost::serialization::make_nvp("NonlinearFactor2",
boost::serialization::base_object<Base>(*this)); boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(z_); ar & BOOST_SERIALIZATION_NVP(measured_);
} }
}; // RangeFactor }; // RangeFactor

28
gtsam/slam/planarSLAM.cpp
View File

@ -26,38 +26,34 @@ namespace planarSLAM {
Graph::Graph(const NonlinearFactorGraph& graph) : Graph::Graph(const NonlinearFactorGraph& graph) :
NonlinearFactorGraph(graph) {} NonlinearFactorGraph(graph) {}
void Graph::addPrior(const PoseKey& i, const Pose2& p, void Graph::addPrior(Index i, const Pose2& p, const SharedNoiseModel& model) {
const SharedNoiseModel& model) { sharedFactor factor(new Prior(PoseKey(i), p, model));
sharedFactor factor(new Prior(i, p, model));
push_back(factor); push_back(factor);
} }
void Graph::addPoseConstraint(const PoseKey& i, const Pose2& p) { void Graph::addPoseConstraint(Index i, const Pose2& p) {
sharedFactor factor(new Constraint(i, p)); sharedFactor factor(new Constraint(PoseKey(i), p));
push_back(factor); push_back(factor);
} }
void Graph::addOdometry(const PoseKey& i, const PoseKey& j, const Pose2& z, void Graph::addOdometry(Index i, Index j, const Pose2& odometry, const SharedNoiseModel& model) {
const SharedNoiseModel& model) { sharedFactor factor(new Odometry(PoseKey(i), PointKey(j), odometry, model));
sharedFactor factor(new Odometry(i, j, z, model));
push_back(factor); push_back(factor);
} }
void Graph::addBearing(const PoseKey& i, const PointKey& j, const Rot2& z, void Graph::addBearing(Index i, Index j, const Rot2& z, const SharedNoiseModel& model) {
const SharedNoiseModel& model) { sharedFactor factor(new Bearing(PoseKey(i), PointKey(j), z, model));
sharedFactor factor(new Bearing(i, j, z, model));
push_back(factor); push_back(factor);
} }
void Graph::addRange(const PoseKey& i, const PointKey& j, double z, void Graph::addRange(Index i, Index j, double z, const SharedNoiseModel& model) {
const SharedNoiseModel& model) { sharedFactor factor(new Range(PoseKey(i), PointKey(j), z, model));
sharedFactor factor(new Range(i, j, z, model));
push_back(factor); push_back(factor);
} }
void Graph::addBearingRange(const PoseKey& i, const PointKey& j, const Rot2& z1, void Graph::addBearingRange(Index i, Index j, const Rot2& z1,
double z2, const SharedNoiseModel& model) { double z2, const SharedNoiseModel& model) {
sharedFactor factor(new BearingRange(i, j, z1, z2, model)); sharedFactor factor(new BearingRange(PoseKey(i), PointKey(j), z1, z2, model));
push_back(factor); push_back(factor);
} }

35
gtsam/slam/planarSLAM.h
View File

@ -38,21 +38,25 @@ namespace planarSLAM {
/// Typedef for a PointKey with Point2 data and 'l' symbol /// Typedef for a PointKey with Point2 data and 'l' symbol
typedef TypedSymbol<Point2, 'l'> PointKey; typedef TypedSymbol<Point2, 'l'> PointKey;
/**
/// Convenience function for constructing a pose key
inline Symbol PoseKey(Index j) { return Symbol('x', j); }
/*
* List of typedefs for factors * List of typedefs for factors
*/ */
/// A hard constraint for PoseKeys to enforce particular values /// A hard constraint for PoseKeys to enforce particular values
typedef NonlinearEquality<PoseKey> Constraint; typedef NonlinearEquality<Pose2> Constraint;
/// A prior factor to bias the value of a PoseKey /// A prior factor to bias the value of a PoseKey
typedef PriorFactor<PoseKey> Prior; typedef PriorFactor<Pose2> Prior;
/// A factor between two PoseKeys set with a Pose2 /// A factor between two PoseKeys set with a Pose2
typedef BetweenFactor<PoseKey> Odometry; typedef BetweenFactor<Pose2> Odometry;
/// A factor between a PoseKey and a PointKey to express difference in rotation (set with a Rot2) /// A factor between a PoseKey and a PointKey to express difference in rotation (set with a Rot2)
typedef BearingFactor<PoseKey, PointKey> Bearing; typedef BearingFactor<Pose2, Point2> Bearing;
/// A factor between a PoseKey and a PointKey to express distance between them (set with a double) /// A factor between a PoseKey and a PointKey to express distance between them (set with a double)
typedef RangeFactor<PoseKey, PointKey> Range; typedef RangeFactor<Pose2, Point2> Range;
/// A factor between a PoseKey and a PointKey to express difference in rotation and location /// A factor between a PoseKey and a PointKey to express difference in rotation and location
typedef BearingRangeFactor<PoseKey, PointKey> BearingRange; typedef BearingRangeFactor<Pose2, Point2> BearingRange;
/** Values class, using specific PoseKeys and PointKeys /** Values class, using specific PoseKeys and PointKeys
* Mainly as a convenience for MATLAB wrapper, which does not allow for identically named methods * Mainly as a convenience for MATLAB wrapper, which does not allow for identically named methods
@ -80,7 +84,6 @@ namespace planarSLAM {
void insertPoint(int key, const Point2& point) {insert(PointKey(key), point); } void insertPoint(int key, const Point2& point) {insert(PointKey(key), point); }
}; };
/// Creates a NonlinearFactorGraph with the Values type /// Creates a NonlinearFactorGraph with the Values type
struct Graph: public NonlinearFactorGraph { struct Graph: public NonlinearFactorGraph {
@ -91,26 +94,22 @@ namespace planarSLAM {
Graph(const NonlinearFactorGraph& graph); Graph(const NonlinearFactorGraph& graph);
/// Biases the value of PoseKey key with Pose2 p given a noise model /// Biases the value of PoseKey key with Pose2 p given a noise model
void addPrior(const PoseKey& key, const Pose2& pose, const SharedNoiseModel& noiseModel); void addPrior(Index poseKey, const Pose2& pose, const SharedNoiseModel& noiseModel);
/// Creates a hard constraint to enforce Pose2 p for PoseKey poseKey's value /// Creates a hard constraint to enforce Pose2 p for PoseKey poseKey's value
void addPoseConstraint(const PoseKey& poseKey, const Pose2& pose); void addPoseConstraint(Index poseKey, const Pose2& pose);
/// Creates a factor with a Pose2 between PoseKeys poseKey and pointKey (poseKey.e. an odometry measurement) /// Creates a factor with a Pose2 between PoseKeys poseKey and pointKey (poseKey.e. an odometry measurement)
void addOdometry(const PoseKey& poseKey, const PoseKey& pointKey, const Pose2& odometry, void addOdometry(Index poseKey, Index pointKey, const Pose2& odometry, const SharedNoiseModel& model);
const SharedNoiseModel& model);
/// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in rotation /// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in rotation
void addBearing(const PoseKey& poseKey, const PointKey& pointKey, const Rot2& bearing, void addBearing(Index poseKey, Index pointKey, const Rot2& bearing, const SharedNoiseModel& model);
const SharedNoiseModel& model);
/// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in location /// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in location
void addRange(const PoseKey& poseKey, const PointKey& pointKey, double range, void addRange(Index poseKey, Index pointKey, double range, const SharedNoiseModel& model);
const SharedNoiseModel& model);
/// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in rotation and location /// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in rotation and location
void addBearingRange(const PoseKey& poseKey, const PointKey& pointKey, void addBearingRange(Index poseKey, Index pointKey, const Rot2& bearing, double range, const SharedNoiseModel& model);
const Rot2& bearing, double range, const SharedNoiseModel& model);
/// Optimize /// Optimize
Values optimize(const Values& initialEstimate) { Values optimize(const Values& initialEstimate) {

12
gtsam/slam/pose2SLAM.cpp
View File

@ -35,20 +35,20 @@ namespace pose2SLAM {
} }
/* ************************************************************************* */ /* ************************************************************************* */
void Graph::addPrior(const PoseKey& i, const Pose2& p, void Graph::addPrior(Index i, const Pose2& p,
const SharedNoiseModel& model) { const SharedNoiseModel& model) {
sharedFactor factor(new Prior(i, p, model)); sharedFactor factor(new Prior(PoseKey(i), p, model));
push_back(factor); push_back(factor);
} }
void Graph::addPoseConstraint(const PoseKey& i, const Pose2& p) { void Graph::addPoseConstraint(Index i, const Pose2& p) {
sharedFactor factor(new HardConstraint(i, p)); sharedFactor factor(new HardConstraint(PoseKey(i), p));
push_back(factor); push_back(factor);
} }
void Graph::addOdometry(const PoseKey& i, const PoseKey& j, const Pose2& z, void Graph::addOdometry(Index i, Index j, const Pose2& z,
const SharedNoiseModel& model) { const SharedNoiseModel& model) {
sharedFactor factor(new Odometry(i, j, z, model)); sharedFactor factor(new Odometry(PoseKey(i), PoseKey(j), z, model));
push_back(factor); push_back(factor);
} }

17
gtsam/slam/pose2SLAM.h
View File

@ -33,9 +33,8 @@ namespace pose2SLAM {
using namespace gtsam; using namespace gtsam;
/// Keys with Pose2 and symbol 'x' /// Convenience function for constructing a pose key
typedef TypedSymbol<Pose2, 'x'> PoseKey; inline Symbol PoseKey(Index j) { return Symbol('x', j); }
/// Values class, inherited from Values, using PoseKeys, mainly used as a convenience for MATLAB wrapper /// Values class, inherited from Values, using PoseKeys, mainly used as a convenience for MATLAB wrapper
struct Values: public gtsam::Values { struct Values: public gtsam::Values {
@ -70,11 +69,11 @@ namespace pose2SLAM {
*/ */
/// A hard constraint to enforce a specific value for a pose /// A hard constraint to enforce a specific value for a pose
typedef NonlinearEquality<PoseKey> HardConstraint; typedef NonlinearEquality<Pose2> HardConstraint;
/// A prior factor on a pose with Pose2 data type. /// A prior factor on a pose with Pose2 data type.
typedef PriorFactor<PoseKey> Prior; typedef PriorFactor<Pose2> Prior;
/// A factor to add an odometry measurement between two poses. /// A factor to add an odometry measurement between two poses.
typedef BetweenFactor<PoseKey> Odometry; typedef BetweenFactor<Pose2> Odometry;
/// Graph /// Graph
struct Graph: public NonlinearFactorGraph { struct Graph: public NonlinearFactorGraph {
@ -86,13 +85,13 @@ namespace pose2SLAM {
Graph(const NonlinearFactorGraph& graph); Graph(const NonlinearFactorGraph& graph);
/// Adds a Pose2 prior with a noise model to one of the keys in the nonlinear factor graph /// Adds a Pose2 prior with a noise model to one of the keys in the nonlinear factor graph
void addPrior(const PoseKey& i, const Pose2& p, const SharedNoiseModel& model); void addPrior(Index i, const Pose2& p, const SharedNoiseModel& model);
/// Creates a hard constraint for key i with the given Pose2 p. /// Creates a hard constraint for key i with the given Pose2 p.
void addPoseConstraint(const PoseKey& i, const Pose2& p); void addPoseConstraint(Index i, const Pose2& p);
/// Creates a between factor between keys i and j with a noise model with Pose2 z in the graph /// Creates a between factor between keys i and j with a noise model with Pose2 z in the graph
void addOdometry(const PoseKey& i, const PoseKey& j, const Pose2& z, void addOdometry(Index i, Index j, const Pose2& z,
const SharedNoiseModel& model); const SharedNoiseModel& model);
/// Optimize /// Optimize

14
gtsam/slam/pose3SLAM.cpp
View File

@ -45,20 +45,18 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
void Graph::addPrior(const Key& i, const Pose3& p, void Graph::addPrior(Index i, const Pose3& p, const SharedNoiseModel& model) {
const SharedNoiseModel& model) { sharedFactor factor(new Prior(PoseKey(i), p, model));
sharedFactor factor(new Prior(i, p, model));
push_back(factor); push_back(factor);
} }
void Graph::addConstraint(const Key& i, const Key& j, const Pose3& z, void Graph::addConstraint(Index i, Index j, const Pose3& z, const SharedNoiseModel& model) {
const SharedNoiseModel& model) { sharedFactor factor(new Constraint(PoseKey(i), PoseKey(j), z, model));
sharedFactor factor(new Constraint(i, j, z, model));
push_back(factor); push_back(factor);
} }
void Graph::addHardConstraint(const Key& i, const Pose3& p) { void Graph::addHardConstraint(Index i, const Pose3& p) {
sharedFactor factor(new HardConstraint(i, p)); sharedFactor factor(new HardConstraint(PoseKey(i), p));
push_back(factor); push_back(factor);
} }

18
gtsam/slam/pose3SLAM.h
View File

@ -30,8 +30,8 @@ namespace gtsam {
/// Use pose3SLAM namespace for specific SLAM instance /// Use pose3SLAM namespace for specific SLAM instance
namespace pose3SLAM { namespace pose3SLAM {
/// Creates a Key with data Pose3 and symbol 'x' /// Convenience function for constructing a pose key
typedef TypedSymbol<Pose3, 'x'> Key; inline Symbol PoseKey(Index j) { return Symbol('x', j); }
/** /**
* Create a circle of n 3D poses tangent to circle of radius R, first pose at (R,0) * Create a circle of n 3D poses tangent to circle of radius R, first pose at (R,0)
@ -42,25 +42,23 @@ namespace gtsam {
Values circle(size_t n, double R); Values circle(size_t n, double R);
/// A prior factor on Key with Pose3 data type. /// A prior factor on Key with Pose3 data type.
typedef PriorFactor<Key> Prior; typedef PriorFactor<Pose3> Prior;
/// A factor to put constraints between two factors. /// A factor to put constraints between two factors.
typedef BetweenFactor<Key> Constraint; typedef BetweenFactor<Pose3> Constraint;
/// A hard constraint would enforce that the given key would have the input value in the results. /// A hard constraint would enforce that the given key would have the input value in the results.
typedef NonlinearEquality<Key> HardConstraint; typedef NonlinearEquality<Pose3> HardConstraint;
/// Graph /// Graph
struct Graph: public NonlinearFactorGraph { struct Graph: public NonlinearFactorGraph {
/// Adds a factor between keys of the same type /// Adds a factor between keys of the same type
void addPrior(const Key& i, const Pose3& p, void addPrior(Index i, const Pose3& p, const SharedNoiseModel& model);
const SharedNoiseModel& model);
/// Creates a between factor between keys i and j with a noise model with Pos3 z in the graph /// Creates a between factor between keys i and j with a noise model with Pos3 z in the graph
void addConstraint(const Key& i, const Key& j, const Pose3& z, void addConstraint(Index i, Index j, const Pose3& z, const SharedNoiseModel& model);
const SharedNoiseModel& model);
/// Creates a hard constraint for key i with the given Pose3 p. /// Creates a hard constraint for key i with the given Pose3 p.
void addHardConstraint(const Key& i, const Pose3& p); void addHardConstraint(Index i, const Pose3& p);
}; };
/// Optimizer /// Optimizer

83
gtsam/slam/simulated2D.h
View File

@ -29,8 +29,12 @@ namespace simulated2D {
using namespace gtsam; using namespace gtsam;
// Simulated2D robots have no orientation, just a position // Simulated2D robots have no orientation, just a position
typedef TypedSymbol<Point2, 'x'> PoseKey;
typedef TypedSymbol<Point2, 'l'> PointKey; /// Convenience function for constructing a pose key
inline Symbol PoseKey(Index j) { return Symbol('x', j); }
/// Convenience function for constructing a landmark key
inline Symbol PointKey(Index j) { return Symbol('l', j); }
/** /**
* Custom Values class that holds poses and points, mainly used as a convenience for MATLAB wrapper * Custom Values class that holds poses and points, mainly used as a convenience for MATLAB wrapper
@ -115,24 +119,23 @@ namespace simulated2D {
/** /**
* Unary factor encoding a soft prior on a vector * Unary factor encoding a soft prior on a vector
*/ */
template<class KEY = PoseKey> template<class VALUE = Point2>
class GenericPrior: public NonlinearFactor1<KEY> { class GenericPrior: public NonlinearFactor1<VALUE> {
public: public:
typedef NonlinearFactor1<KEY> Base; ///< base class typedef NonlinearFactor1<VALUE> Base; ///< base class
typedef boost::shared_ptr<GenericPrior<KEY> > shared_ptr; typedef boost::shared_ptr<GenericPrior<VALUE> > shared_ptr;
typedef typename KEY::Value Pose; ///< shortcut to Pose type typedef VALUE Pose; ///< shortcut to Pose type
Pose z_; ///< prior mean Pose measured_; ///< prior mean
/// Create generic prior /// Create generic prior
GenericPrior(const Pose& z, const SharedNoiseModel& model, const KEY& key) : GenericPrior(const Pose& z, const SharedNoiseModel& model, const Symbol& key) :
NonlinearFactor1<KEY>(model, key), z_(z) { Base(model, key), measured_(z) {
} }
/// Return error and optional derivative /// Return error and optional derivative
Vector evaluateError(const Pose& x, boost::optional<Matrix&> H = Vector evaluateError(const Pose& x, boost::optional<Matrix&> H = boost::none) const {
boost::none) const { return (prior(x, H) - measured_).vector();
return (prior(x, H) - z_).vector();
} }
private: private:
@ -146,33 +149,32 @@ namespace simulated2D {
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_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(z_); ar & BOOST_SERIALIZATION_NVP(measured_);
} }
}; };
/** /**
* Binary factor simulating "odometry" between two Vectors * Binary factor simulating "odometry" between two Vectors
*/ */
template<class KEY = PoseKey> template<class VALUE = Pose2>
class GenericOdometry: public NonlinearFactor2<KEY, KEY> { class GenericOdometry: public NonlinearFactor2<VALUE, VALUE> {
public: public:
typedef NonlinearFactor2<KEY, KEY> Base; ///< base class typedef NonlinearFactor2<VALUE, VALUE> Base; ///< base class
typedef boost::shared_ptr<GenericOdometry<KEY> > shared_ptr; typedef boost::shared_ptr<GenericOdometry<VALUE> > shared_ptr;
typedef typename KEY::Value Pose; ///< shortcut to Pose type typedef VALUE Pose; ///< shortcut to Pose type
Pose z_; ///< odometry measurement Pose measured_; ///< odometry measurement
/// Create odometry /// Create odometry
GenericOdometry(const Pose& z, const SharedNoiseModel& model, GenericOdometry(const Pose& measured, const SharedNoiseModel& model, const Symbol& key1, const Symbol& key2) :
const KEY& i1, const KEY& i2) : Base(model, key1, key2), measured_(measured) {
NonlinearFactor2<KEY, KEY>(model, i1, i2), z_(z) {
} }
/// Evaluate error and optionally return derivatives /// Evaluate error and optionally return derivatives
Vector evaluateError(const Pose& x1, const Pose& x2, Vector evaluateError(const Pose& x1, const Pose& 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 (odo(x1, x2, H1, H2) - z_).vector(); return (odo(x1, x2, H1, H2) - measured_).vector();
} }
private: private:
@ -186,34 +188,33 @@ namespace simulated2D {
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_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(z_); ar & BOOST_SERIALIZATION_NVP(measured_);
} }
}; };
/** /**
* Binary factor simulating "measurement" between two Vectors * Binary factor simulating "measurement" between two Vectors
*/ */
template<class XKEY = PoseKey, class LKEY = PointKey> template<class POSE, class LANDMARK>
class GenericMeasurement: public NonlinearFactor2<XKEY, LKEY> { class GenericMeasurement: public NonlinearFactor2<POSE, LANDMARK> {
public: public:
typedef NonlinearFactor2<XKEY, LKEY> Base; ///< base class typedef NonlinearFactor2<POSE, LANDMARK> Base; ///< base class
typedef boost::shared_ptr<GenericMeasurement<XKEY, LKEY> > shared_ptr; typedef boost::shared_ptr<GenericMeasurement<POSE, LANDMARK> > shared_ptr;
typedef typename XKEY::Value Pose; ///< shortcut to Pose type typedef POSE Pose; ///< shortcut to Pose type
typedef typename LKEY::Value Point; ///< shortcut to Point type typedef LANDMARK Landmark; ///< shortcut to Landmark type
Point z_; ///< Measurement Landmark z_; ///< Measurement
/// Create measurement factor /// Create measurement factor
GenericMeasurement(const Point& z, const SharedNoiseModel& model, GenericMeasurement(const Landmark& measured, const SharedNoiseModel& model, const Symbol& poseKey, const Symbol& landmarkKey) :
const XKEY& i, const LKEY& j) : Base(model, poseKey, landmarkKey), measured_(measured) {
NonlinearFactor2<XKEY, LKEY>(model, i, j), z_(z) {
} }
/// Evaluate error and optionally return derivatives /// Evaluate error and optionally return derivatives
Vector evaluateError(const Pose& x1, const Point& x2, Vector evaluateError(const Pose& x1, const Landmark& 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 (mea(x1, x2, H1, H2) - z_).vector(); return (mea(x1, x2, H1, H2) - measured_).vector();
} }
private: private:
@ -227,14 +228,14 @@ namespace simulated2D {
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_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(z_); ar & BOOST_SERIALIZATION_NVP(measured_);
} }
}; };
/** Typedefs for regular use */ /** Typedefs for regular use */
typedef GenericPrior<PoseKey> Prior; typedef GenericPrior<Point2> Prior;
typedef GenericOdometry<PoseKey> Odometry; typedef GenericOdometry<Point2> Odometry;
typedef GenericMeasurement<PoseKey, PointKey> Measurement; typedef GenericMeasurement<Point2, Point2> Measurement;
// Specialization of a graph for this example domain // Specialization of a graph for this example domain
// TODO: add functions to add factor types // TODO: add functions to add factor types

60
gtsam/slam/simulated2DConstraints.h
View File

@ -33,13 +33,13 @@ namespace simulated2D {
namespace equality_constraints { namespace equality_constraints {
/** Typedefs for regular use */ /** Typedefs for regular use */
typedef NonlinearEquality1<PoseKey> UnaryEqualityConstraint; typedef NonlinearEquality1<Point2> UnaryEqualityConstraint;
typedef NonlinearEquality1<PointKey> UnaryEqualityPointConstraint; typedef NonlinearEquality1<Point2> UnaryEqualityPointConstraint;
typedef BetweenConstraint<PoseKey> OdoEqualityConstraint; typedef BetweenConstraint<Point2> OdoEqualityConstraint;
/** Equality between variables */ /** Equality between variables */
typedef NonlinearEquality2<PoseKey> PoseEqualityConstraint; typedef NonlinearEquality2<Point2> PoseEqualityConstraint;
typedef NonlinearEquality2<PointKey> PointEqualityConstraint; typedef NonlinearEquality2<Point2> PointEqualityConstraint;
} // \namespace equality_constraints } // \namespace equality_constraints
@ -47,15 +47,14 @@ namespace simulated2D {
/** /**
* Unary inequality constraint forcing a coordinate to be greater/less than a fixed value (c) * Unary inequality constraint forcing a coordinate to be greater/less than a fixed value (c)
* @tparam VALUES is the values structure for the graph * @tparam VALUE is the value type for the variable constrained, e.g. Pose2, Point3, etc.
* @tparam KEY is the key type for the variable constrained
* @tparam IDX is an index in tangent space to constrain, must be less than KEY::VALUE::Dim() * @tparam IDX is an index in tangent space to constrain, must be less than KEY::VALUE::Dim()
*/ */
template<class KEY, unsigned int IDX> template<class VALUE, unsigned int IDX>
struct ScalarCoordConstraint1: public BoundingConstraint1<KEY> { struct ScalarCoordConstraint1: public BoundingConstraint1<VALUE> {
typedef BoundingConstraint1<KEY> Base; ///< Base class convenience typedef typedef BoundingConstraint1<VALUE> Base; ///< Base class convenience typedef
typedef boost::shared_ptr<ScalarCoordConstraint1<KEY, IDX> > shared_ptr; ///< boost::shared_ptr convenience typedef typedef boost::shared_ptr<ScalarCoordConstraint1<VALUE, IDX> > shared_ptr; ///< boost::shared_ptr convenience typedef
typedef typename KEY::Value Point; ///< Constrained variable type typedef VALUE Point; ///< Constrained variable type
virtual ~ScalarCoordConstraint1() {} virtual ~ScalarCoordConstraint1() {}
@ -66,7 +65,7 @@ namespace simulated2D {
* @param isGreaterThan is a flag to set inequality as greater than or less than * @param isGreaterThan is a flag to set inequality as greater than or less than
* @param mu is the penalty function gain * @param mu is the penalty function gain
*/ */
ScalarCoordConstraint1(const KEY& key, double c, ScalarCoordConstraint1(const Symbol& key, double c,
bool isGreaterThan, double mu = 1000.0) : bool isGreaterThan, double mu = 1000.0) :
Base(key, c, isGreaterThan, mu) { Base(key, c, isGreaterThan, mu) {
} }
@ -94,8 +93,8 @@ namespace simulated2D {
}; };
/** typedefs for use with simulated2D systems */ /** typedefs for use with simulated2D systems */
typedef ScalarCoordConstraint1<PoseKey, 0> PoseXInequality; ///< Simulated2D domain example factor constraining X typedef ScalarCoordConstraint1<Point2, 0> PoseXInequality; ///< Simulated2D domain example factor constraining X
typedef ScalarCoordConstraint1<PoseKey, 1> PoseYInequality; ///< Simulated2D domain example factor constraining Y typedef ScalarCoordConstraint1<Point2, 1> PoseYInequality; ///< Simulated2D domain example factor constraining Y
/** /**
* Trait for distance constraints to provide distance * Trait for distance constraints to provide distance
@ -114,10 +113,10 @@ namespace simulated2D {
* @tparam VALUES is the variable set for the graph * @tparam VALUES is the variable set for the graph
* @tparam KEY is the type of the keys for the variables constrained * @tparam KEY is the type of the keys for the variables constrained
*/ */
template<class KEY> template<class VALUE>
struct MaxDistanceConstraint : public BoundingConstraint2<KEY, KEY> { struct MaxDistanceConstraint : public BoundingConstraint2<VALUE, VALUE> {
typedef BoundingConstraint2<KEY, KEY> Base; ///< Base class for factor typedef BoundingConstraint2<VALUE, VALUE> Base; ///< Base class for factor
typedef typename KEY::Value Point; ///< Type of variable constrained typedef VALUE Point; ///< Type of variable constrained
virtual ~MaxDistanceConstraint() {} virtual ~MaxDistanceConstraint() {}
@ -128,8 +127,8 @@ namespace simulated2D {
* @param range_bound is the maximum range allowed between the variables * @param range_bound is the maximum range allowed between the variables
* @param mu is the gain for the penalty function * @param mu is the gain for the penalty function
*/ */
MaxDistanceConstraint(const KEY& key1, const KEY& key2, double range_bound, double mu = 1000.0) MaxDistanceConstraint(const Symbol& key1, const Symbol& key2, double range_bound, double mu = 1000.0) :
: Base(key1, key2, range_bound, false, mu) {} Base(key1, key2, range_bound, false, mu) {}
/** /**
* computes the range with derivatives * computes the range with derivatives
@ -153,15 +152,14 @@ namespace simulated2D {
/** /**
* Binary inequality constraint forcing a minimum range * Binary inequality constraint forcing a minimum range
* NOTE: this is not a convex function! Be careful with initialization. * NOTE: this is not a convex function! Be careful with initialization.
* @tparam VALUES is the variable set for the graph * @tparam POSE is the type of the pose value constrained
* @tparam XKEY is the type of the pose key constrained * @tparam POINT is the type of the point value constrained
* @tparam PKEY is the type of the point key constrained
*/ */
template<class XKEY, class PKEY> template<class POSE, class POINT>
struct MinDistanceConstraint : public BoundingConstraint2<XKEY, PKEY> { struct MinDistanceConstraint : public BoundingConstraint2<POSE, POINT> {
typedef BoundingConstraint2<XKEY, PKEY> Base; ///< Base class for factor typedef BoundingConstraint2<POSE, POINT> Base; ///< Base class for factor
typedef typename XKEY::Value Pose; ///< Type of pose variable constrained typedef POSE Pose; ///< Type of pose variable constrained
typedef typename PKEY::Value Point; ///< Type of point variable constrained typedef POINT Point; ///< Type of point variable constrained
virtual ~MinDistanceConstraint() {} virtual ~MinDistanceConstraint() {}
@ -172,7 +170,7 @@ namespace simulated2D {
* @param range_bound is the minimum range allowed between the variables * @param range_bound is the minimum range allowed between the variables
* @param mu is the gain for the penalty function * @param mu is the gain for the penalty function
*/ */
MinDistanceConstraint(const XKEY& key1, const PKEY& key2, MinDistanceConstraint(const Symbol& key1, const Symbol& key2,
double range_bound, double mu = 1000.0) double range_bound, double mu = 1000.0)
: Base(key1, key2, range_bound, true, mu) {} : Base(key1, key2, range_bound, true, mu) {}
@ -193,7 +191,7 @@ namespace simulated2D {
} }
}; };
typedef MinDistanceConstraint<PoseKey, PointKey> LandmarkAvoid; ///< Simulated2D domain example factor typedef MinDistanceConstraint<Point2, Point2> LandmarkAvoid; ///< Simulated2D domain example factor
} // \namespace inequality_constraints } // \namespace inequality_constraints

37
gtsam/slam/simulated2DOriented.h
View File

@ -27,9 +27,11 @@ namespace simulated2DOriented {
using namespace gtsam; using namespace gtsam;
// The types that take an oriented pose2 rather than point2 /// Convenience function for constructing a pose key
typedef TypedSymbol<Point2, 'l'> PointKey; inline Symbol PoseKey(Index j) { return Symbol('x', j); }
typedef TypedSymbol<Pose2, 'x'> PoseKey;
/// Convenience function for constructing a landmark key
inline Symbol PointKey(Index j) { return Symbol('l', j); }
/// Specialized Values structure with syntactic sugar for /// Specialized Values structure with syntactic sugar for
/// compatibility with matlab /// compatibility with matlab
@ -75,21 +77,20 @@ namespace simulated2DOriented {
boost::none, boost::optional<Matrix&> H2 = boost::none); boost::none, boost::optional<Matrix&> H2 = boost::none);
/// Unary factor encoding a soft prior on a vector /// Unary factor encoding a soft prior on a vector
template<class Key = PoseKey> template<class VALUE = Pose2>
struct GenericPosePrior: public NonlinearFactor1<Key> { struct GenericPosePrior: public NonlinearFactor1<VALUE> {
Pose2 z_; ///< measurement Pose2 measured_; ///< measurement
/// Create generic pose prior /// Create generic pose prior
GenericPosePrior(const Pose2& z, const SharedNoiseModel& model, GenericPosePrior(const Pose2& measured, const SharedNoiseModel& model, const Symbol& key) :
const Key& key) : NonlinearFactor1<VALUE>(model, key), measured_(measured) {
NonlinearFactor1<Key>(model, key), z_(z) {
} }
/// Evaluate error and optionally derivative /// Evaluate error and optionally derivative
Vector evaluateError(const Pose2& x, boost::optional<Matrix&> H = Vector evaluateError(const Pose2& x, boost::optional<Matrix&> H =
boost::none) const { boost::none) const {
return z_.localCoordinates(prior(x, H)); return measured_.localCoordinates(prior(x, H));
} }
}; };
@ -97,28 +98,28 @@ namespace simulated2DOriented {
/** /**
* Binary factor simulating "odometry" between two Vectors * Binary factor simulating "odometry" between two Vectors
*/ */
template<class KEY = PoseKey> template<class VALUE = Pose2>
struct GenericOdometry: public NonlinearFactor2<KEY, KEY> { struct GenericOdometry: public NonlinearFactor2<VALUE, VALUE> {
Pose2 z_; ///< Between measurement for odometry factor Pose2 measured_; ///< Between measurement for odometry factor
/** /**
* Creates an odometry factor between two poses * Creates an odometry factor between two poses
*/ */
GenericOdometry(const Pose2& z, const SharedNoiseModel& model, GenericOdometry(const Pose2& measured, const SharedNoiseModel& model,
const KEY& i1, const KEY& i2) : const Symbol& i1, const Symbol& i2) :
NonlinearFactor2<KEY, KEY>(model, i1, i2), z_(z) { NonlinearFactor2<VALUE, VALUE>(model, i1, i2), measured_(measured) {
} }
/// Evaluate error and optionally derivative /// Evaluate error and optionally derivative
Vector evaluateError(const Pose2& x1, const Pose2& x2, Vector evaluateError(const Pose2& x1, const Pose2& 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 z_.localCoordinates(odo(x1, x2, H1, H2)); return measured_.localCoordinates(odo(x1, x2, H1, H2));
} }
}; };
typedef GenericOdometry<PoseKey> Odometry; typedef GenericOdometry<Pose2> Odometry;
/// Graph specialization for syntactic sugar use with matlab /// Graph specialization for syntactic sugar use with matlab
class Graph : public NonlinearFactorGraph { class Graph : public NonlinearFactorGraph {

53
tests/testNonlinearFactor.cpp
View File

@ -235,13 +235,10 @@ TEST( NonlinearFactor, linearize_constraint2 )
} }
/* ************************************************************************* */ /* ************************************************************************* */
typedef TypedSymbol<LieVector, 'x'> TestKey; class TestFactor4 : public NonlinearFactor4<LieVector, LieVector, LieVector, LieVector> {
/* ************************************************************************* */
class TestFactor4 : public NonlinearFactor4<TestKey, TestKey, TestKey, TestKey> {
public: public:
typedef NonlinearFactor4<TestKey, TestKey, TestKey, TestKey> Base; typedef NonlinearFactor4<LieVector, LieVector, LieVector, LieVector> Base;
TestFactor4() : Base(sharedSigmas(Vector_(1, 2.0)), 1, 2, 3, 4) {} TestFactor4() : Base(sharedSigmas(Vector_(1, 2.0)), "x1", "x2", "x3", "x4") {}
virtual Vector virtual Vector
evaluateError(const LieVector& x1, const LieVector& x2, const LieVector& x3, const LieVector& x4, evaluateError(const LieVector& x1, const LieVector& x2, const LieVector& x3, const LieVector& x4,
@ -263,13 +260,13 @@ public:
TEST(NonlinearFactor, NonlinearFactor4) { TEST(NonlinearFactor, NonlinearFactor4) {
TestFactor4 tf; TestFactor4 tf;
Values tv; Values tv;
tv.insert(TestKey(1), LieVector(1, 1.0)); tv.insert("x1", LieVector(1, 1.0));
tv.insert(TestKey(2), LieVector(1, 2.0)); tv.insert("x2", LieVector(1, 2.0));
tv.insert(TestKey(3), LieVector(1, 3.0)); tv.insert("x3", LieVector(1, 3.0));
tv.insert(TestKey(4), LieVector(1, 4.0)); tv.insert("x4", LieVector(1, 4.0));
EXPECT(assert_equal(Vector_(1, 10.0), tf.unwhitenedError(tv))); EXPECT(assert_equal(Vector_(1, 10.0), tf.unwhitenedError(tv)));
DOUBLES_EQUAL(25.0/2.0, tf.error(tv), 1e-9); DOUBLES_EQUAL(25.0/2.0, tf.error(tv), 1e-9);
Ordering ordering; ordering += TestKey(1), TestKey(2), TestKey(3), TestKey(4); Ordering ordering; ordering += "x1", "x2", "x3", "x4";
JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv, ordering))); JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv, ordering)));
LONGS_EQUAL(jf.keys()[0], 0); LONGS_EQUAL(jf.keys()[0], 0);
LONGS_EQUAL(jf.keys()[1], 1); LONGS_EQUAL(jf.keys()[1], 1);
@ -283,9 +280,9 @@ TEST(NonlinearFactor, NonlinearFactor4) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
class TestFactor5 : public NonlinearFactor5<TestKey, TestKey, TestKey, TestKey, TestKey> { class TestFactor5 : public NonlinearFactor5<LieVector, LieVector, LieVector, LieVector, LieVector> {
public: public:
typedef NonlinearFactor5<TestKey, TestKey, TestKey, TestKey, TestKey> Base; typedef NonlinearFactor5<LieVector, LieVector, LieVector, LieVector, LieVector> Base;
TestFactor5() : Base(sharedSigmas(Vector_(1, 2.0)), 1, 2, 3, 4, 5) {} TestFactor5() : Base(sharedSigmas(Vector_(1, 2.0)), 1, 2, 3, 4, 5) {}
virtual Vector virtual Vector
@ -310,14 +307,14 @@ public:
TEST(NonlinearFactor, NonlinearFactor5) { TEST(NonlinearFactor, NonlinearFactor5) {
TestFactor5 tf; TestFactor5 tf;
Values tv; Values tv;
tv.insert(TestKey(1), LieVector(1, 1.0)); tv.insert("x1", LieVector(1, 1.0));
tv.insert(TestKey(2), LieVector(1, 2.0)); tv.insert("x2", LieVector(1, 2.0));
tv.insert(TestKey(3), LieVector(1, 3.0)); tv.insert("x3", LieVector(1, 3.0));
tv.insert(TestKey(4), LieVector(1, 4.0)); tv.insert("x4", LieVector(1, 4.0));
tv.insert(TestKey(5), LieVector(1, 5.0)); tv.insert("x5", LieVector(1, 5.0));
EXPECT(assert_equal(Vector_(1, 15.0), tf.unwhitenedError(tv))); EXPECT(assert_equal(Vector_(1, 15.0), tf.unwhitenedError(tv)));
DOUBLES_EQUAL(56.25/2.0, tf.error(tv), 1e-9); DOUBLES_EQUAL(56.25/2.0, tf.error(tv), 1e-9);
Ordering ordering; ordering += TestKey(1), TestKey(2), TestKey(3), TestKey(4), TestKey(5); Ordering ordering; ordering += "x1", "x2", "x3", "x4", "x5";
JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv, ordering))); JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv, ordering)));
LONGS_EQUAL(jf.keys()[0], 0); LONGS_EQUAL(jf.keys()[0], 0);
LONGS_EQUAL(jf.keys()[1], 1); LONGS_EQUAL(jf.keys()[1], 1);
@ -333,9 +330,9 @@ TEST(NonlinearFactor, NonlinearFactor5) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
class TestFactor6 : public NonlinearFactor6<TestKey, TestKey, TestKey, TestKey, TestKey, TestKey> { class TestFactor6 : public NonlinearFactor6<LieVector, LieVector, LieVector, LieVector, LieVector, LieVector> {
public: public:
typedef NonlinearFactor6<TestKey, TestKey, TestKey, TestKey, TestKey, TestKey> Base; typedef NonlinearFactor6<LieVector, LieVector, LieVector, LieVector, LieVector, LieVector> Base;
TestFactor6() : Base(sharedSigmas(Vector_(1, 2.0)), 1, 2, 3, 4, 5, 6) {} TestFactor6() : Base(sharedSigmas(Vector_(1, 2.0)), 1, 2, 3, 4, 5, 6) {}
virtual Vector virtual Vector
@ -362,15 +359,15 @@ public:
TEST(NonlinearFactor, NonlinearFactor6) { TEST(NonlinearFactor, NonlinearFactor6) {
TestFactor6 tf; TestFactor6 tf;
Values tv; Values tv;
tv.insert(TestKey(1), LieVector(1, 1.0)); tv.insert("x1", LieVector(1, 1.0));
tv.insert(TestKey(2), LieVector(1, 2.0)); tv.insert("x2", LieVector(1, 2.0));
tv.insert(TestKey(3), LieVector(1, 3.0)); tv.insert("x3", LieVector(1, 3.0));
tv.insert(TestKey(4), LieVector(1, 4.0)); tv.insert("x4", LieVector(1, 4.0));
tv.insert(TestKey(5), LieVector(1, 5.0)); tv.insert("x5", LieVector(1, 5.0));
tv.insert(TestKey(6), LieVector(1, 6.0)); tv.insert("x6", LieVector(1, 6.0));
EXPECT(assert_equal(Vector_(1, 21.0), tf.unwhitenedError(tv))); EXPECT(assert_equal(Vector_(1, 21.0), tf.unwhitenedError(tv)));
DOUBLES_EQUAL(110.25/2.0, tf.error(tv), 1e-9); DOUBLES_EQUAL(110.25/2.0, tf.error(tv), 1e-9);
Ordering ordering; ordering += TestKey(1), TestKey(2), TestKey(3), TestKey(4), TestKey(5), TestKey(6); Ordering ordering; ordering += "x1", "x2", "x3", "x4", "x5", "x6";
JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv, ordering))); JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv, ordering)));
LONGS_EQUAL(jf.keys()[0], 0); LONGS_EQUAL(jf.keys()[0], 0);
LONGS_EQUAL(jf.keys()[1], 1); LONGS_EQUAL(jf.keys()[1], 1);