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