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

Equals

release/4.3a0
dellaert 2015-02-21 16:39:39 +01:00
parent 5120e4b77a
commit c3c5c0d80c
1 changed files with 15 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
gtsam/slam/RegularImplicitSchurFactor.h
View File

@ -96,11 +96,17 @@ public:
/// equals /// equals
bool equals(const GaussianFactor& lf, double tol) const { bool equals(const GaussianFactor& lf, double tol) const {
if (!dynamic_cast<const RegularImplicitSchurFactor*>(&lf)) const This* f = dynamic_cast<const This*>(&lf);
return false; if (!f)
else {
return false; return false;
for (size_t pos = 0; pos < size(); ++pos) {
if (keys_[pos] != f->keys_[pos]) return false;
if (Fblocks_[pos].first != f->Fblocks_[pos].first) return false;
if (!equal_with_abs_tol(Fblocks_[pos].second,f->Fblocks_[pos].second,tol)) return false;
} }
return equal_with_abs_tol(PointCovariance_, f->PointCovariance_, tol)
&& equal_with_abs_tol(E_, f->E_, tol)
&& equal_with_abs_tol(b_, f->b_, tol);
} }
/// Degrees of freedom of camera /// Degrees of freedom of camera
@ -460,7 +466,12 @@ public:
}; };
// RegularImplicitSchurFactor // end class RegularImplicitSchurFactor
// traits
template<size_t D> struct traits<RegularImplicitSchurFactor<D> > : public Testable<
RegularImplicitSchurFactor<D> > {
};
} }