diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h index 888dcb76b..bc5e9c87f 100644 --- a/gtsam/slam/PriorFactor.h +++ b/gtsam/slam/PriorFactor.h @@ -67,14 +67,14 @@ namespace gtsam { /** print */ virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { std::cout << s << "PriorFactor on " << keyFormatter(this->key()) << "\n"; - prior_.print(" prior mean: "); + traits::print()(prior_, " prior mean: "); 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->prior_.equals(e->prior_, tol); + return e != NULL && Base::equals(*e, tol) && traits::equals()(prior_, e->prior_, tol); } /** implement functions needed to derive from Factor */ @@ -83,7 +83,8 @@ namespace gtsam { Vector evaluateError(const T& p, boost::optional H = boost::none) const { if (H) (*H) = eye(p.dim()); // manifold equivalent of h(x)-z -> log(z,h(x)) - return prior_.localCoordinates(p); + DefaultChart chart; + return chart.local(prior_,p); } const VALUE & prior() const { return prior_; }