diff --git a/gtsam_unstable/nonlinear/LinearizedFactor.cpp b/gtsam_unstable/nonlinear/LinearizedFactor.cpp index 82e5c9eef..2d3d1af72 100644 --- a/gtsam_unstable/nonlinear/LinearizedFactor.cpp +++ b/gtsam_unstable/nonlinear/LinearizedFactor.cpp @@ -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(&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; diff --git a/gtsam_unstable/nonlinear/LinearizedFactor.h b/gtsam_unstable/nonlinear/LinearizedFactor.h index 8f9461d02..28102d8fd 100644 --- a/gtsam_unstable/nonlinear/LinearizedFactor.h +++ b/gtsam_unstable/nonlinear/LinearizedFactor.h @@ -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