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