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

Fixed the prior factor to use charts and traits

release/4.3a0
Paul Furgale 2014-12-01 15:29:40 +01:00
parent c90bc5c34a
commit 6b4d2321b4
1 changed files with 4 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
gtsam/slam/PriorFactor.h
View File

@ -67,14 +67,14 @@ namespace gtsam {
/** print */
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "PriorFactor on " << keyFormatter(this->key()) << "\n";
prior_.print(" prior mean: ");
traits::print<T>()(prior_, " prior mean: ");
this->noiseModel_->print(" noise model: ");
}
/** equals */
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This* e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol) && this->prior_.equals(e->prior_, tol);
return e != NULL && Base::equals(*e, tol) && traits::equals<T>()(prior_, e->prior_, tol);
}
/** implement functions needed to derive from Factor */
@ -83,7 +83,8 @@ namespace gtsam {
Vector evaluateError(const T& p, boost::optional<Matrix&> H = boost::none) const {
if (H) (*H) = eye(p.dim());
// manifold equivalent of h(x)-z -> log(z,h(x))
return prior_.localCoordinates(p);
DefaultChart<T> chart;
return chart.local(prior_,p);
}
const VALUE & prior() const { return prior_; }