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

Code formatting and inserting copyright notice

release/4.3a0
Richard Roberts 2010-10-26 15:01:34 +00:00
parent 67ecfed86e
commit 07532b815b
8 changed files with 440 additions and 366 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
examples/vSLAMexample/Feature2D.cpp
View File

@ -1,2 +1,19 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file Feature2D.cpp
* @brief
* @author Duy-Nguyen
*/
#include "Feature2D.h" #include "Feature2D.h"

55
examples/vSLAMexample/Feature2D.h
View File

@ -1,28 +1,41 @@
#ifndef FEATURE2D_H /* ----------------------------------------------------------------------------
#define FEATURE2D_H
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file Feature2D.h
* @brief
* @author Duy-Nguyen
*/
#pragma once
#include "gtsam/geometry/Point2.h" #include "gtsam/geometry/Point2.h"
#include <iostream> #include <iostream>
class Feature2D struct Feature2D
{ {
public:
gtsam::Point2 m_p;
int m_idCamera; // id of the camera pose that makes this measurement
int m_idLandmark; // id of the 3D landmark that it is associated with
public:
Feature2D(int idCamera, int idLandmark, gtsam::Point2 p)
:m_p(p),
m_idCamera(idCamera),
m_idLandmark(idLandmark)
{};
void print(const std::string& s = "") const gtsam::Point2 m_p;
{ int m_idCamera; // id of the camera pose that makes this measurement
std::cout << s << std::endl; int m_idLandmark; // id of the 3D landmark that it is associated with
std::cout << "Pose id: " << m_idCamera << " -- Landmark id: " << m_idLandmark << std::endl;
m_p.print("\tMeasurement: "); Feature2D(int idCamera, int idLandmark, gtsam::Point2 p) :
} m_p(p),
m_idCamera(idCamera),
m_idLandmark(idLandmark) {}
void print(const std::string& s = "") const {
std::cout << s << std::endl;
std::cout << "Pose id: " << m_idCamera << " -- Landmark id: " << m_idLandmark << std::endl;
m_p.print("\tMeasurement: ");
}
}; };
#endif // FEATURE2D_H

51
examples/vSLAMexample/ISAMLoop-inl.h
View File

