104 lines
2.6 KiB
C++
104 lines
2.6 KiB
C++
/**
|
|
* @file VSLAMGraph.h
|
|
* @brief A factor graph for the VSLAM problem
|
|
* @author Alireza Fathi
|
|
* @author Carlos Nieto
|
|
*/
|
|
|
|
#include <set>
|
|
#include <fstream>
|
|
#include <boost/foreach.hpp>
|
|
|
|
#include "VSLAMGraph.h"
|
|
|
|
using namespace std;
|
|
namespace gtsam{
|
|
|
|
/* ************************************************************************* */
|
|
//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, 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();
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
VSLAMGraph::VSLAMGraph(const std::string& path,
|
|
int nrFrames, double sigma,
|
|
const gtsam::Cal3_S2 &K)
|
|
{
|
|
ifstream ifs(path.c_str(), ios::in);
|
|
|
|
if(ifs) {
|
|
int cameraFrameNumber, landmarkNumber;
|
|
double landmarkX, landmarkY, landmarkZ;
|
|
double u, v;
|
|
ifs >> cameraFrameNumber >> landmarkNumber >> u >> v >> landmarkX >> landmarkY >> landmarkZ;
|
|
|
|
//Store the measurements
|
|
Vector z(2);
|
|
z(0)=u;
|
|
z(1)=v;
|
|
|
|
//VSLAMFactor::shared_ptr f1(new VSLAMFactor<VSLAMConfig>::VSLAMFactor(z, sigma, cameraFrameNumber, landmarkNumber, K));
|
|
//factors_.push_back(f1);
|
|
}
|
|
else {
|
|
printf("Unable to load values in %s\n", path.c_str());
|
|
exit(0);
|
|
}
|
|
|
|
ifs.close();
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
|
|
} // namespace gtsam
|
|
|