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

cleaned up, and removed Testable here because it's already provided by base class FactorGraph

release/4.3a0
Chris Beall 2009-12-09 19:11:45 +00:00
parent 8e079a9794
commit 3b3c76b273
2 changed files with 5 additions and 80 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

59
cpp/VSLAMGraph.cpp
View File

@ -24,66 +24,7 @@ template class NonlinearFactorGraph<VSLAMConfig>;
template class NonlinearOptimizer<VSLAMGraph,VSLAMConfig>;
/* ************************************************************************* */
//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<nrFrames;i++) {
int nrMeasurements;
ifs >> nrMeasurements;
// read all measurements in ith frame
for (int k=0;k<nrMeasurements;k++) {
int j; // landmark number
double u, v;
ifs >> 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<int, int>(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,

26
cpp/VSLAMGraph.h
View File

@ -23,23 +23,13 @@ namespace gtsam{
/**
* Non-linear factor graph for visual SLAM
*/
class VSLAMGraph : public gtsam::NonlinearFactorGraph<VSLAMConfig>, Testable<VSLAMGraph>{
private:
int nFrames;
typedef map <int, int> feat_ids_type;
feat_ids_type feat_ids;
class VSLAMGraph : public gtsam::NonlinearFactorGraph<VSLAMConfig>{
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<VSLAMConfig>::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<class Archive>
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