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

Fixed factor equals

release/4.3a0
Alex Cunningham 2012-06-10 19:55:01 +00:00
parent 653d4ef131
commit 94560fcc45
2 changed files with 10 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
gtsam_unstable/nonlinear/LinearizedFactor.cpp
View File

@ -129,16 +129,17 @@ void LinearizedFactor::print(const std::string& s, const KeyFormatter& keyFormat
}
/* ************************************************************************* */
bool LinearizedFactor::equals(const LinearizedFactor& other, double tol) const {
if (!Base::equals(other, tol)
|| !lin_points_.equals(other.lin_points_, tol)
|| !equal_with_abs_tol(b_, other.b_, tol)
|| !model_->equals(*other.model_, tol)
|| matrices_.size() != other.matrices_.size())
bool LinearizedFactor::equals(const NonlinearFactor& other, double tol) const {
const LinearizedFactor* e = dynamic_cast<const LinearizedFactor*>(&other);
if (!e || !Base::equals(other, tol)
|| !lin_points_.equals(e->lin_points_, tol)
|| !equal_with_abs_tol(b_, e->b_, tol)
|| !model_->equals(*e->model_, tol)
|| matrices_.size() != e->matrices_.size())
return false;
KeyMatrixMap::const_iterator map1 = matrices_.begin(), map2 = other.matrices_.begin();
for (; map1 != matrices_.end(), map2 != other.matrices_.end(); ++map1, ++map2)
KeyMatrixMap::const_iterator map1 = matrices_.begin(), map2 = e->matrices_.begin();
for (; map1 != matrices_.end(), map2 != e->matrices_.end(); ++map1, ++map2)
if ((map1->first != map2->first) || !equal_with_abs_tol(map1->second, map2->second, tol))
return false;
return true;

2
gtsam_unstable/nonlinear/LinearizedFactor.h
View File

@ -100,7 +100,7 @@ public:
virtual void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/** equals function with optional tolerance */
virtual bool equals(const LinearizedFactor& other, double tol = 1e-9) const;
virtual bool equals(const NonlinearFactor& other, double tol = 1e-9) const;
// access functions