124 lines
3.1 KiB
C++
124 lines
3.1 KiB
C++
#include "landmarkUtils.h"
|
|
#include <fstream>
|
|
#include <cstdio>
|
|
|
|
using namespace gtsam;
|
|
using namespace std;
|
|
|
|
/* ************************************************************************* */
|
|
bool readLandMarks(const char* landmarkFile, std::map<int, Point3>& landmarks)
|
|
{
|
|
ifstream file(landmarkFile);
|
|
if (!file) {
|
|
cout << "Cannot read landmark file: " << landmarkFile << endl;
|
|
exit(0);
|
|
}
|
|
|
|
int num;
|
|
file >> num;
|
|
landmarks.clear();
|
|
for (int i = 0; i<num; i++)
|
|
{
|
|
int color_id;
|
|
float x, y, z;
|
|
file >> color_id >> x >> y >> z;
|
|
landmarks[color_id] = Point3(x, y, z);
|
|
}
|
|
|
|
file.close();
|
|
return true;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
/**
|
|
* Read pose from file, output by Panda3D.
|
|
* Warning: row major!!!
|
|
*/
|
|
gtsam::Pose3 readPose(const char* Fn)
|
|
{
|
|
ifstream poseFile(Fn);
|
|
if (!poseFile)
|
|
{
|
|
cout << "Cannot read pose file: " << Fn << endl;
|
|
exit(0);
|
|
}
|
|
|
|
double v[16];
|
|
for (int i = 0; i<16; i++)
|
|
poseFile >> v[i];
|
|
poseFile.close();
|
|
|
|
// Because panda3d's camera is z-up, y-view,
|
|
// we swap z and y to have y-up, z-view, then negate z to stick with the right-hand rule
|
|
//... similar to OpenGL's camera
|
|
for (int i = 0; i<3; i++)
|
|
{
|
|
float t = v[4+i];
|
|
v[4+i] = v[8+i];
|
|
v[8+i] = -t;
|
|
}
|
|
|
|
::Vector vec = Vector_(16, v);
|
|
|
|
Matrix T = Matrix_(4,4, vec); // column order !!!
|
|
|
|
Pose3 pose(T);
|
|
return pose;
|
|
}
|
|
/* ************************************************************************* */
|
|
gtsam::Pose3 readPose(const char* poseFn_pre, const char* poseFn_suf, int poseId)
|
|
{
|
|
char poseFn[128];
|
|
sprintf(poseFn, "%s%d%s", poseFn_pre, poseId, poseFn_suf);
|
|
return readPose(poseFn);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
gtsam::Cal3_S2 readCalibData(const char* calibFn)
|
|
{
|
|
ifstream calibFile(calibFn);
|
|
if (!calibFile)
|
|
{
|
|
cout << "Cannot read calib file: " << calibFn << endl;
|
|
exit(0);
|
|
}
|
|
int imX, imY;
|
|
float fx, fy, ox, oy;
|
|
calibFile >> imX >> imY >> fx >> fy >> ox >> oy;
|
|
calibFile.close();
|
|
|
|
Cal3_S2 K(fx, fy, 0, ox, oy); // skew factor = 0
|
|
return K;
|
|
}
|
|
/* ************************************************************************* */
|
|
std::vector<Feature2D> readFeatures(const char* filename)
|
|
{
|
|
ifstream file(filename);
|
|
if (!file)
|
|
{
|
|
cout << "Cannot read feature file: " << filename<< endl;
|
|
exit(0);
|
|
}
|
|
|
|
int numFeatures;
|
|
file >> numFeatures ;
|
|
|
|
std::vector<Feature2D> vFeatures_;
|
|
for (size_t i = 0; i < numFeatures; i++)
|
|
{
|
|
int id; double x, y;
|
|
file >> id >> x >> y;
|
|
vFeatures_.push_back(Feature2D(id, Point2(x, y)));
|
|
}
|
|
|
|
file.close();
|
|
return vFeatures_;
|
|
}
|
|
/* ************************************************************************* */
|
|
std::vector<Feature2D> readFeatures(const char* featFn_pre, const char* featFn_suf, int imageId)
|
|
{
|
|
char featFn[128];
|
|
sprintf(featFn, "%s%d%s", featFn_pre, imageId, featFn_suf);
|
|
return readFeatures(featFn);
|
|
}
|