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"

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 <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
{
std::cout << s << std::endl;
std::cout << "Pose id: " << m_idCamera << " -- Landmark id: " << m_idLandmark << std::endl;
m_p.print("\tMeasurement: ");
}
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
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
*
@ -22,29 +33,29 @@ void ISAMLoop<Values>::update(const Factors& newFactors, const Values& initialVa
if(newFactors.size() > 0) {
// Reorder and relinearize every reorderInterval updates
if(reorderInterval_ > 0 && ++reorderCounter_ >= reorderInterval_) {
reorder_relinearize();
reorderCounter_ = 0;
// Reorder and relinearize every reorderInterval updates
if(reorderInterval_ > 0 && ++reorderCounter_ >= reorderInterval_) {
reorder_relinearize();
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
// 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());
}
}
boost::shared_ptr<GaussianFactorGraph> linearizedNewFactors(newFactors.linearize(linPoint_, ordering_));
// Update ISAM
isam.update(*linearizedNewFactors);
// 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
*

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
* @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
* Data is stored in global variables, which are used later to simulate incremental updates.
*/
void readAllDataISAM()
{
g_calib = readCalibData(g_dataFolder + CALIB_FILE);
* 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.
*/
void readAllDataISAM() {
g_calib = readCalibData(g_dataFolder + CALIB_FILE);
// Read groundtruth landmarks' positions. These will be used later as intial estimates and priors for landmark nodes.
g_landmarks = readLandMarks(g_dataFolder + LANDMARKS_FILE);
// Read groundtruth landmarks' positions. These will be used later as intial estimates and priors 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 = readPosesISAM(g_dataFolder, POSES_FILE);
// Read groundtruth camera poses. These will be used later as intial estimates for pose nodes.
g_poses = readPosesISAM(g_dataFolder, POSES_FILE);
// Read all 2d measurements. Those will become factors linking their associating pose and the corresponding landmark.
g_measurements = readAllMeasurementsISAM(g_dataFolder, MEASUREMENTS_FILE);
// Read all 2d measurements. Those will become factors linking their associating pose and the corresponding landmark.
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,
int pose_id, Pose3& pose,
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);
}
int pose_id, Pose3& pose, std::vector<Feature2D>& measurements, SharedGaussian measurementSigma, shared_ptrK 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 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
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
initialValues = shared_ptr<Values>(new Values());
initialValues->insert(pose_id, pose);
for (size_t i= 0; i<measurements.size(); i++)
{
initialValues->insert( measurements[i].m_idLandmark, g_landmarks[measurements[i].m_idLandmark] );
}
// Create initial values for all nodes in the newFactors
initialValues = shared_ptr<Values> (new Values());
initialValues->insert(pose_id, pose);
for (size_t i = 0; i < measurements.size(); i++) {
initialValues->insert(measurements[i].m_idLandmark, g_landmarks[measurements[i].m_idLandmark]);
}
}
/* ************************************************************************* */
int main(int argc, char* argv[])
{
if (argc <2)
{
cout << "Usage: vISAMexample <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: vISAMexample '$gtsam_source_folder$/examples/vSLAMexample/Data/'" << endl;
exit(0);
}
int main(int argc, char* argv[]) {
if (argc < 2) {
cout << "Usage: vISAMexample <DataFolder>" << endl << endl;
cout << "\tPlease specify <DataFolder>, which contains calibration file, initial\n"
"landmarks, initial poses, and feature data." << 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;
exit(0);
}
g_dataFolder = string(argv[1]) + "/";
readAllDataISAM();
g_dataFolder = string(argv[1]) + "/";
readAllDataISAM();
// Create an ISAMLoop which will be relinearized and reordered after every "relinearizeInterval" updates
int relinearizeInterval = 3;
ISAMLoop<Values> isam(relinearizeInterval);
// Create an ISAMLoop which will be relinearized and reordered after every "relinearizeInterval" updates
int relinearizeInterval = 3;
ISAMLoop<Values> isam(relinearizeInterval);
// 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
for (size_t i = 0; i<g_measurements.size(); i++)
{
shared_ptr<Graph> newFactors;
shared_ptr<Values> initialValues;
createNewFactors(newFactors, initialValues,
i, g_poses[i],
g_measurements[i], measurementSigma, g_calib);
// 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
for (size_t i = 0; i < g_measurements.size(); i++) {
shared_ptr<Graph> newFactors;
shared_ptr<Values> initialValues;
createNewFactors(newFactors, initialValues, i, g_poses[i], g_measurements[i], measurementSigma, g_calib);
isam.update(*newFactors, *initialValues);
Values currentEstimate = isam.estimate();
cout << "****************************************************" << endl;
currentEstimate.print("Current estimate: ");
}
isam.update(*newFactors, *initialValues);
Values currentEstimate = isam.estimate();
cout << "****************************************************" << endl;
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
* @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
* Data is stored in global variables.
*/
void readAllData()
{
g_calib = readCalibData(g_dataFolder +"/" + CALIB_FILE);
* Read all data: calibration file, landmarks, poses, and all features measurements
* Data is stored in global variables.
*/
void readAllData() {
// Read groundtruth landmarks' positions. These will be used later as intial estimates for landmark nodes.
g_landmarks = readLandMarks(g_dataFolder +"/"+ LANDMARKS_FILE);
g_calib = readCalibData(g_dataFolder + "/" + CALIB_FILE);
// Read groundtruth camera poses. These will be used later as intial estimates for pose nodes.
g_poses = readPoses(g_dataFolder, POSES_FILE);
// Read groundtruth landmarks' positions. These will be used later as intial estimates for landmark nodes.
g_landmarks = readLandMarks(g_dataFolder + "/" + LANDMARKS_FILE);
// Read all 2d measurements. Those will become factors linking their associating pose and the corresponding landmark.
g_measurements = readAllMeasurements(g_dataFolder, MEASUREMENTS_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;
* 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) {
cout << "Built graph: " << endl;
for (size_t i= 0; i<measurements.size(); i++)
{
measurements[i].print();
Graph g;
g.addMeasurement(measurements[i].m_p,
measurementSigma,
measurements[i].m_idCamera,
measurements[i].m_idLandmark,
calib);
}
cout << "Built graph: " << endl;
for (size_t i = 0; i < measurements.size(); i++) {
measurements[i].print();
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.
* 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;
* 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) {
// Initialize landmarks 3D positions.
for (map<int, Point3>::iterator lmit = landmarks.begin(); lmit != landmarks.end(); lmit++)
initValues.insert( lmit->first, lmit->second );
Values initValues;
// Initialize camera poses.
for (map<int, Pose3>::iterator poseit = poses.begin(); poseit != poses.end(); poseit++)
initValues.insert( poseit->first, poseit->second);
// Initialize landmarks 3D positions.
for (map<int, Point3>::iterator lmit = landmarks.begin(); lmit != landmarks.end(); lmit++)
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[])
{
if (argc <2)
{
cout << "Usage: vSFMexample <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: vSFMexample '$gtsam_source_folder$/examples/vSLAMexample/Data'" << endl;
exit(0);
}
int main(int argc, char* argv[]) {
if (argc < 2) {
cout << "Usage: vSFMexample <DataFolder>" << endl << endl;
cout << "\tPlease specify <DataFolder>, which contains calibration file, initial\n"
"landmarks, initial poses, and feature data." << 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;
exit(0);
}
g_dataFolder = string(argv[1]) + "/";
readAllData();
g_dataFolder = string(argv[1]) + "/";
readAllData();
// 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)));
// 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)));
// 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: ");
// 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));
// 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 );
// 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: ");
// Print final results
cout << "*******************************************************" << endl;
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 <fstream>
#include <cstdio>
@ -6,206 +23,187 @@ 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);
}
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);
}
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;
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);
}
* 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();
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;
}
// 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);
::Vector vec = Vector_(16, v);
Matrix T = Matrix_(4,4, vec); // column order !!!
Matrix T = Matrix_(4,4, vec); // column order !!!
Pose3 pose(T);
return pose;
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;
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;
string poseFileName;
posesFile >> poseFileName;
Pose3 pose = readPose((baseFolder+poseFileName).c_str());
poses[poseId] = pose;
}
Pose3 pose = readPose((baseFolder+poseFileName).c_str());
poses[poseId] = pose;
}
return poses;
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();
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 (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;
return shared_ptrK(new Cal3_S2(fx, fy, 0, ox, oy)); // skew factor = 0
}
/* ************************************************************************* */
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<Feature2D> readFeatures(int pose_id, const char* filename) {
ifstream file(filename);
if (!file) {
cout << "Cannot read feature file: " << filename<< endl;
exit(0);
}
std::vector<std::vector<Feature2D> > allFeatures;
int numFeatures;
file >> numFeatures ;
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);
}
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)));
}
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 <vector>
@ -21,7 +37,3 @@ 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);
std::vector< std::vector<Feature2D> > readAllMeasurementsISAM(const std::string& baseFolder, const std::string& measurementsFn);
#endif // LANDMARKUTILS_H