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());
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());
}
}

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 {
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;
}
/* ************************************************************************* */

44
gtsam/nonlinear/NonlinearFactor.h
View File

@ -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<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);
}
@ -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<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_);
}
@ -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<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_)
&& (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<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_)
&& (key2_ == f.key2_) && (key3_ == f.key3_);
}

7
gtsam/slam/BetweenFactor.h
View File

@ -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<VALUES>& expected, double tol) const {
virtual bool equals(const NonlinearFactor<VALUES>& expected, double tol) const {
const BetweenFactor<VALUES, KEY1, KEY2> *e =
dynamic_cast<const BetweenFactor<VALUES, KEY1, KEY2>*> (&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 */

7
gtsam/slam/PriorFactor.h
View File

@ -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<VALUES>& expected, double tol) const {
virtual bool equals(const NonlinearFactor<VALUES>& expected, double tol) const {
const PriorFactor<VALUES, KEY> *e = dynamic_cast<const PriorFactor<
VALUES, KEY>*> (&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 */