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

Fixed printing

release/4.3a0
Frank Dellaert 2011-10-22 19:56:53 +00:00
parent fabde56642
commit 2ddc60063f
3 changed files with 26 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
gtsam/slam/BearingRangeFactor.h
View File

@ -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<VALUES>& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&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<Matrix&> H1, boost::optional<Matrix&> H2) const {
@ -82,14 +100,6 @@ namespace gtsam {
return std::make_pair(bearing_, range_);
}
/** equals */
virtual bool equals(const NonlinearFactor<VALUES>& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol) &&
fabs(this->range_ - e->range_) < tol &&
this->bearing_.equals(e->bearing_, tol);
}
private:
/** Serialization function */

7
gtsam/slam/BetweenFactor.h
View File

@ -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 */

5
gtsam/slam/PriorFactor.h
View File

@ -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 */