delete old files
Duy-Nguyen Ta
parent
099552dbc9
commit
5add714480
|
|
@ -1,163 +0,0 @@
|
||||||
#include "landmarkUtils.h"
|
|
||||||
#include <fstream>
|
|
||||||
#include <cstdio>
|
|
||||||
|
|
||||||
using namespace gtsam;
|
|
||||||
using namespace std;
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
std::map<int, Point3> readLandMarks(const std::string& landmarkFile)
|
|
||||||
{
|
|
||||||
ifstream file(landmarkFile.c_str());
|
|
||||||
if (!file) {
|
|
||||||
cout << "Cannot read landmark file: " << landmarkFile << endl;
|
|
||||||
exit(0);
|
|
||||||
}
|
|
||||||
|
|
||||||
int num;
|
|
||||||
file >> num;
|
|
||||||
std::map<int, Point3> landmarks;
|
|
||||||
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 landmarks;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
/**
|
|
||||||
* 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;
|
|
||||||
}
|
|
||||||
/* ************************************************************************* */
|
|
||||||
std::map<int, gtsam::Pose3> readPoses(const std::string& baseFolder, const std::string& posesFn)
|
|
||||||
{
|
|
||||||
ifstream posesFile((baseFolder+posesFn).c_str());
|
|
||||||
if (!posesFile)
|
|
||||||
{
|
|
||||||
cout << "Cannot read all pose file: " << posesFn << endl;
|
|
||||||
exit(0);
|
|
||||||
}
|
|
||||||
int numPoses;
|
|
||||||
posesFile >> numPoses;
|
|
||||||
map<int, Pose3> poses;
|
|
||||||
for (int i = 0; i<numPoses; i++)
|
|
||||||
{
|
|
||||||
int poseId;
|
|
||||||
posesFile >> poseId;
|
|
||||||
|
|
||||||
string poseFileName;
|
|
||||||
posesFile >> poseFileName;
|
|
||||||
|
|
||||||
Pose3 pose = readPose((baseFolder+poseFileName).c_str());
|
|
||||||
poses[poseId] = pose;
|
|
||||||
}
|
|
||||||
|
|
||||||
return poses;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
gtsam::shared_ptrK readCalibData(const std::string& calibFn)
|
|
||||||
{
|
|
||||||
ifstream calibFile(calibFn.c_str());
|
|
||||||
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();
|
|
||||||
|
|
||||||
return shared_ptrK(new Cal3_S2(fx, fy, 0, ox, oy)); // skew factor = 0
|
|
||||||
}
|
|
||||||
/* ************************************************************************* */
|
|
||||||
std::vector<Feature2D> readFeatures(int pose_id, 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 landmark_id; double x, y;
|
|
||||||
file >> landmark_id >> x >> y;
|
|
||||||
vFeatures_.push_back(Feature2D(pose_id, landmark_id, Point2(x, y)));
|
|
||||||
}
|
|
||||||
|
|
||||||
file.close();
|
|
||||||
return vFeatures_;
|
|
||||||
}
|
|
||||||
/* ************************************************************************* */
|
|
||||||
std::vector<Feature2D> readAllMeasurements(const std::string& baseFolder, const std::string& measurementsFn)
|
|
||||||
{;
|
|
||||||
ifstream measurementsFile((baseFolder+measurementsFn).c_str());
|
|
||||||
if (!measurementsFile)
|
|
||||||
{
|
|
||||||
cout << "Cannot read all pose file: " << measurementsFn << endl;
|
|
||||||
exit(0);
|
|
||||||
}
|
|
||||||
int numPoses;
|
|
||||||
measurementsFile >> numPoses;
|
|
||||||
|
|
||||||
vector<Feature2D> allFeatures;
|
|
||||||
allFeatures.clear();
|
|
||||||
|
|
||||||
for (int i = 0; i<numPoses; i++)
|
|
||||||
{
|
|
||||||
int poseId;
|
|
||||||
measurementsFile >> poseId;
|
|
||||||
|
|
||||||
string featureFileName;
|
|
||||||
measurementsFile >> featureFileName;
|
|
||||||
vector<Feature2D> features = readFeatures(poseId, (baseFolder+featureFileName).c_str());
|
|
||||||
allFeatures.insert( allFeatures.end(), features.begin(), features.end() );
|
|
||||||
}
|
|
||||||
|
|
||||||
return allFeatures;
|
|
||||||
}
|
|
||||||
|
|
@ -1,23 +0,0 @@
|
||||||
#ifndef LANDMARKUTILS_H
|
|
||||||
#define LANDMARKUTILS_H
|
|
||||||
|
|
||||||
#include <map>
|
|
||||||
#include <vector>
|
|
||||||
#include "Feature2D.h"
|
|
||||||
#include "gtsam/geometry/Pose3.h"
|
|
||||||
#include "gtsam/geometry/Point3.h"
|
|
||||||
#include "gtsam/geometry/Cal3_S2.h"
|
|
||||||
|
|
||||||
|
|
||||||
std::map<int, gtsam::Point3> readLandMarks(const std::string& landmarkFile);
|
|
||||||
|
|
||||||
gtsam::Pose3 readPose(const char* poseFn);
|
|
||||||
std::map<int, gtsam::Pose3> readPoses(const std::string& baseFolder, const std::string& posesFN);
|
|
||||||
|
|
||||||
gtsam::shared_ptrK readCalibData(const std::string& calibFn);
|
|
||||||
|
|
||||||
std::vector<Feature2D> readFeatureFile(const char* filename);
|
|
||||||
std::vector<Feature2D> readAllMeasurements(const std::string& baseFolder, const std::string& measurementsFn);
|
|
||||||
|
|
||||||
|
|
||||||
#endif // LANDMARKUTILS_H
|
|
||||||
|
|
@ -1,148 +0,0 @@
|
||||||
/**
|
|
||||||
* @file vSLAMexample.cpp
|
|
||||||
* @brief An vSLAM example for synthesis sequence
|
|
||||||
* single camera
|
|
||||||
* @author Duy-Nguyen
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include <boost/shared_ptr.hpp>
|
|
||||||
using namespace boost;
|
|
||||||
|
|
||||||
#define GTSAM_MAGIC_KEY
|
|
||||||
|
|
||||||
#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>
|
|
||||||
#include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
|
|
||||||
#include <gtsam/inference/graph-inl.h>
|
|
||||||
#include <gtsam/slam/visualSLAM.h>
|
|
||||||
#include <gtsam/slam/PriorFactor.h>
|
|
||||||
|
|
||||||
#include "landmarkUtils.h"
|
|
||||||
#include "Feature2D.h"
|
|
||||||
|
|
||||||
using namespace std;
|
|
||||||
using namespace gtsam;
|
|
||||||
using namespace gtsam::visualSLAM;
|
|
||||||
using namespace boost;
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
#define CALIB_FILE "calib.txt"
|
|
||||||
#define LANDMARKS_FILE "landmarks.txt"
|
|
||||||
#define POSES_FILE "poses.txt"
|
|
||||||
#define MEASUREMENTS_FILE "measurements.txt"
|
|
||||||
|
|
||||||
// Base data folder
|
|
||||||
string g_dataFolder;
|
|
||||||
|
|
||||||
// Store groundtruth values, read from files
|
|
||||||
shared_ptrK g_calib;
|
|
||||||
map<int, Point3> g_landmarks; // map: <landmark_id, landmark_position>
|
|
||||||
map<int, Pose3> g_poses; // map: <camera_id, pose>
|
|
||||||
std::vector<Feature2D> g_measurements; // Feature2D: {camera_id, landmark_id, 2d feature_position}
|
|
||||||
|
|
||||||
// Noise models
|
|
||||||
SharedGaussian measurementSigma(noiseModel::Isotropic::Sigma(2, 5.0f));
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
/**
|
|
||||||
* Read all data: calibration file, landmarks, poses, and all features measurements
|
|
||||||
* Data is stored in global variables.
|
|
||||||
*/
|
|
||||||
void readAllData()
|
|
||||||
{
|
|
||||||
g_calib = readCalibData(g_dataFolder + CALIB_FILE);
|
|
||||||
|
|
||||||
// Read groundtruth landmarks' positions. These will be used later as intial estimates for landmark nodes.
|
|
||||||
g_landmarks = readLandMarks(g_dataFolder + LANDMARKS_FILE);
|
|
||||||
|
|
||||||
// Read groundtruth camera poses. These will be used later as intial estimates for pose nodes.
|
|
||||||
g_poses = readPoses(g_dataFolder, POSES_FILE);
|
|
||||||
|
|
||||||
// Read all 2d measurements. Those will become factors linking their associating pose and the corresponding landmark.
|
|
||||||
g_measurements = readAllMeasurements(g_dataFolder, MEASUREMENTS_FILE);
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
/**
|
|
||||||
* Setup vSLAM graph
|
|
||||||
* by adding and linking 2D features (measurements) detected in each captured image
|
|
||||||
* with their corresponding landmarks.
|
|
||||||
*/
|
|
||||||
Graph setupGraph(std::vector<Feature2D>& measurements, SharedGaussian measurementSigma, shared_ptrK calib)
|
|
||||||
{
|
|
||||||
Graph g;
|
|
||||||
|
|
||||||
cout << "Built graph: " << endl;
|
|
||||||
for (size_t i= 0; i<measurements.size(); i++)
|
|
||||||
{
|
|
||||||
measurements[i].print();
|
|
||||||
|
|
||||||
g.addMeasurement(measurements[i].m_p,
|
|
||||||
measurementSigma,
|
|
||||||
measurements[i].m_idCamera,
|
|
||||||
measurements[i].m_idLandmark,
|
|
||||||
calib);
|
|
||||||
}
|
|
||||||
|
|
||||||
return g;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
/**
|
|
||||||
* Create a structure of initial estimates for all nodes (landmarks and poses) in the graph.
|
|
||||||
* The returned Values structure contains all initial values for all nodes.
|
|
||||||
*/
|
|
||||||
Values initialize(std::map<int, Point3> landmarks, std::map<int, Pose3> poses)
|
|
||||||
{
|
|
||||||
Values initValues;
|
|
||||||
|
|
||||||
// Initialize landmarks 3D positions.
|
|
||||||
for (map<int, Point3>::iterator lmit = landmarks.begin(); lmit != landmarks.end(); lmit++)
|
|
||||||
initValues.insert( lmit->first, lmit->second );
|
|
||||||
|
|
||||||
// Initialize camera poses.
|
|
||||||
for (map<int, Pose3>::iterator poseit = poses.begin(); poseit != poses.end(); poseit++)
|
|
||||||
initValues.insert( poseit->first, poseit->second);
|
|
||||||
|
|
||||||
return initValues;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
int main(int argc, char* argv[])
|
|
||||||
{
|
|
||||||
if (argc <2)
|
|
||||||
{
|
|
||||||
cout << "Usage: vSLAMexample <DataFolder>" << endl << endl;
|
|
||||||
cout << "\tPlease specify <DataFolder>, which contains calibration file, initial landmarks, initial poses, and feature data." << endl;
|
|
||||||
cout << "\tSample folder is in $gtsam_source_folder$/examples/vSLAMexample/Data" << endl << endl;
|
|
||||||
cout << "Example usage: vSLAMexample '$gtsam_source_folder$/examples/vSLAMexample/Data'" << endl;
|
|
||||||
exit(0);
|
|
||||||
}
|
|
||||||
|
|
||||||
g_dataFolder = string(argv[1]);
|
|
||||||
readAllData();
|
|
||||||
|
|
||||||
// Create a graph using the 2D measurements (features) and the calibration data
|
|
||||||
shared_ptr<Graph> graph(new Graph(setupGraph(g_measurements, measurementSigma, g_calib)));
|
|
||||||
|
|
||||||
// Create an initial Values structure using groundtruth values as the initial estimates
|
|
||||||
boost::shared_ptr<Values> initialEstimates(new Values(initialize(g_landmarks, g_poses)));
|
|
||||||
cout << "*******************************************************" << endl;
|
|
||||||
initialEstimates->print("INITIAL ESTIMATES: ");
|
|
||||||
|
|
||||||
// Add prior factor for all poses in the graph
|
|
||||||
map<int, Pose3>::iterator poseit = g_poses.begin();
|
|
||||||
for (; poseit != g_poses.end(); poseit++)
|
|
||||||
graph->addPosePrior(poseit->first, poseit->second, noiseModel::Unit::Create(1));
|
|
||||||
|
|
||||||
// Optimize the graph
|
|
||||||
cout << "*******************************************************" << endl;
|
|
||||||
Optimizer::Parameters::verbosityLevel verborsity = Optimizer::Parameters::DAMPED;
|
|
||||||
Optimizer::shared_values result = Optimizer::optimizeGN( graph, initialEstimates, verborsity );
|
|
||||||
|
|
||||||
// Print final results
|
|
||||||
cout << "*******************************************************" << endl;
|
|
||||||
result->print("FINAL RESULTS: ");
|
|
||||||
|
|
||||||
}
|
|
||||||
/* ************************************************************************* */
|
|
||||||
|
|
||||||
Loading…
Reference in New Issue