From a735b7cdeb40031ea740869dfa262859f788cd6f Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 26 Oct 2010 20:21:52 +0000 Subject: [PATCH] Virtual equals and print methods in NonlinearFactor so that printing nonlinear factors works correctly --- gtsam/inference/FactorGraph-inl.h | 2 +- gtsam/linear/NoiseModel.cpp | 8 +++--- gtsam/nonlinear/NonlinearFactor.h | 44 +++++++++++++++---------------- gtsam/slam/BetweenFactor.h | 7 +++-- gtsam/slam/PriorFactor.h | 7 +++-- 5 files changed, 33 insertions(+), 35 deletions(-) diff --git a/gtsam/inference/FactorGraph-inl.h b/gtsam/inference/FactorGraph-inl.h index b2e4b105d..1a4d53af5 100644 --- a/gtsam/inference/FactorGraph-inl.h +++ b/gtsam/inference/FactorGraph-inl.h @@ -61,7 +61,7 @@ namespace gtsam { printf("size: %d\n", (int) size()); for (size_t i = 0; i < factors_.size(); i++) { stringstream ss; - ss << "factor " << i << ":"; + ss << "factor " << i; if (factors_[i] != NULL) factors_[i]->print(ss.str()); } } diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index ad34be5f2..10c8c76a2 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -271,7 +271,7 @@ Diagonal::shared_ptr Diagonal::Sigmas(const Vector& sigmas, bool smart) { } void Diagonal::print(const string& name) const { - gtsam::print(sigmas_, "Diagonal sigmas " + name); + gtsam::print(sigmas_, name + ": diagonal sigmas"); } Vector Diagonal::whiten(const Vector& v) const { @@ -309,7 +309,7 @@ Vector Diagonal::sample() const { /* ************************************************************************* */ void Constrained::print(const std::string& name) const { - gtsam::print(sigmas_, "Constrained sigmas " + name); + gtsam::print(sigmas_, name + ": constrained sigmas"); } Vector Constrained::whiten(const Vector& v) const { @@ -419,7 +419,7 @@ Isotropic::shared_ptr Isotropic::Variance(size_t dim, double variance, bool smar } void Isotropic::print(const string& name) const { - cout << "Isotropic sigma " << name << " " << sigma_ << endl; + cout << name << ": isotropic sigma " << " " << sigma_ << endl; } double Isotropic::Mahalanobis(const Vector& v) const { @@ -460,7 +460,7 @@ Vector Isotropic::sample() const { /* ************************************************************************* */ void Unit::print(const std::string& name) const { - cout << "Unit (" << dim_ << ") " << name << endl; + cout << name << ": unit (" << dim_ << ") " << endl; } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 532fb4661..833a85530 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -78,13 +78,13 @@ namespace gtsam { } /** print */ - void print(const std::string& s = "") const { - std::cout << "NonlinearFactor " << s << std::endl; - noiseModel_->print("noise model"); + virtual void print(const std::string& s = "") const { + std::cout << s << ": NonlinearFactor\n"; + noiseModel_->print(" noise model"); } /** Check if two NonlinearFactor objects are equal */ - bool equals(const NonlinearFactor& f, double tol = 1e-9) const { + virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { return noiseModel_->equals(*f.noiseModel_, tol); } @@ -198,14 +198,14 @@ namespace gtsam { } /* print */ - void print(const std::string& s = "") const { - std::cout << "NonlinearFactor1 " << s << std::endl; - std::cout << "key: " << (std::string) key_ << std::endl; - Base::print("parent"); + virtual void print(const std::string& s = "") const { + std::cout << s << ": NonlinearFactor1\n"; + std::cout << " key: " << (std::string) key_ << std::endl; + this->noiseModel_->print(" noise model: "); } /** Check if two factors are equal. Note type is IndexFactor and needs cast. */ - bool equals(const NonlinearFactor1& f, double tol = 1e-9) const { + virtual bool equals(const NonlinearFactor1& f, double tol = 1e-9) const { return Base::noiseModel_->equals(*f.noiseModel_, tol) && (key_ == f.key_); } @@ -309,15 +309,15 @@ namespace gtsam { } /** Print */ - void print(const std::string& s = "") const { - std::cout << "NonlinearFactor2 " << s << std::endl; - std::cout << "key1: " << (std::string) key1_ << std::endl; - std::cout << "key2: " << (std::string) key2_ << std::endl; - Base::print("parent"); + virtual void print(const std::string& s = "") const { + std::cout << s << ": NonlinearFactor2\n"; + std::cout << " key1: " << (std::string) key1_ << "\n"; + std::cout << " key2: " << (std::string) key2_ << "\n"; + this->noiseModel_->print(" noise model: "); } /** Check if two factors are equal */ - bool equals(const NonlinearFactor2& f, double tol = 1e-9) const { + virtual bool equals(const NonlinearFactor2& f, double tol = 1e-9) const { return Base::noiseModel_->equals(*f.noiseModel_, tol) && (key1_ == f.key1_) && (key2_ == f.key2_); } @@ -448,16 +448,16 @@ namespace gtsam { } /** Print */ - void print(const std::string& s = "") const { - std::cout << "NonlinearFactor3 " << s << std::endl; - std::cout << "key1: " << (std::string) key1_ << std::endl; - std::cout << "key2: " << (std::string) key2_ << std::endl; - std::cout << "key3: " << (std::string) key3_ << std::endl; - Base::print("parent"); + virtual void print(const std::string& s = "") const { + std::cout << s << ": NonlinearFactor3\n"; + std::cout << " key1: " << (std::string) key1_ << "\n"; + std::cout << " key2: " << (std::string) key2_ << "\n"; + std::cout << " key3: " << (std::string) key3_ << "\n"; + this->noiseModel_->print(" noise model: "); } /** Check if two factors are equal */ - bool equals(const NonlinearFactor3& f, double tol = 1e-9) const { + virtual bool equals(const NonlinearFactor3& f, double tol = 1e-9) const { return Base::noiseModel_->equals(*f.noiseModel_, tol) && (key1_ == f.key1_) && (key2_ == f.key2_) && (key3_ == f.key3_); } diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index e3a74e59d..e1c20b96e 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -56,17 +56,16 @@ namespace gtsam { /** implement functions needed for Testable */ /** print */ - void print(const std::string& s) const { + virtual void print(const std::string& s) const { Base::print(s); measured_.print("measured"); } /** equals */ - bool equals(const NonlinearFactor& expected, double tol) const { + virtual bool equals(const NonlinearFactor& expected, double tol) const { const BetweenFactor *e = dynamic_cast*> (&expected); - return e != NULL && Base::equals(expected, tol) && this->measured_.equals( - e->measured_, tol); + return e != NULL && Base::equals(*e, tol) && this->measured_.equals(e->measured_, tol); } /** implement functions needed to derive from Factor */ diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h index d2b7e233f..850377ec7 100644 --- a/gtsam/slam/PriorFactor.h +++ b/gtsam/slam/PriorFactor.h @@ -57,17 +57,16 @@ namespace gtsam { /** implement functions needed for Testable */ /** print */ - void print(const std::string& s) const { + virtual void print(const std::string& s) const { Base::print(s); prior_.print("prior"); } /** equals */ - bool equals(const NonlinearFactor& expected, double tol) const { + virtual bool equals(const NonlinearFactor& expected, double tol) const { const PriorFactor *e = dynamic_cast*> (&expected); - return e != NULL && Base::equals(expected, tol) && this->prior_.equals( - e->prior_, tol); + return e != NULL && Base::equals(*e, tol) && this->prior_.equals(e->prior_, tol); } /** implement functions needed to derive from Factor */