From c271e8f49be8ca101f649b4c7d195e84e78406f1 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Mon, 17 Oct 2011 19:40:19 +0000 Subject: [PATCH] Improved print() for nonlinear factors and constraints --- gtsam/nonlinear/NonlinearConstraint.h | 63 +++++++++++++++++++++++++++ gtsam/nonlinear/NonlinearFactor.h | 57 ++++++++++++++++++++++++ 2 files changed, 120 insertions(+) diff --git a/gtsam/nonlinear/NonlinearConstraint.h b/gtsam/nonlinear/NonlinearConstraint.h index 4b04e4a34..71972ff67 100644 --- a/gtsam/nonlinear/NonlinearConstraint.h +++ b/gtsam/nonlinear/NonlinearConstraint.h @@ -202,6 +202,14 @@ public: } } + /** Print */ + virtual void print(const std::string& s = "") const { + std::cout << s << ": NonlinearConstraint(" + << (std::string) this->key_ << ")," << + " mu = " << this->mu_ << + " dim = " << this->dim_ << std::endl; + } + /** * Override this method to finish implementing a unary factor. * If the optional Matrix reference argument is specified, it should compute @@ -247,6 +255,14 @@ public: /** Always active, so fixed value for active() */ virtual bool active(const VALUES& c) const { return true; } + /** Print */ + virtual void print(const std::string& s = "") const { + std::cout << s << ": NonlinearEqualityConstraint(" + << (std::string) this->key_ << ")," << + " mu = " << this->mu_ << + " dim = " << this->dim_ << std::endl; + } + private: /** Serialization function */ @@ -310,6 +326,15 @@ public: } } + /** Print */ + virtual void print(const std::string& s = "") const { + std::cout << s << ": NonlinearConstraint(" + << (std::string) this->key1_ << ")," + << (std::string) this->key2_ << ")," << + " mu = " << this->mu_ << + " dim = " << this->dim_ << std::endl; + } + /** * Override this method to finish implementing a binary factor. * If any of the optional Matrix reference arguments are specified, it should compute @@ -358,6 +383,15 @@ public: /** Always active, so fixed value for active() */ virtual bool active(const VALUES& c) const { return true; } + /** Print */ + virtual void print(const std::string& s = "") const { + std::cout << s << ": NonlinearEqualityConstraint(" + << (std::string) this->key1_ << ")," + << (std::string) this->key2_ << ")," << + " mu = " << this->mu_ << + " dim = " << this->dim_ << std::endl; + } + private: /** Serialization function */ @@ -425,6 +459,16 @@ public: } } + /** Print */ + virtual void print(const std::string& s = "") const { + std::cout << s << ": NonlinearConstraint(" + << (std::string) this->key1_ << ")," + << (std::string) this->key2_ << ")," + << (std::string) this->key3_ << ")," << + " mu = " << this->mu_ << + " dim = " << this->dim_ << std::endl; + } + /** * Override this method to finish implementing a trinary factor. * If any of the optional Matrix reference arguments are specified, it should compute @@ -478,6 +522,16 @@ public: /** Always active, so fixed value for active() */ virtual bool active(const VALUES& c) const { return true; } + /** Print */ + virtual void print(const std::string& s = "") const { + std::cout << s << ": NonlinearEqualityConstraint(" + << (std::string) this->key1_ << ")," + << (std::string) this->key2_ << ")," + << (std::string) this->key3_ << ")," << + " mu = " << this->mu_ << + " dim = " << this->dim_ << std::endl; + } + private: /** Serialization function */ @@ -522,6 +576,15 @@ public: return value_.logmap(x1); } + /** Print */ + virtual void print(const std::string& s = "") const { + std::cout << s << ": NonlinearEquality(" + << (std::string) this->key_ << "),"<< + " mu = " << this->mu_ << + " dim = " << this->dim_ << "\n"; + value_.print("Value"); + } + private: /** Serialization function */ diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index c3dacb85f..aae12a4ab 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -334,6 +334,12 @@ public: return evaluateError(x[key_]); } + /** Print */ + virtual void print(const std::string& s = "") const { + std::cout << s << ": NoiseModelFactor(" << (std::string) this->key_ << ")\n"; + this->noiseModel_->print(" noise model: "); + } + /** * Override this method to finish implementing a unary factor. * If the optional Matrix reference argument is specified, it should compute @@ -406,6 +412,14 @@ public: return evaluateError(x[key1_], x[key2_]); } + /** Print */ + virtual void print(const std::string& s = "") const { + std::cout << s << ": NoiseModelFactor(" + << (std::string) this->key1_ << "," + << (std::string) this->key2_ << ")\n"; + this->noiseModel_->print(" noise model: "); + } + /** * Override this method to finish implementing a binary factor. * If any of the optional Matrix reference arguments are specified, it should compute @@ -483,6 +497,16 @@ public: return evaluateError(x[key1_], x[key2_], x[key3_]); } + /** Print */ + virtual void print(const std::string& s = "") const { + std::cout << s << ": NoiseModelFactor(" + << (std::string) this->key1_ << "," + << (std::string) this->key2_ << "," + << (std::string) this->key3_ << ")\n"; + this->noiseModel_->print(" noise model: "); + } + + /** * Override this method to finish implementing a trinary factor. * If any of the optional Matrix reference arguments are specified, it should compute @@ -567,6 +591,16 @@ public: return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_]); } + /** Print */ + virtual void print(const std::string& s = "") const { + std::cout << s << ": NoiseModelFactor(" + << (std::string) this->key1_ << "," + << (std::string) this->key2_ << "," + << (std::string) this->key3_ << "," + << (std::string) this->key4_ << ")\n"; + this->noiseModel_->print(" noise model: "); + } + /** * Override this method to finish implementing a 4-way factor. * If any of the optional Matrix reference arguments are specified, it should compute @@ -657,6 +691,17 @@ public: return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_]); } + /** Print */ + virtual void print(const std::string& s = "") const { + std::cout << s << ": NoiseModelFactor(" + << (std::string) this->key1_ << "," + << (std::string) this->key2_ << "," + << (std::string) this->key3_ << "," + << (std::string) this->key4_ << "," + << (std::string) this->key5_ << ")\n"; + this->noiseModel_->print(" noise model: "); + } + /** * Override this method to finish implementing a 5-way factor. * If any of the optional Matrix reference arguments are specified, it should compute @@ -753,6 +798,18 @@ public: return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_], x[key6_]); } + /** Print */ + virtual void print(const std::string& s = "") const { + std::cout << s << ": NoiseModelFactor(" + << (std::string) this->key1_ << "," + << (std::string) this->key2_ << "," + << (std::string) this->key3_ << "," + << (std::string) this->key4_ << "," + << (std::string) this->key5_ << "," + << (std::string) this->key6_ << ")\n"; + this->noiseModel_->print(" noise model: "); + } + /** * Override this method to finish implementing a 6-way factor. * If any of the optional Matrix reference arguments are specified, it should compute