diff --git a/gtsam/slam/BearingRangeFactor.h b/gtsam/slam/BearingRangeFactor.h index 3c4a7d8a8..833708b5d 100644 --- a/gtsam/slam/BearingRangeFactor.h +++ b/gtsam/slam/BearingRangeFactor.h @@ -57,6 +57,24 @@ namespace gtsam { virtual ~BearingRangeFactor() {} + /** Print */ + virtual void print(const std::string& s = "") const { + std::cout << s << ": BearingRangeFactor(" + << (std::string) this->key1_ << "," + << (std::string) this->key2_ << ")\n"; + bearing_.print(" measured bearing"); + std::cout << " measured range: " << range_ << std::endl; + 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) && + fabs(this->range_ - e->range_) < tol && + this->bearing_.equals(e->bearing_, tol); + } + /** h(x)-z -> between(z,h(x)) for Rot manifold */ Vector evaluateError(const Pose& pose, const Point& point, boost::optional H1, boost::optional H2) const { @@ -82,14 +100,6 @@ namespace gtsam { return std::make_pair(bearing_, range_); } - /** 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) && - fabs(this->range_ - e->range_) < tol && - this->bearing_.equals(e->bearing_, tol); - } - private: /** Serialization function */ diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index 8be8b02e3..d95f3980b 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -62,8 +62,11 @@ namespace gtsam { /** print */ virtual void print(const std::string& s) const { - Base::print(s); - measured_.print("measured"); + std::cout << s << ": BetweenFactor(" + << (std::string) this->key1_ << "," + << (std::string) this->key2_ << ")\n"; + measured_.print(" measured"); + this->noiseModel_->print(" noise model"); } /** equals */ diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h index 5351145e7..5bb22506f 100644 --- a/gtsam/slam/PriorFactor.h +++ b/gtsam/slam/PriorFactor.h @@ -63,8 +63,9 @@ namespace gtsam { /** print */ virtual void print(const std::string& s) const { - Base::print(s); - prior_.print("prior"); + std::cout << s << ": PriorFactor(" << (std::string) this->key_ << ")\n"; + prior_.print(" prior"); + this->noiseModel_->print(" noise model"); } /** equals */