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

using traits and Chart in BetweenFactor

release/4.3a0
Christian Forster 2014-12-01 14:44:22 -05:00
parent 962f3a5155
commit 85122bed95
1 changed files with 7 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
gtsam/slam/BetweenFactor.h
View File

@ -74,14 +74,14 @@ namespace gtsam {
std::cout << s << "BetweenFactor(" std::cout << s << "BetweenFactor("
<< keyFormatter(this->key1()) << "," << keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << ")\n"; << keyFormatter(this->key2()) << ")\n";
measured_.print(" measured: "); traits::print<T>()(measured_, " measured: ");
this->noiseModel_->print(" noise model: "); this->noiseModel_->print(" noise model: ");
} }
/** 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->measured_.equals(e->measured_, tol); return e != NULL && Base::equals(*e, tol) && traits::equals<T>()(this->measured_, e->measured_, tol);
} }
/** implement functions needed to derive from Factor */ /** implement functions needed to derive from Factor */
@ -92,7 +92,8 @@ namespace gtsam {
boost::none) const { boost::none) const {
T hx = p1.between(p2, H1, H2); // h(x) T hx = p1.between(p2, H1, H2); // h(x)
// manifold equivalent of h(x)-z -> log(z,h(x)) // manifold equivalent of h(x)-z -> log(z,h(x))
return measured_.localCoordinates(hx); DefaultChart<T> chart;
return chart.local(measured_, hx);
} }
/** return the measured */ /** return the measured */
@ -129,7 +130,9 @@ namespace gtsam {
/** Syntactic sugar for constrained version */ /** Syntactic sugar for constrained version */
BetweenConstraint(const VALUE& measured, Key key1, Key key2, double mu = 1000.0) : BetweenConstraint(const VALUE& measured, Key key1, Key key2, double mu = 1000.0) :
BetweenFactor<VALUE>(key1, key2, measured, noiseModel::Constrained::All(VALUE::Dim(), fabs(mu))) {} BetweenFactor<VALUE>(key1, key2, measured,
noiseModel::Constrained::All(traits::dimension<VALUE>(), fabs(mu)))
{}
private: private: