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

306
gtsam/nonlinear/NonlinearFactor.h
View File

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

30
gtsam/slam/BearingFactor.h
View File

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

40
gtsam/slam/BearingRangeFactor.h
View File

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

30
gtsam/slam/BetweenFactor.h
View File

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

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
*/
template <class CamK, class LmK>
template <class CAMERA, class LANDMARK>
class GeneralSFMFactor:
public NonlinearFactor2<CamK, LmK> {
public NonlinearFactor2<CAMERA, LANDMARK> {
protected:
Point2 z_; ///< the 2D measurement
Point2 measured_; ///< the 2D measurement
public:
typedef typename CamK::Value Cam; ///< typedef for camera type
typedef GeneralSFMFactor<CamK, LmK> Self ; ///< typedef for this object
typedef NonlinearFactor2<CamK, LmK> Base; ///< typedef for the base class
typedef typename CAMERA Cam; ///< typedef for camera type
typedef GeneralSFMFactor<CAMERA, LANDMARK> This ; ///< typedef for this object
typedef NonlinearFactor2<CAMERA, LANDMARK> Base; ///< typedef for the base class
typedef Point2 Measurement; ///< typedef for the measurement
// 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
@ -52,11 +52,11 @@ namespace gtsam {
* @param i is basically the frame number
* @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(const Point2 & p):z_(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():measured_(0.0,0.0) {} ///< default constructor
GeneralSFMFactor(const Point2 & p):measured_(p) {} ///< constructor that takes a Point2
GeneralSFMFactor(double x, double y):measured_(x,y) {} ///< constructor that takes doubles x,y to make a Point2
virtual ~GeneralSFMFactor() {} ///< destructor
@ -73,7 +73,7 @@ namespace gtsam {
* equals
*/
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 */
@ -83,14 +83,14 @@ namespace gtsam {
boost::optional<Matrix&> H1=boost::none,
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");
return error;
}
/** return the measured */
inline const Point2 measured() const {
return z_;
return measured_;
}
private:
@ -98,7 +98,7 @@ namespace gtsam {
friend class boost::serialization::access;
template<class Archive>
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
* construct the mask.
*/
template<class KEY>
class PartialPriorFactor: public NonlinearFactor1<KEY> {
template<class VALUE>
class PartialPriorFactor: public NonlinearFactor1<VALUE> {
public:
typedef typename KEY::Value T;
typedef VALUE T;
protected:
typedef NonlinearFactor1<KEY> Base;
typedef PartialPriorFactor<KEY> This;
typedef NonlinearFactor1<VALUE> Base;
typedef PartialPriorFactor<VALUE> This;
Vector prior_; ///< measurement on logmap parameters, in compressed form
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
* 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) {}
public:
@ -71,14 +71,14 @@ namespace gtsam {
virtual ~PartialPriorFactor() {}
/** 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())) {
assert(model->dim() == 1);
this->fillH();
}
/** 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) :
Base(model, key), prior_(prior), mask_(mask), H_(zeros(mask.size(), T::Dim())) {
assert((size_t)prior_.size() == mask.size());

29
gtsam/slam/PriorFactor.h
View File

@ -21,32 +21,30 @@
namespace gtsam {
/**
* A class for a soft prior on any Lie 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
* A class for a soft prior on any Value type
*/
template<class KEY>
class PriorFactor: public NonlinearFactor1<KEY> {
template<class VALUE>
class PriorFactor: public NonlinearFactor1<VALUE> {
public:
typedef typename KEY::Value T;
typedef VALUE T;
private:
typedef NonlinearFactor1<KEY> Base;
typedef NonlinearFactor1<VALUE> Base;
T prior_; /** The measurement */
VALUE prior_; /** The measurement */
/** concept check by type */
GTSAM_CONCEPT_TESTABLE_TYPE(T)
public:
// shorthand for a smart pointer to a factor
typedef typename boost::shared_ptr<PriorFactor> shared_ptr;
/// shorthand for a smart pointer to a factor
typedef typename boost::shared_ptr<PriorFactor<VALUE> > shared_ptr;
/// Typedef to this class
typedef PriorFactor<VALUE> This;
/** default constructor - only use for serialization */
PriorFactor() {}
@ -54,8 +52,7 @@ namespace gtsam {
virtual ~PriorFactor() {}
/** Constructor */
PriorFactor(const KEY& key, const T& prior,
const SharedNoiseModel& model) :
PriorFactor(const Symbol& key, const VALUE& prior, const SharedNoiseModel& model) :
Base(model, key), prior_(prior) {
}
@ -70,7 +67,7 @@ namespace gtsam {
/** equals */
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);
}

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.
* i.e. the main building block for visual SLAM.
*/
template<class LMK, class POSK>
class GenericProjectionFactor: public NonlinearFactor2<POSK, LMK> {
template<class POSE, class LANDMARK>
class GenericProjectionFactor: public NonlinearFactor2<POSE, LANDMARK> {
protected:
// 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
public:
/// 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
typedef boost::shared_ptr<GenericProjectionFactor<LMK, POSK> > shared_ptr;
typedef boost::shared_ptr<This> shared_ptr;
/// Default constructor
GenericProjectionFactor() :
@ -52,15 +55,16 @@ namespace gtsam {
/**
* Constructor
* TODO: Mark argument order standard (keys, measurement, parameters)
* @param z is the 2 dimensional location of point in image (the measurement)
* @param model is the standard deviation
* @param j_pose is basically the frame number
* @param j_landmark is the index of the landmark
* @param K shared pointer to the constant calibration
*/
GenericProjectionFactor(const Point2& z, const SharedNoiseModel& model,
POSK j_pose, LMK j_landmark, const shared_ptrK& K) :
Base(model, j_pose, j_landmark), z_(z), K_(K) {
GenericProjectionFactor(const Point2& measured, const SharedNoiseModel& model,
const Symbol poseKey, const Symbol& pointKey, const shared_ptrK& K) :
Base(model, poseKey, pointKey), measured_(measured), K_(K) {
}
/**
@ -69,14 +73,13 @@ namespace gtsam {
*/
void print(const std::string& s = "ProjectionFactor") const {
Base::print(s);
z_.print(s + ".z");
measured_.print(s + ".z");
}
/// equals
bool equals(const GenericProjectionFactor<LMK, POSK>& p
, double tol = 1e-9) const {
return Base::equals(p, tol) && this->z_.equals(p.z_, tol)
&& this->K_->equals(*p.K_, tol);
virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
const This *e = dynamic_cast<const This*> (&expected);
return e && Base::equals(p, tol) && this->measured_.equals(e->z_, tol) && this->K_->equals(*e->K_, tol);
}
/// Evaluate error h(x)-z and optionally derivatives
@ -84,10 +87,9 @@ namespace gtsam {
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
try {
SimpleCamera camera(*K_, pose);
Point2 reprojectionError(camera.project(point, H1, H2) - z_);
Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
return reprojectionError.vector();
}
catch( CheiralityException& e) {
} catch( CheiralityException& e) {
if (H1) *H1 = zeros(2,6);
if (H2) *H2 = zeros(2,3);
cout << e.what() << ": Landmark "<< this->key2_.index() <<
@ -97,8 +99,8 @@ namespace gtsam {
}
/** return the measurement */
inline const Point2 measured() const {
return z_;
const Point2& measured() const {
return measured_;
}
private:
@ -108,7 +110,7 @@ namespace gtsam {
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(z_);
ar & BOOST_SERIALIZATION_NVP(measured_);
ar & BOOST_SERIALIZATION_NVP(K_);
}
};

33
gtsam/slam/RangeFactor.h
View File

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

28
gtsam/slam/planarSLAM.cpp
View File

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

103
gtsam/slam/planarSLAM.h
View File

@ -31,28 +31,32 @@
// Use planarSLAM namespace for specific SLAM instance
namespace planarSLAM {
using namespace gtsam;
using namespace gtsam;
/// Typedef for a PoseKey with Pose2 data and 'x' symbol
typedef TypedSymbol<Pose2, 'x'> PoseKey;
/// Typedef for a PoseKey with Pose2 data and 'x' symbol
typedef TypedSymbol<Pose2, 'x'> PoseKey;
/// Typedef for a PointKey with Point2 data and 'l' symbol
typedef TypedSymbol<Point2, 'l'> PointKey;
/**
* List of typedefs for factors
*/
/// A hard constraint for PoseKeys to enforce particular values
typedef NonlinearEquality<PoseKey> Constraint;
/// A prior factor to bias the value of a PoseKey
typedef PriorFactor<PoseKey> Prior;
/// A factor between two PoseKeys set with a Pose2
typedef BetweenFactor<PoseKey> Odometry;
/// A factor between a PoseKey and a PointKey to express difference in rotation (set with a Rot2)
typedef BearingFactor<PoseKey, PointKey> Bearing;
/// A factor between a PoseKey and a PointKey to express distance between them (set with a double)
typedef RangeFactor<PoseKey, PointKey> Range;
/// A factor between a PoseKey and a PointKey to express difference in rotation and location
typedef BearingRangeFactor<PoseKey, PointKey> BearingRange;
/// Typedef for a PointKey with Point2 data and 'l' symbol
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
*/
/// A hard constraint for PoseKeys to enforce particular values
typedef NonlinearEquality<Pose2> Constraint;
/// A prior factor to bias the value of a PoseKey
typedef PriorFactor<Pose2> Prior;
/// A factor between two PoseKeys set with a Pose2
typedef BetweenFactor<Pose2> Odometry;
/// A factor between a PoseKey and a PointKey to express difference in rotation (set with a Rot2)
typedef BearingFactor<Pose2, Point2> Bearing;
/// A factor between a PoseKey and a PointKey to express distance between them (set with a double)
typedef RangeFactor<Pose2, Point2> Range;
/// A factor between a PoseKey and a PointKey to express difference in rotation and location
typedef BearingRangeFactor<Pose2, Point2> BearingRange;
/** Values class, using specific PoseKeys and PointKeys
* Mainly as a convenience for MATLAB wrapper, which does not allow for identically named methods
@ -80,48 +84,43 @@ namespace planarSLAM {
void insertPoint(int key, const Point2& point) {insert(PointKey(key), point); }
};
/// Creates a NonlinearFactorGraph with the Values type
struct Graph: public NonlinearFactorGraph {
/// Creates a NonlinearFactorGraph with the Values type
struct Graph: public NonlinearFactorGraph {
/// Default constructor for a NonlinearFactorGraph
Graph(){}
/// Default constructor for a NonlinearFactorGraph
Graph(){}
/// Creates a NonlinearFactorGraph based on another NonlinearFactorGraph
Graph(const NonlinearFactorGraph& graph);
/// Creates a NonlinearFactorGraph based on another NonlinearFactorGraph
Graph(const NonlinearFactorGraph& graph);
/// Biases the value of PoseKey key with Pose2 p given a noise model
void addPrior(Index poseKey, const Pose2& pose, const SharedNoiseModel& noiseModel);
/// Biases the value of PoseKey key with Pose2 p given a noise model
void addPrior(const PoseKey& key, const Pose2& pose, const SharedNoiseModel& noiseModel);
/// Creates a hard constraint to enforce Pose2 p for PoseKey poseKey's value
void addPoseConstraint(Index poseKey, const Pose2& pose);
/// Creates a hard constraint to enforce Pose2 p for PoseKey poseKey's value
void addPoseConstraint(const PoseKey& poseKey, const Pose2& pose);
/// Creates a factor with a Pose2 between PoseKeys poseKey and pointKey (poseKey.e. an odometry measurement)
void addOdometry(Index poseKey, Index pointKey, const Pose2& odometry, const SharedNoiseModel& model);
/// 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,
const SharedNoiseModel& model);
/// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in rotation
void addBearing(Index poseKey, Index pointKey, const Rot2& bearing, const SharedNoiseModel& model);
/// 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,
const SharedNoiseModel& model);
/// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in location
void addRange(Index poseKey, Index pointKey, double range, const SharedNoiseModel& model);
/// 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,
const SharedNoiseModel& model);
/// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in rotation and location
void addBearingRange(Index poseKey, Index pointKey, const Rot2& bearing, double range, const SharedNoiseModel& model);
/// 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,
const Rot2& bearing, double range, const SharedNoiseModel& model);
/// Optimize
Values optimize(const Values& initialEstimate) {
typedef NonlinearOptimizer<Graph> Optimizer;
return *Optimizer::optimizeLM(*this, initialEstimate,
NonlinearOptimizationParameters::LAMBDA);
}
};
/// Optimize
Values optimize(const Values& initialEstimate) {
typedef NonlinearOptimizer<Graph> Optimizer;
return *Optimizer::optimizeLM(*this, initialEstimate,
NonlinearOptimizationParameters::LAMBDA);
}
};
/// Optimizer
typedef NonlinearOptimizer<Graph> Optimizer;
/// Optimizer
typedef NonlinearOptimizer<Graph> Optimizer;
} // planarSLAM

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) {
sharedFactor factor(new Prior(i, p, model));
sharedFactor factor(new Prior(PoseKey(i), p, model));
push_back(factor);
}
void Graph::addPoseConstraint(const PoseKey& i, const Pose2& p) {
sharedFactor factor(new HardConstraint(i, p));
void Graph::addPoseConstraint(Index i, const Pose2& p) {
sharedFactor factor(new HardConstraint(PoseKey(i), p));
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) {
sharedFactor factor(new Odometry(i, j, z, model));
sharedFactor factor(new Odometry(PoseKey(i), PoseKey(j), z, model));
push_back(factor);
}

17
gtsam/slam/pose2SLAM.h
View File

@ -33,9 +33,8 @@ namespace pose2SLAM {
using namespace gtsam;
/// Keys with Pose2 and symbol 'x'
typedef TypedSymbol<Pose2, 'x'> PoseKey;
/// Convenience function for constructing a pose key
inline Symbol PoseKey(Index j) { return Symbol('x', j); }
/// Values class, inherited from Values, using PoseKeys, mainly used as a convenience for MATLAB wrapper
struct Values: public gtsam::Values {
@ -70,11 +69,11 @@ namespace pose2SLAM {
*/
/// 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.
typedef PriorFactor<PoseKey> Prior;
typedef PriorFactor<Pose2> Prior;
/// A factor to add an odometry measurement between two poses.
typedef BetweenFactor<PoseKey> Odometry;
typedef BetweenFactor<Pose2> Odometry;
/// Graph
struct Graph: public NonlinearFactorGraph {
@ -86,13 +85,13 @@ namespace pose2SLAM {
Graph(const NonlinearFactorGraph& 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.
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
void addOdometry(const PoseKey& i, const PoseKey& j, const Pose2& z,
void addOdometry(Index i, Index j, const Pose2& z,
const SharedNoiseModel& model);
/// Optimize

14
gtsam/slam/pose3SLAM.cpp
View File

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

18
gtsam/slam/pose3SLAM.h
View File

@ -30,8 +30,8 @@ namespace gtsam {
/// Use pose3SLAM namespace for specific SLAM instance
namespace pose3SLAM {
/// Creates a Key with data Pose3 and symbol 'x'
typedef TypedSymbol<Pose3, 'x'> Key;
/// Convenience function for constructing a pose 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)
@ -42,25 +42,23 @@ namespace gtsam {
Values circle(size_t n, double R);
/// 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.
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.
typedef NonlinearEquality<Key> HardConstraint;
typedef NonlinearEquality<Pose3> HardConstraint;
/// Graph
struct Graph: public NonlinearFactorGraph {
/// Adds a factor between keys of the same type
void addPrior(const Key& i, const Pose3& p,
const SharedNoiseModel& model);
void addPrior(Index i, const Pose3& p, const SharedNoiseModel& model);
/// 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,
const SharedNoiseModel& model);
void addConstraint(Index i, Index j, const Pose3& z, const SharedNoiseModel& model);
/// 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

83
gtsam/slam/simulated2D.h
View File

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

60
gtsam/slam/simulated2DConstraints.h
View File

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

39
gtsam/slam/simulated2DOriented.h
View File

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

53
tests/testNonlinearFactor.cpp
View File

@ -235,13 +235,10 @@ TEST( NonlinearFactor, linearize_constraint2 )
}
/* ************************************************************************* */
typedef TypedSymbol<LieVector, 'x'> TestKey;
/* ************************************************************************* */
class TestFactor4 : public NonlinearFactor4<TestKey, TestKey, TestKey, TestKey> {
class TestFactor4 : public NonlinearFactor4<LieVector, LieVector, LieVector, LieVector> {
public:
typedef NonlinearFactor4<TestKey, TestKey, TestKey, TestKey> Base;
TestFactor4() : Base(sharedSigmas(Vector_(1, 2.0)), 1, 2, 3, 4) {}
typedef NonlinearFactor4<LieVector, LieVector, LieVector, LieVector> Base;
TestFactor4() : Base(sharedSigmas(Vector_(1, 2.0)), "x1", "x2", "x3", "x4") {}
virtual Vector
evaluateError(const LieVector& x1, const LieVector& x2, const LieVector& x3, const LieVector& x4,
@ -263,13 +260,13 @@ public:
TEST(NonlinearFactor, NonlinearFactor4) {
TestFactor4 tf;
Values tv;
tv.insert(TestKey(1), LieVector(1, 1.0));
tv.insert(TestKey(2), LieVector(1, 2.0));
tv.insert(TestKey(3), LieVector(1, 3.0));
tv.insert(TestKey(4), LieVector(1, 4.0));
tv.insert("x1", LieVector(1, 1.0));
tv.insert("x2", LieVector(1, 2.0));
tv.insert("x3", LieVector(1, 3.0));
tv.insert("x4", LieVector(1, 4.0));
EXPECT(assert_equal(Vector_(1, 10.0), tf.unwhitenedError(tv)));
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)));
LONGS_EQUAL(jf.keys()[0], 0);
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:
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) {}
virtual Vector
@ -310,14 +307,14 @@ public:
TEST(NonlinearFactor, NonlinearFactor5) {
TestFactor5 tf;
Values tv;
tv.insert(TestKey(1), LieVector(1, 1.0));
tv.insert(TestKey(2), LieVector(1, 2.0));
tv.insert(TestKey(3), LieVector(1, 3.0));
tv.insert(TestKey(4), LieVector(1, 4.0));
tv.insert(TestKey(5), LieVector(1, 5.0));
tv.insert("x1", LieVector(1, 1.0));
tv.insert("x2", LieVector(1, 2.0));
tv.insert("x3", LieVector(1, 3.0));
tv.insert("x4", LieVector(1, 4.0));
tv.insert("x5", LieVector(1, 5.0));
EXPECT(assert_equal(Vector_(1, 15.0), tf.unwhitenedError(tv)));
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)));
LONGS_EQUAL(jf.keys()[0], 0);
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:
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) {}
virtual Vector
@ -362,15 +359,15 @@ public:
TEST(NonlinearFactor, NonlinearFactor6) {
TestFactor6 tf;
Values tv;
tv.insert(TestKey(1), LieVector(1, 1.0));
tv.insert(TestKey(2), LieVector(1, 2.0));
tv.insert(TestKey(3), LieVector(1, 3.0));
tv.insert(TestKey(4), LieVector(1, 4.0));
tv.insert(TestKey(5), LieVector(1, 5.0));
tv.insert(TestKey(6), LieVector(1, 6.0));
tv.insert("x1", LieVector(1, 1.0));
tv.insert("x2", LieVector(1, 2.0));
tv.insert("x3", LieVector(1, 3.0));
tv.insert("x4", LieVector(1, 4.0));
tv.insert("x5", LieVector(1, 5.0));
tv.insert("x6", LieVector(1, 6.0));
EXPECT(assert_equal(Vector_(1, 21.0), tf.unwhitenedError(tv)));
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)));
LONGS_EQUAL(jf.keys()[0], 0);
LONGS_EQUAL(jf.keys()[1], 1);