@ -1,3 +1,14 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/* /*
* ISAMLoop.cpp * ISAMLoop.cpp
* *
@ -22,29 +33,29 @@ void ISAMLoop<Values>::update(const Factors& newFactors, const Values& initialVa
if(newFactors.size() > 0) { if(newFactors.size() > 0) {
// Reorder and relinearize every reorderInterval updates // Reorder and relinearize every reorderInterval updates
if(reorderInterval_ > 0 && ++reorderCounter_ >= reorderInterval_) { if(reorderInterval_ > 0 && ++reorderCounter_ >= reorderInterval_) {
reorder_relinearize(); reorder_relinearize();
reorderCounter_ = 0; reorderCounter_ = 0;
}
factors_.push_back(newFactors);
// Linearize new factors and insert them
// TODO: optimize for whole config?
linPoint_.insert(initialValues);
// Augment ordering
BOOST_FOREACH(const typename Factors::sharedFactor& factor, newFactors) {
BOOST_FOREACH(const Symbol& key, factor->keys()) {
ordering_.tryInsert(key, ordering_.nVars());
} }
}
factors_.push_back(newFactors); boost::shared_ptr<GaussianFactorGraph> linearizedNewFactors(newFactors.linearize(linPoint_, ordering_));
// Linearize new factors and insert them // Update ISAM
// TODO: optimize for whole config? isam.update(*linearizedNewFactors);
linPoint_.insert(initialValues);
// Augment ordering
BOOST_FOREACH(const typename Factors::sharedFactor& factor, newFactors) {
BOOST_FOREACH(const Symbol& key, factor->keys()) {
ordering_.tryInsert(key, ordering_.nVars());
}
}
boost::shared_ptr<GaussianFactorGraph> linearizedNewFactors(newFactors.linearize(linPoint_, ordering_));
// Update ISAM
isam.update(*linearizedNewFactors);
} }
} }

11
examples/vSLAMexample/ISAMLoop.h
View File

@ -1,3 +1,14 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/* /*
* ISAMLoop.h * ISAMLoop.h
* *

143
examples/vSLAMexample/vISAMexample.cpp
View File

@ -1,3 +1,14 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/** /**
* @file vSLAMexample.cpp * @file vSLAMexample.cpp
* @brief An vSLAM example for synthesis sequence * @brief An vSLAM example for synthesis sequence
@ -51,96 +62,86 @@ SharedGaussian poseSigma(noiseModel::Unit::Create(1));
/* ************************************************************************* */ /* ************************************************************************* */
/** /**
* Read all data: calibration file, landmarks, poses, and all features measurements * Read all data: calibration file, landmarks, poses, and all features measurements
* Data is stored in global variables, which are used later to simulate incremental updates. * Data is stored in global variables, which are used later to simulate incremental updates.
*/ */
void readAllDataISAM() void readAllDataISAM() {
{ g_calib = readCalibData(g_dataFolder + CALIB_FILE);
g_calib = readCalibData(g_dataFolder + CALIB_FILE);
// Read groundtruth landmarks' positions. These will be used later as intial estimates and priors for landmark nodes. // Read groundtruth landmarks' positions. These will be used later as intial estimates and priors for landmark nodes.
g_landmarks = readLandMarks(g_dataFolder + LANDMARKS_FILE); g_landmarks = readLandMarks(g_dataFolder + LANDMARKS_FILE);
// Read groundtruth camera poses. These will be used later as intial estimates for pose nodes. // Read groundtruth camera poses. These will be used later as intial estimates for pose nodes.
g_poses = readPosesISAM(g_dataFolder, POSES_FILE); g_poses = readPosesISAM(g_dataFolder, POSES_FILE);
// Read all 2d measurements. Those will become factors linking their associating pose and the corresponding landmark. // Read all 2d measurements. Those will become factors linking their associating pose and the corresponding landmark.
g_measurements = readAllMeasurementsISAM(g_dataFolder, MEASUREMENTS_FILE); g_measurements = readAllMeasurementsISAM(g_dataFolder, MEASUREMENTS_FILE);
} }
/* ************************************************************************* */ /* ************************************************************************* */
/** /**
* Setup newFactors and initialValues for each new pose and set of measurements at each frame. * Setup newFactors and initialValues for each new pose and set of measurements at each frame.
*/ */
void createNewFactors(shared_ptr<Graph>& newFactors, boost::shared_ptr<Values>& initialValues, void createNewFactors(shared_ptr<Graph>& newFactors, boost::shared_ptr<Values>& initialValues,
int pose_id, Pose3& pose, int pose_id, Pose3& pose, std::vector<Feature2D>& measurements, SharedGaussian measurementSigma, shared_ptrK calib) {
std::vector<Feature2D>& measurements, SharedGaussian measurementSigma, shared_ptrK calib)
{
// Create a graph of newFactors with new measurements
newFactors = shared_ptr<Graph>(new Graph());
for (size_t i= 0; i<measurements.size(); i++)
{
newFactors->addMeasurement(measurements[i].m_p,
measurementSigma,
pose_id,
measurements[i].m_idLandmark,
calib);
}
// ... we need priors on the new pose and all new landmarks // Create a graph of newFactors with new measurements
newFactors->addPosePrior(pose_id, pose, poseSigma); newFactors = shared_ptr<Graph> (new Graph());
for (size_t i= 0; i<measurements.size(); i++) for (size_t i = 0; i < measurements.size(); i++) {
{ newFactors->addMeasurement(
newFactors->addPointPrior(measurements[i].m_idLandmark, g_landmarks[measurements[i].m_idLandmark]); measurements[i].m_p,
} measurementSigma,
pose_id,
measurements[i].m_idLandmark,
calib);
}
// ... we need priors on the new pose and all new landmarks
newFactors->addPosePrior(pose_id, pose, poseSigma);
for (size_t i = 0; i < measurements.size(); i++) {
newFactors->addPointPrior(measurements[i].m_idLandmark, g_landmarks[measurements[i].m_idLandmark]);
}
// Create initial values for all nodes in the newFactors // Create initial values for all nodes in the newFactors
initialValues = shared_ptr<Values>(new Values()); initialValues = shared_ptr<Values> (new Values());
initialValues->insert(pose_id, pose); initialValues->insert(pose_id, pose);
for (size_t i= 0; i<measurements.size(); i++) for (size_t i = 0; i < measurements.size(); i++) {
{ initialValues->insert(measurements[i].m_idLandmark, g_landmarks[measurements[i].m_idLandmark]);
initialValues->insert( measurements[i].m_idLandmark, g_landmarks[measurements[i].m_idLandmark] ); }
}
} }
/* ************************************************************************* */ /* ************************************************************************* */
int main(int argc, char* argv[]) int main(int argc, char* argv[]) {
{ if (argc < 2) {
if (argc <2) cout << "Usage: vISAMexample <DataFolder>" << endl << endl;
{ cout << "\tPlease specify <DataFolder>, which contains calibration file, initial\n"
cout << "Usage: vISAMexample <DataFolder>" << endl << endl; "landmarks, initial poses, and feature data." << 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 << "\tSample folder is in $gtsam_source_folder$/examples/vSLAMexample/Data/" << endl << endl; cout << "Example usage: vISAMexample '$gtsam_source_folder$/examples/vSLAMexample/Data/'" << endl;
cout << "Example usage: vISAMexample '$gtsam_source_folder$/examples/vSLAMexample/Data/'" << endl; exit(0);
exit(0); }
}
g_dataFolder = string(argv[1]) + "/"; g_dataFolder = string(argv[1]) + "/";
readAllDataISAM(); readAllDataISAM();
// Create an ISAMLoop which will be relinearized and reordered after every "relinearizeInterval" updates // Create an ISAMLoop which will be relinearized and reordered after every "relinearizeInterval" updates
int relinearizeInterval = 3; int relinearizeInterval = 3;
ISAMLoop<Values> isam(relinearizeInterval); ISAMLoop<Values> isam(relinearizeInterval);
// At each frame i with new camera pose and new set of measurements associated with it, // At each frame i with new camera pose and new set of measurements associated with it,
// create a graph of new factors and update ISAM // create a graph of new factors and update ISAM
for (size_t i = 0; i<g_measurements.size(); i++) for (size_t i = 0; i < g_measurements.size(); i++) {
{ shared_ptr<Graph> newFactors;
shared_ptr<Graph> newFactors; shared_ptr<Values> initialValues;
shared_ptr<Values> initialValues; createNewFactors(newFactors, initialValues, i, g_poses[i], g_measurements[i], measurementSigma, g_calib);
createNewFactors(newFactors, initialValues,
i, g_poses[i],
g_measurements[i], measurementSigma, g_calib);
isam.update(*newFactors, *initialValues); isam.update(*newFactors, *initialValues);
Values currentEstimate = isam.estimate(); Values currentEstimate = isam.estimate();
cout << "****************************************************" << endl; cout << "****************************************************" << endl;
currentEstimate.print("Current estimate: "); currentEstimate.print("Current estimate: ");
} }
return 0;
} }
/* ************************************************************************* */ /* ************************************************************************* */

