12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Virtual equals and print methods in NonlinearFactor so that printing nonlinear factors works correctly

release/4.3a0
Richard Roberts 2010-10-26 20:21:52 +00:00
parent daa50ffe9a
commit a735b7cdeb
5 changed files with 33 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/inference/FactorGraph-inl.h
View File

@ -61,7 +61,7 @@ namespace gtsam {
printf("size: %d\n", (int) size()); printf("size: %d\n", (int) size());
for (size_t i = 0; i < factors_.size(); i++) { for (size_t i = 0; i < factors_.size(); i++) {
stringstream ss; stringstream ss;
ss << "factor " << i << ":"; ss << "factor " << i;
if (factors_[i] != NULL) factors_[i]->print(ss.str()); if (factors_[i] != NULL) factors_[i]->print(ss.str());
} }
} }

8
gtsam/linear/NoiseModel.cpp
View File

@ -271,7 +271,7 @@ Diagonal::shared_ptr Diagonal::Sigmas(const Vector& sigmas, bool smart) {
} }
void Diagonal::print(const string& name) const { 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 { Vector Diagonal::whiten(const Vector& v) const {
@ -309,7 +309,7 @@ Vector Diagonal::sample() const {
/* ************************************************************************* */ /* ************************************************************************* */
void Constrained::print(const std::string& name) 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 { 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 { 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 { double Isotropic::Mahalanobis(const Vector& v) const {
@ -460,7 +460,7 @@ Vector Isotropic::sample() const {
/* ************************************************************************* */ /* ************************************************************************* */
void Unit::print(const std::string& name) const { void Unit::print(const std::string& name) const {
cout << "Unit (" << dim_ << ") " << name << endl; cout << name << ": unit (" << dim_ << ") " << endl;
} }
/* ************************************************************************* */ /* ************************************************************************* */

44
gtsam/nonlinear/NonlinearFactor.h
View File

@ -78,13 +78,13 @@ namespace gtsam {
} }
/** print */ /** print */
void print(const std::string& s = "") const { virtual void print(const std::string& s = "") const {
std::cout << "NonlinearFactor " << s << std::endl; std::cout << s << ": NonlinearFactor\n";
noiseModel_->print("noise model"); noiseModel_->print(" noise model");
} }
/** Check if two NonlinearFactor objects are equal */ /** Check if two NonlinearFactor objects are equal */
bool equals(const NonlinearFactor<VALUES>& f, double tol = 1e-9) const { virtual bool equals(const NonlinearFactor<VALUES>& f, double tol = 1e-9) const {
return noiseModel_->equals(*f.noiseModel_, tol); return noiseModel_->equals(*f.noiseModel_, tol);
} }
@ -198,14 +198,14 @@ namespace gtsam {
} }
/* print */ /* print */
void print(const std::string& s = "") const { virtual void print(const std::string& s = "") const {
std::cout << "NonlinearFactor1 " << s << std::endl; std::cout << s << ": NonlinearFactor1\n";
std::cout << "key: " << (std::string) key_ << std::endl; std::cout << " key: " << (std::string) key_ << std::endl;
Base::print("parent"); this->noiseModel_->print(" noise model: ");
} }
/** Check if two factors are equal. Note type is IndexFactor and needs cast. */ /** Check if two factors are equal. Note type is IndexFactor and needs cast. */
bool equals(const NonlinearFactor1<VALUES,KEY>& f, double tol = 1e-9) const { virtual bool equals(const NonlinearFactor1<VALUES,KEY>& f, double tol = 1e-9) const {
return Base::noiseModel_->equals(*f.noiseModel_, tol) && (key_ == f.key_); return Base::noiseModel_->equals(*f.noiseModel_, tol) && (key_ == f.key_);
} }
@ -309,15 +309,15 @@ namespace gtsam {
} }
/** Print */ /** Print */
void print(const std::string& s = "") const { virtual void print(const std::string& s = "") const {
std::cout << "NonlinearFactor2 " << s << std::endl; std::cout << s << ": NonlinearFactor2\n";
std::cout << "key1: " << (std::string) key1_ << std::endl; std::cout << " key1: " << (std::string) key1_ << "\n";
std::cout << "key2: " << (std::string) key2_ << std::endl; std::cout << " key2: " << (std::string) key2_ << "\n";
Base::print("parent"); this->noiseModel_->print(" noise model: ");
} }
/** Check if two factors are equal */ /** Check if two factors are equal */
bool equals(const NonlinearFactor2<VALUES,KEY1,KEY2>& f, double tol = 1e-9) const { virtual bool equals(const NonlinearFactor2<VALUES,KEY1,KEY2>& f, double tol = 1e-9) const {
return Base::noiseModel_->equals(*f.noiseModel_, tol) && (key1_ == f.key1_) return Base::noiseModel_->equals(*f.noiseModel_, tol) && (key1_ == f.key1_)
&& (key2_ == f.key2_); && (key2_ == f.key2_);
} }
@ -448,16 +448,16 @@ namespace gtsam {
} }
/** Print */ /** Print */
void print(const std::string& s = "") const { virtual void print(const std::string& s = "") const {
std::cout << "NonlinearFactor3 " << s << std::endl; std::cout << s << ": NonlinearFactor3\n";
std::cout << "key1: " << (std::string) key1_ << std::endl; std::cout << " key1: " << (std::string) key1_ << "\n";
std::cout << "key2: " << (std::string) key2_ << std::endl; std::cout << " key2: " << (std::string) key2_ << "\n";
std::cout << "key3: " << (std::string) key3_ << std::endl; std::cout << " key3: " << (std::string) key3_ << "\n";
Base::print("parent"); this->noiseModel_->print(" noise model: ");
} }
/** Check if two factors are equal */ /** Check if two factors are equal */
bool equals(const NonlinearFactor3<VALUES,KEY1,KEY2,KEY3>& f, double tol = 1e-9) const { virtual bool equals(const NonlinearFactor3<VALUES,KEY1,KEY2,KEY3>& f, double tol = 1e-9) const {
return Base::noiseModel_->equals(*f.noiseModel_, tol) && (key1_ == f.key1_) return Base::noiseModel_->equals(*f.noiseModel_, tol) && (key1_ == f.key1_)
&& (key2_ == f.key2_) && (key3_ == f.key3_); && (key2_ == f.key2_) && (key3_ == f.key3_);
} }

7
gtsam/slam/BetweenFactor.h
View File

@ -56,17 +56,16 @@ namespace gtsam {
/** implement functions needed for Testable */ /** implement functions needed for Testable */
/** print */ /** print */
void print(const std::string& s) const { virtual void print(const std::string& s) const {
Base::print(s); Base::print(s);
measured_.print("measured"); measured_.print("measured");
} }
/** equals */ /** equals */
bool equals(const NonlinearFactor<VALUES>& expected, double tol) const { virtual bool equals(const NonlinearFactor<VALUES>& expected, double tol) const {
const BetweenFactor<VALUES, KEY1, KEY2> *e = const BetweenFactor<VALUES, KEY1, KEY2> *e =
dynamic_cast<const BetweenFactor<VALUES, KEY1, KEY2>*> (&expected); dynamic_cast<const BetweenFactor<VALUES, KEY1, KEY2>*> (&expected);
return e != NULL && Base::equals(expected, tol) && this->measured_.equals( return e != NULL && Base::equals(*e, tol) && this->measured_.equals(e->measured_, tol);
e->measured_, tol);
} }
/** implement functions needed to derive from Factor */ /** implement functions needed to derive from Factor */

7
gtsam/slam/PriorFactor.h
View File

@ -57,17 +57,16 @@ namespace gtsam {
/** implement functions needed for Testable */ /** implement functions needed for Testable */
/** print */ /** print */
void print(const std::string& s) const { virtual void print(const std::string& s) const {
Base::print(s); Base::print(s);
prior_.print("prior"); prior_.print("prior");
} }
/** equals */ /** equals */
bool equals(const NonlinearFactor<VALUES>& expected, double tol) const { virtual bool equals(const NonlinearFactor<VALUES>& expected, double tol) const {
const PriorFactor<VALUES, KEY> *e = dynamic_cast<const PriorFactor< const PriorFactor<VALUES, KEY> *e = dynamic_cast<const PriorFactor<
VALUES, KEY>*> (&expected); VALUES, KEY>*> (&expected);
return e != NULL && Base::equals(expected, tol) && this->prior_.equals( return e != NULL && Base::equals(*e, tol) && this->prior_.equals(e->prior_, tol);
e->prior_, tol);
} }
/** implement functions needed to derive from Factor */ /** implement functions needed to derive from Factor */