diff --git a/cpp/VSLAMGraph.cpp b/cpp/VSLAMGraph.cpp index e404dee57..da034e939 100644 --- a/cpp/VSLAMGraph.cpp +++ b/cpp/VSLAMGraph.cpp @@ -24,66 +24,7 @@ template class NonlinearFactorGraph; template class NonlinearOptimizer; /* ************************************************************************* */ -//TODO: CB: This constructor is specific to loading VO data. Should probably -// get rid of this. -VSLAMGraph::VSLAMGraph(const std::string& path) -{ - ifstream ifs(path.c_str(), ios::in); - if(ifs) { - // read calibration K - double fx, fy, s, u0, v0; - ifs >> fx >> fy >> s >> u0 >> v0; - Cal3_S2 K(fx, fy, s, u0, v0); - - // read sigma - double sigma; - ifs >> sigma; - - // read number of frames - int nrFrames; - ifs >> nrFrames; - nFrames = nrFrames; - - // read all frames - for (int i=0;i> nrMeasurements; - // read all measurements in ith frame - for (int k=0;k> j >> u >> v; - Point2 z(u,v); - - // this works - //VSLAMFactor::shared_ptr testing(new VSLAMFactor()); - //factors_.push_back(testing); - - VSLAMFactor::shared_ptr f1(new VSLAMFactor::VSLAMFactor(z.vector(), sigma, i+1, j, - VSLAMFactor::shared_ptrK(new Cal3_S2(K)))); - factors_.push_back(f1); - //cout << "Added factor " << i+1 << endl; - // just to keep a record of all the feature id's that have been encountered - // value is unused/useless right now, but could be used to keep count - feat_ids.insert(pair(j,0)); - } - } - } - else { - printf("Unable to load values in %s\n", path.c_str()); - exit(0); - } - - 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, diff --git a/cpp/VSLAMGraph.h b/cpp/VSLAMGraph.h index 72fb1f758..0e2b8b1a8 100644 --- a/cpp/VSLAMGraph.h +++ b/cpp/VSLAMGraph.h @@ -23,23 +23,13 @@ namespace gtsam{ /** * Non-linear factor graph for visual SLAM */ -class VSLAMGraph : public gtsam::NonlinearFactorGraph, Testable{ -private: - int nFrames; - typedef map feat_ids_type; - feat_ids_type feat_ids; +class VSLAMGraph : public gtsam::NonlinearFactorGraph{ public: /** default constructor is empty graph */ VSLAMGraph() {} - /** - * Constructor that loads measurements from file - * @param path to the file - */ - VSLAMGraph(const std::string& path); - /** * print out graph */ @@ -50,12 +40,9 @@ public: /** * equals */ - bool equals(const VSLAMGraph&, double tol=1e-9) const; - - // Getters - int Get_nFrames(){return nFrames;}; - int Get_nFeat_ids(){return feat_ids.size();}; - feat_ids_type* Get_feat_ids_map(){return &feat_ids;}; + bool equals(const VSLAMGraph& p, double tol=1e-9) const { + return gtsam::NonlinearFactorGraph::equals(p, tol); + } /** * Add a constraint on a landmark (for now, *must* be satisfied in any Config) @@ -75,10 +62,7 @@ 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); - } + void serialize(Archive & ar, const unsigned int version) {} }; } // namespace gtsam