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"

41
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
{
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
};

11
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
*

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
*

57
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
@ -54,8 +65,7 @@ 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()
{
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.
@ -73,14 +83,13 @@ void readAllDataISAM()
* 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)
{
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,
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,
@ -89,29 +98,24 @@ void createNewFactors(shared_ptr<Graph>& newFactors, boost::shared_ptr<Values>&
// ... 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++)
{
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 = 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] );
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)
{
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 << "\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);
@ -126,13 +130,10 @@ int main(int argc, char* argv[])
// 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++)
{
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);
createNewFactors(newFactors, initialValues, i, g_poses[i], g_measurements[i], measurementSigma, g_calib);
isam.update(*newFactors, *initialValues);
Values currentEstimate = isam.estimate();
@ -140,7 +141,7 @@ int main(int argc, char* argv[])
currentEstimate.print("Current estimate: ");
}
return 0;
}
/* ************************************************************************* */

49
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
@ -47,12 +58,12 @@ 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);
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);
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);
@ -67,16 +78,16 @@ void readAllData()
* 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 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++)
{
for (size_t i = 0; i < measurements.size(); i++) {
measurements[i].print();
g.addMeasurement(measurements[i].m_p,
g.addMeasurement(
measurements[i].m_p,
measurementSigma,
measurements[i].m_idCamera,
measurements[i].m_idLandmark,
@ -91,28 +102,27 @@ Graph setupGraph(std::vector<Feature2D>& measurements, SharedGaussian measuremen
* 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 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 );
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);
initValues.insert(poseit->first, poseit->second);
return initValues;
}
/* ************************************************************************* */
int main(int argc, char* argv[])
{
if (argc <2)
{
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 << "\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);
@ -137,12 +147,13 @@ int main(int argc, char* argv[])
// Optimize the graph
cout << "*******************************************************" << endl;
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
cout << "*******************************************************" << endl;
result->print("FINAL RESULTS: ");
return 0;
}
/* ************************************************************************* */

86
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,8 +23,7 @@ using namespace gtsam;
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());
if (!file) {
cout << "Cannot read landmark file: " << landmarkFile << endl;
@ -18,8 +34,7 @@ std::map<int, Point3> readLandMarks(const std::string& landmarkFile)
file >> num;
std::map<int, Point3> landmarks;
landmarks.clear();
for (int i = 0; i<num; i++)
{
for (int i = 0; i<num; i++) {
int color_id;
float x, y, z;
file >> color_id >> x >> y >> z;
@ -35,11 +50,9 @@ std::map<int, Point3> readLandMarks(const std::string& landmarkFile)
* Read pose from file, output by Panda3D.
* Warning: row major!!!
*/
gtsam::Pose3 readPose(const char* Fn)
{
gtsam::Pose3 readPose(const char* Fn) {
ifstream poseFile(Fn);
if (!poseFile)
{
if (!poseFile) {
cout << "Cannot read pose file: " << Fn << endl;
exit(0);
}
@ -52,8 +65,7 @@ gtsam::Pose3 readPose(const char* Fn)
// 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++)
{
for (int i = 0; i<3; i++) {
float t = v[4+i];
v[4+i] = v[8+i];
v[8+i] = -t;
@ -67,19 +79,16 @@ gtsam::Pose3 readPose(const char* Fn)
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());
if (!posesFile)
{
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++)
{
for (int i = 0; i<numPoses; i++) {
int poseId;
posesFile >> poseId;
@ -94,11 +103,9 @@ std::map<int, gtsam::Pose3> readPoses(const std::string& baseFolder, const std::
}
/* ************************************************************************* */
gtsam::shared_ptrK readCalibData(const std::string& calibFn)
{
gtsam::shared_ptrK readCalibData(const std::string& calibFn) {
ifstream calibFile(calibFn.c_str());
if (!calibFile)
{
if (!calibFile) {
cout << "Cannot read calib file: " << calibFn << endl;
exit(0);
}
@ -109,12 +116,11 @@ gtsam::shared_ptrK readCalibData(const std::string& calibFn)
return shared_ptrK(new Cal3_S2(fx, fy, 0, ox, oy)); // skew factor = 0
}
/* ************************************************************************* */
std::vector<Feature2D> readFeatures(int pose_id, const char* filename)
{
std::vector<Feature2D> readFeatures(int pose_id, const char* filename) {
ifstream file(filename);
if (!file)
{
if (!file) {
cout << "Cannot read feature file: " << filename<< endl;
exit(0);
}
@ -123,8 +129,7 @@ std::vector<Feature2D> readFeatures(int pose_id, const char* filename)
file >> numFeatures ;
std::vector<Feature2D> vFeatures_;
for (int i = 0; i < numFeatures; i++)
{
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)));
@ -133,12 +138,11 @@ std::vector<Feature2D> readFeatures(int pose_id, const char* filename)
file.close();
return vFeatures_;
}
/* ************************************************************************* */
std::vector<Feature2D> readAllMeasurements(const std::string& baseFolder, const std::string& measurementsFn)
{
std::vector<Feature2D> readAllMeasurements(const std::string& baseFolder, const std::string& measurementsFn) {
ifstream measurementsFile((baseFolder+measurementsFn).c_str());
if (!measurementsFile)
{
if (!measurementsFile) {
cout << "Cannot read all pose file: " << baseFolder+measurementsFn << endl;
exit(0);
}
@ -148,8 +152,7 @@ std::vector<Feature2D> readAllMeasurements(const std::string& baseFolder, const
vector<Feature2D> allFeatures;
allFeatures.clear();
for (int i = 0; i<numPoses; i++)
{
for (int i = 0; i<numPoses; i++) {
int poseId;
measurementsFile >> poseId;
@ -161,20 +164,18 @@ std::vector<Feature2D> readAllMeasurements(const std::string& baseFolder, const
return allFeatures;
}
/* ************************************************************************* */
std::vector<gtsam::Pose3> readPosesISAM(const std::string& baseFolder, const std::string& posesFn)
{
std::vector<gtsam::Pose3> readPosesISAM(const std::string& baseFolder, const std::string& posesFn) {
ifstream posesFile((baseFolder+posesFn).c_str());
if (!posesFile)
{
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++)
{
for (int i = 0; i<numPoses; i++) {
string poseFileName;
posesFile >> poseFileName;
@ -186,11 +187,9 @@ std::vector<gtsam::Pose3> readPosesISAM(const std::string& baseFolder, const std
}
/* ************************************************************************* */
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) {
ifstream measurementsFile((baseFolder+measurementsFn).c_str());
if (!measurementsFile)
{
if (!measurementsFile) {
cout << "Cannot read all pose file: " << baseFolder+measurementsFn << endl;
exit(0);
}
@ -199,8 +198,7 @@ std::vector<std::vector<Feature2D> > readAllMeasurementsISAM(const std::string&
std::vector<std::vector<Feature2D> > allFeatures;
for (int i = 0; i<numPoses; i++)
{
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

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