diff --git a/cpp/VSLAMGraph.cpp b/cpp/VSLAMGraph.cpp index 830650c22..d09a1ff44 100644 --- a/cpp/VSLAMGraph.cpp +++ b/cpp/VSLAMGraph.cpp @@ -78,6 +78,13 @@ VSLAMGraph::VSLAMGraph(const std::string& path) ifs.close(); } +/* ************************************************************************* */ +bool VSLAMGraph::equals(const VSLAMGraph& p, double tol) const { + if (&p == NULL) return false; + if (nFrames != p.nFrames || feat_ids != p.feat_ids ) return false; + return true; +} + /* ************************************************************************* */ bool compareLandmark(const std::string& key, const VSLAMConfig& feasible, diff --git a/cpp/VSLAMGraph.h b/cpp/VSLAMGraph.h index dd116072f..8e97eec36 100644 --- a/cpp/VSLAMGraph.h +++ b/cpp/VSLAMGraph.h @@ -16,13 +16,14 @@ #include "FactorGraph-inl.h" #include "VSLAMFactor.h" #include "VSLAMConfig.h" +#include "Testable.h" namespace gtsam{ /** * Non-linear factor graph for visual SLAM */ -class VSLAMGraph : public gtsam::NonlinearFactorGraph{ +class VSLAMGraph : public gtsam::NonlinearFactorGraph, Testable{ private: int nFrames; typedef map feat_ids_type; @@ -46,6 +47,11 @@ public: gtsam::NonlinearFactorGraph::print(s); } + /** + * equals + */ + bool equals(const VSLAMGraph&, double tol=1e-9) const; + // Getters int Get_nFrames(){return nFrames;}; int Get_nFeat_ids(){return feat_ids.size();}; @@ -57,6 +63,15 @@ public: * @param p to which point to constrain it to */ void addLandmarkConstraint(int j, const Point3& p = Point3()); + +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(nFrames); + ar & BOOST_SERIALIZATION_NVP(feat_ids); + } }; } // namespace gtsam