155
examples/vSLAMexample/vSFMexample.cpp
View File

@ -1,3 +1,14 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/** /**
* @file vSLAMexample.cpp * @file vSLAMexample.cpp
* @brief An vSLAM example for synthesis sequence * @brief An vSLAM example for synthesis sequence
@ -44,105 +55,105 @@ SharedGaussian measurementSigma(noiseModel::Isotropic::Sigma(2, 5.0f));
/* ************************************************************************* */ /* ************************************************************************* */
/** /**
* Read all data: calibration file, landmarks, poses, and all features measurements * Read all data: calibration file, landmarks, poses, and all features measurements
* Data is stored in global variables. * Data is stored in global variables.
*/ */
void readAllData() 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_calib = readCalibData(g_dataFolder + "/" + CALIB_FILE);
g_landmarks = readLandMarks(g_dataFolder +"/"+ LANDMARKS_FILE);
// Read groundtruth camera poses. These will be used later as intial estimates for pose nodes. // Read groundtruth landmarks' positions. These will be used later as intial estimates for landmark nodes.
g_poses = readPoses(g_dataFolder, POSES_FILE); g_landmarks = readLandMarks(g_dataFolder + "/" + LANDMARKS_FILE);
// Read all 2d measurements. Those will become factors linking their associating pose and the corresponding landmark. // Read groundtruth camera poses. These will be used later as intial estimates for pose nodes.
g_measurements = readAllMeasurements(g_dataFolder, MEASUREMENTS_FILE); 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 * Setup vSLAM graph
* by adding and linking 2D features (measurements) detected in each captured image * by adding and linking 2D features (measurements) detected in each captured image
* with their corresponding landmarks. * with their corresponding landmarks.
*/ */
Graph setupGraph(std::vector<Feature2D>& measurements, SharedGaussian measurementSigma, shared_ptrK calib) Graph setupGraph(std::vector<Feature2D>& measurements, SharedGaussian measurementSigma, shared_ptrK calib) {
{
Graph g;
cout << "Built graph: " << endl; Graph g;
for (size_t i= 0; i<measurements.size(); i++)
{
measurements[i].print();
g.addMeasurement(measurements[i].m_p, cout << "Built graph: " << endl;
measurementSigma, for (size_t i = 0; i < measurements.size(); i++) {
measurements[i].m_idCamera, measurements[i].print();
measurements[i].m_idLandmark,
calib);
}
return g; 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. * 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. * The returned Values structure contains all initial values for all nodes.
*/ */
Values initialize(std::map<int, Point3> landmarks, std::map<int, Pose3> poses) Values initialize(std::map<int, Point3> landmarks, std::map<int, Pose3> poses) {
{
Values initValues;
// Initialize landmarks 3D positions. Values initValues;
for (map<int, Point3>::iterator lmit = landmarks.begin(); lmit != landmarks.end(); lmit++)
initValues.insert( lmit->first, lmit->second );
// Initialize camera poses. // Initialize landmarks 3D positions.
for (map<int, Pose3>::iterator poseit = poses.begin(); poseit != poses.end(); poseit++) for (map<int, Point3>::iterator lmit = landmarks.begin(); lmit != landmarks.end(); lmit++)
initValues.insert( poseit->first, poseit->second); initValues.insert(lmit->first, lmit->second);
return initValues; // 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[]) int main(int argc, char* argv[]) {
{ if (argc < 2) {
if (argc <2) cout << "Usage: vSFMexample <DataFolder>" << endl << endl;
{ cout << "\tPlease specify <DataFolder>, which contains calibration file, initial\n"
cout << "Usage: vSFMexample <DataFolder>" << endl << endl; "landmarks, initial poses, and feature data." << 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 << "\tSample folder is in $gtsam_source_folder$/examples/vSLAMexample/Data" << endl << endl; cout << "Example usage: vSFMexample '$gtsam_source_folder$/examples/vSLAMexample/Data'" << endl;
cout << "Example usage: vSFMexample '$gtsam_source_folder$/examples/vSLAMexample/Data'" << endl; exit(0);
exit(0); }
}
g_dataFolder = string(argv[1]) + "/"; g_dataFolder = string(argv[1]) + "/";
readAllData(); readAllData();
// Create a graph using the 2D measurements (features) and the calibration data // Create a graph using the 2D measurements (features) and the calibration data
boost::shared_ptr<Graph> graph(new Graph(setupGraph(g_measurements, measurementSigma, g_calib))); boost::shared_ptr<Graph> graph(new Graph(setupGraph(g_measurements, measurementSigma, g_calib)));
// Create an initial Values structure using groundtruth values as the initial estimates // Create an initial Values structure using groundtruth values as the initial estimates
boost::shared_ptr<Values> initialEstimates(new Values(initialize(g_landmarks, g_poses))); boost::shared_ptr<Values> initialEstimates(new Values(initialize(g_landmarks, g_poses)));
cout << "*******************************************************" << endl; cout << "*******************************************************" << endl;
initialEstimates->print("INITIAL ESTIMATES: "); initialEstimates->print("INITIAL ESTIMATES: ");
// Add prior factor for all poses in the graph // Add prior factor for all poses in the graph
map<int, Pose3>::iterator poseit = g_poses.begin(); map<int, Pose3>::iterator poseit = g_poses.begin();
for (; poseit != g_poses.end(); poseit++) for (; poseit != g_poses.end(); poseit++)
graph->addPosePrior(poseit->first, poseit->second, noiseModel::Unit::Create(1)); graph->addPosePrior(poseit->first, poseit->second, noiseModel::Unit::Create(1));
// Optimize the graph // Optimize the graph
cout << "*******************************************************" << endl; cout << "*******************************************************" << endl;
Optimizer::Parameters::verbosityLevel verborsity = Optimizer::Parameters::DAMPED; Optimizer::Parameters::verbosityLevel verborsity = Optimizer::Parameters::DAMPED;
Optimizer::shared_values result = Optimizer::optimizeGN( graph, initialEstimates, verborsity ); Optimizer::shared_values result = Optimizer::optimizeGN(graph, initialEstimates, verborsity);
// Print final results // Print final results
cout << "*******************************************************" << endl; cout << "*******************************************************" << endl;
result->print("FINAL RESULTS: "); result->print("FINAL RESULTS: ");
return 0;
} }
/* ************************************************************************* */ /* ************************************************************************* */

350
examples/vSLAMexample/vSLAMutils.cpp
View File

@ -1,3 +1,20 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file VSLAMutils.cpp
* @brief
* @author Duy-Nguyen
*/
#include "vSLAMutils.h" #include "vSLAMutils.h"
#include <fstream> #include <fstream>
#include <cstdio> #include <cstdio>
@ -6,206 +23,187 @@ using namespace gtsam;
using namespace std; using namespace std;
/* ************************************************************************* */ /* ************************************************************************* */
std::map<int, Point3> readLandMarks(const std::string& landmarkFile) std::map<int, Point3> readLandMarks(const std::string& landmarkFile) {
{ ifstream file(landmarkFile.c_str());
ifstream file(landmarkFile.c_str()); if (!file) {
if (!file) { cout << "Cannot read landmark file: " << landmarkFile << endl;
cout << "Cannot read landmark file: " << landmarkFile << endl; exit(0);
exit(0); }
}
int num; int num;
file >> num; file >> num;
std::map<int, Point3> landmarks; std::map<int, Point3> landmarks;
landmarks.clear(); landmarks.clear();
for (int i = 0; i<num; i++) for (int i = 0; i<num; i++) {
{ int color_id;
int color_id; float x, y, z;
float x, y, z; file >> color_id >> x >> y >> z;
file >> color_id >> x >> y >> z; landmarks[color_id] = Point3(x, y, z);
landmarks[color_id] = Point3(x, y, z); }
}
file.close(); file.close();
return landmarks; return landmarks;
} }
/* ************************************************************************* */ /* ************************************************************************* */
/** /**
* Read pose from file, output by Panda3D. * Read pose from file, output by Panda3D.
* Warning: row major!!! * Warning: row major!!!
*/ */
gtsam::Pose3 readPose(const char* Fn) gtsam::Pose3 readPose(const char* Fn) {
{ ifstream poseFile(Fn);
ifstream poseFile(Fn); if (!poseFile) {
if (!poseFile) cout << "Cannot read pose file: " << Fn << endl;
{ exit(0);
cout << "Cannot read pose file: " << Fn << endl; }
exit(0);
}
double v[16]; double v[16];
for (int i = 0; i<16; i++) for (int i = 0; i<16; i++)
poseFile >> v[i]; poseFile >> v[i];
poseFile.close(); poseFile.close();
// Because panda3d's camera is z-up, y-view, // 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 // 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 //... similar to OpenGL's camera
for (int i = 0; i<3; i++) for (int i = 0; i<3; i++) {
{ float t = v[4+i];
float t = v[4+i]; v[4+i] = v[8+i];
v[4+i] = v[8+i]; v[8+i] = -t;
v[8+i] = -t; }
}
::Vector vec = Vector_(16, v); ::Vector vec = Vector_(16, v);
Matrix T = Matrix_(4,4, vec); // column order !!! Matrix T = Matrix_(4,4, vec); // column order !!!
Pose3 pose(T); Pose3 pose(T);
return pose; return pose;
} }
/* ************************************************************************* */ /* ************************************************************************* */
std::map<int, gtsam::Pose3> readPoses(const std::string& baseFolder, const std::string& posesFn) std::map<int, gtsam::Pose3> readPoses(const std::string& baseFolder, const std::string& posesFn) {
{ ifstream posesFile((baseFolder+posesFn).c_str());
ifstream posesFile((baseFolder+posesFn).c_str()); if (!posesFile) {
if (!posesFile) cout << "Cannot read all pose file: " << posesFn << endl;
{ exit(0);
cout << "Cannot read all pose file: " << posesFn << endl; }
exit(0); int numPoses;
} posesFile >> numPoses;
int numPoses; map<int, Pose3> poses;
posesFile >> numPoses; for (int i = 0; i<numPoses; i++) {
map<int, Pose3> poses; int poseId;
for (int i = 0; i<numPoses; i++) posesFile >> poseId;
{
int poseId;
posesFile >> poseId;
string poseFileName; string poseFileName;
posesFile >> poseFileName; posesFile >> poseFileName;
Pose3 pose = readPose((baseFolder+poseFileName).c_str()); Pose3 pose = readPose((baseFolder+poseFileName).c_str());
poses[poseId] = pose; poses[poseId] = pose;
} }
return poses; return poses;
} }
/* ************************************************************************* */ /* ************************************************************************* */
gtsam::shared_ptrK readCalibData(const std::string& calibFn) gtsam::shared_ptrK readCalibData(const std::string& calibFn) {
{ ifstream calibFile(calibFn.c_str());
ifstream calibFile(calibFn.c_str()); if (!calibFile) {
if (!calibFile) cout << "Cannot read calib file: " << calibFn << endl;
{ exit(0);
cout << "Cannot read calib file: " << calibFn << endl; }
exit(0); int imX, imY;
} float fx, fy, ox, oy;
int imX, imY; calibFile >> imX >> imY >> fx >> fy >> ox >> oy;
float fx, fy, ox, oy; calibFile.close();
calibFile >> imX >> imY >> fx >> fy >> ox >> oy;
calibFile.close();
return shared_ptrK(new Cal3_S2(fx, fy, 0, ox, oy)); // skew factor = 0 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 (int 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: " << baseFolder+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;
}
/* ************************************************************************* */
std::vector<gtsam::Pose3> readPosesISAM(const std::string& baseFolder, const std::string& posesFn)
{
ifstream posesFile((baseFolder+posesFn).c_str());
if (!posesFile)
{
cout << "Cannot read all pose ISAM file: " << posesFn << endl;
exit(0);
}
int numPoses;
posesFile >> numPoses;
vector<Pose3> poses;
for (int i = 0; i<numPoses; i++)
{
string poseFileName;
posesFile >> poseFileName;
Pose3 pose = readPose((baseFolder+poseFileName).c_str());
poses.push_back(pose);
}
return poses;
} }
/* ************************************************************************* */ /* ************************************************************************* */
std::vector<std::vector<Feature2D> > readAllMeasurementsISAM(const std::string& baseFolder, const std::string& measurementsFn) std::vector<Feature2D> readFeatures(int pose_id, const char* filename) {
{ ifstream file(filename);
ifstream measurementsFile((baseFolder+measurementsFn).c_str()); if (!file) {
if (!measurementsFile) cout << "Cannot read feature file: " << filename<< endl;
{ exit(0);
cout << "Cannot read all pose file: " << baseFolder+measurementsFn << endl; }
exit(0);
}
int numPoses;
measurementsFile >> numPoses;
std::vector<std::vector<Feature2D> > allFeatures; int numFeatures;
file >> numFeatures ;
for (int i = 0; i<numPoses; i++) std::vector<Feature2D> vFeatures_;
{ for (int i = 0; i < numFeatures; i++) {
string featureFileName; int landmark_id; double x, y;
measurementsFile >> featureFileName; file >> landmark_id >> x >> y;
vector<Feature2D> features = readFeatures(-1, (baseFolder+featureFileName).c_str()); // we don't care about pose id in ISAM vFeatures_.push_back(Feature2D(pose_id, landmark_id, Point2(x, y)));
allFeatures.push_back(features); }
}
return allFeatures; 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: " << baseFolder+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;
}
/* ************************************************************************* */
std::vector<gtsam::Pose3> readPosesISAM(const std::string& baseFolder, const std::string& posesFn) {
ifstream posesFile((baseFolder+posesFn).c_str());
if (!posesFile) {
cout << "Cannot read all pose ISAM file: " << posesFn << endl;
exit(0);
}
int numPoses;
posesFile >> numPoses;
vector<Pose3> poses;
for (int i = 0; i<numPoses; i++) {
string poseFileName;
posesFile >> poseFileName;
Pose3 pose = readPose((baseFolder+poseFileName).c_str());
poses.push_back(pose);
}
return poses;
}
/* ************************************************************************* */
std::vector<std::vector<Feature2D> > readAllMeasurementsISAM(const std::string& baseFolder, const std::string& measurementsFn) {
ifstream measurementsFile((baseFolder+measurementsFn).c_str());
if (!measurementsFile) {
cout << "Cannot read all pose file: " << baseFolder+measurementsFn << endl;
exit(0);
}
int numPoses;
measurementsFile >> numPoses;
std::vector<std::vector<Feature2D> > allFeatures;
for (int i = 0; i<numPoses; i++) {
string featureFileName;
measurementsFile >> featureFileName;
vector<Feature2D> features = readFeatures(-1, (baseFolder+featureFileName).c_str()); // we don't care about pose id in ISAM
allFeatures.push_back(features);
}
return allFeatures;
} }

24
examples/vSLAMexample/vSLAMutils.h
View File

@ -1,5 +1,21 @@
#ifndef LANDMARKUTILS_H /* ----------------------------------------------------------------------------
#define LANDMARKUTILS_H
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file Feature2D.cpp
* @brief
* @author Duy-Nguyen
*/
#pragma once
#include <map> #include <map>
#include <vector> #include <vector>
@ -21,7 +37,3 @@ gtsam::shared_ptrK readCalibData(const std::string& calibFn);
std::vector<Feature2D> readFeatureFile(const char* filename); std::vector<Feature2D> readFeatureFile(const char* filename);
std::vector<Feature2D> readAllMeasurements(const std::string& baseFolder, const std::string& measurementsFn); std::vector<Feature2D> readAllMeasurements(const std::string& baseFolder, const std::string& measurementsFn);
std::vector< std::vector<Feature2D> > readAllMeasurementsISAM(const std::string& baseFolder, const std::string& measurementsFn); std::vector< std::vector<Feature2D> > readAllMeasurementsISAM(const std::string& baseFolder, const std::string& measurementsFn);
#endif // LANDMARKUTILS_H