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

- rename vSLAMexample to vSFMexample 

- add vISAMexample. NOT working yet
release/4.3a0
Duy-Nguyen Ta 2010-10-21 22:51:20 +00:00
parent 6458ac46f8
commit 099552dbc9
6 changed files with 708 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

118
examples/vSLAMexample/ISAMLoop-inl.h Normal file
View File

@ -0,0 +1,118 @@
/*
* ISAMLoop.cpp
*
* Created on: Jan 19, 2010
* Author: Viorela Ila and Richard Roberts
*/
#pragma once
#include <gtsam/linear/GaussianFactorGraph.h>
//#include <BayesTree-inl.h>
#include <gtsam/inference/ISAM-inl.h>
#include <gtsam/nonlinear/Ordering.h>
//#include <gtsam/inference/IndexTable.h>
#include <boost/foreach.hpp>
#include "ISAMLoop.h"
using namespace gtsam;
/* ************************************************************************* */
template<class Values>
void ISAMLoop<Values>::update(const Factors& newFactors, const Values& initialValues) {
// Reorder and relinearize every reorderInterval updates
if(newFactors.size() > 0) {
if(reorderInterval_ > 0 && ++reorderCounter_ >= reorderInterval_) {
reorder_relinearize();
reorderCounter_ = 0;
}
factors_.push_back(newFactors);
// BOOST_FOREACH(typename Factors::sharedFactor f, newFactors) {
// f->print("Adding factor: ");
// }
// 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());
}
}
ordering_.print();
newFactors.linearize(linPoint_, ordering_);
cout << "Don linearize!" << endl;
boost::shared_ptr<GaussianFactorGraph> linearizedNewFactors(newFactors.linearize(linPoint_, ordering_));
cout << "After linearize: " << endl;
BOOST_FOREACH(GaussianFactorGraph::sharedFactor f, *linearizedNewFactors) {
f->print("Linearized factor: ");
}
isam.update(*linearizedNewFactors);
}
}
/* ************************************************************************* */
template<class Values>
void ISAMLoop<Values>::reorder_relinearize() {
//cout << "Reordering " << reorderCounter_;
cout << "Reordering, relinearizing..." << endl;
// Obtain the new linearization point
const Values newLinPoint = estimate();
isam.clear();
// Compute an ordering
ordering_ = *factors_.orderingCOLAMD(newLinPoint);
// cout << "Got estimate" << endl;
// newLinPoint.print("newLinPoint");
// factors_.print("factors");
// Create a linear factor graph at the new linearization point
boost::shared_ptr<GaussianFactorGraph> gfg(factors_.linearize(newLinPoint, ordering_));
// Just recreate the whole BayesTree
isam.update(*gfg);
//cout << "Reeliminating..." << endl;
// // Eliminate linear factor graph to a BayesNet with colamd ordering
// Ordering ordering = gfg->getOrdering();
// const BayesNet<GaussianConditional> bn(
// eliminate<GaussianFactor, GaussianConditional>(*gfg, ordering));
//
//// cout << "Rebuilding BayesTree..." << endl;
//
// // Replace the BayesTree with a new one
// isam.clear();
// BOOST_REVERSE_FOREACH(const GaussianISAM::sharedConditional c, bn) {
// isam.insert(c, ordering);
// }
linPoint_ = newLinPoint;
// cout << "Done!" << endl;
}
/* ************************************************************************* */
template<class Values>
Values ISAMLoop<Values>::estimate() {
// cout << "ISAMLoop::estimate(): " << endl;
// linPoint_.print("linPoint_");
// isam.print("isam");
if(isam.size() > 0)
return linPoint_.expmap(optimize(isam), ordering_);
else
return linPoint_;
}

68
examples/vSLAMexample/ISAMLoop.h Normal file
View File

@ -0,0 +1,68 @@
/*
* ISAMLoop.h
*
* Created on: Jan 19, 2010
* Author: Viorela Ila and Richard Roberts
*/
#pragma once
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/linear/GaussianISAM.h>
template<class Values>
class ISAMLoop {
public:
typedef gtsam::NonlinearFactorGraph<Values> Factors;
public:
//protected:
/** The internal iSAM object */
gtsam::GaussianISAM isam;
/** The current linearization point */
Values linPoint_;
/** The ordering */
gtsam::Ordering ordering_;
/** The original factors, used when relinearizing */
Factors factors_;
/** The reordering interval and counter */
int reorderInterval_;
int reorderCounter_;
public:
ISAMLoop() : reorderInterval_(0), reorderCounter_(0) {}
/** Periodically reorder and relinearize */
ISAMLoop(int reorderInterval) : reorderInterval_(reorderInterval), reorderCounter_(0) {}
/** Add new factors along with their initial linearization points */
void update(const Factors& newFactors, const Values& initialValues);
/** Return the current solution estimate */
Values estimate();
Values calculateEstimate() { return estimate(); }
/** Return the current linearization point */
const Values& getLinearizationPoint() { return linPoint_; }
/** Get the ordering */
const gtsam::Ordering& getOrdering() const { return ordering_; }
const Factors& getFactorsUnsafe() { return factors_; }
/**
* Relinearization and reordering of variables
*/
void reorder_relinearize();
};

136
examples/vSLAMexample/vISAMexample.cpp Normal file
View File

@ -0,0 +1,136 @@
/**
* @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 <gtsam/inference/ISAM-inl.h>
#include <gtsam/linear/GaussianISAM.h>
#include "ISAMLoop.h"
#include "ISAMLoop-inl.h"
#include "vSLAMutils.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 "/posesISAM.txt"
#define MEASUREMENTS_FILE "/measurementsISAM.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>
std::vector<Pose3> g_poses; // map: <camera_id, pose>
std::vector<std::vector<Feature2D> > g_measurements; // map: <camera_id, detected_features> -- where: Feature2D: {camera_id, landmark_id, 2d feature_position}
// Noise models
SharedGaussian measurementSigma(noiseModel::Isotropic::Sigma(2, 5.0f));
SharedGaussian poseSigma(noiseModel::Unit::Create(1));
/* ************************************************************************* */
/**
* 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 = 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);
}
/* ************************************************************************* */
/**
* Setup vSLAM graph
* by adding and linking 2D features (measurements) detected in each captured image
* with their corresponding landmarks.
*/
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)
{
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);
}
newFactors->addPosePrior(pose_id, pose, poseSigma);
initialValues = shared_ptr<Values>(new Values());
initialValues->insert(pose_id, pose);
}
/* ************************************************************************* */
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);
}
g_dataFolder = string(argv[1]);
readAllData();
ISAMLoop<Values> isam(3);
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);
cout << "Add prior pose and measurements of camera " << i << endl;
newFactors->print();
initialValues->print();
isam.update(*newFactors, *initialValues);
Values currentEstimate = isam.estimate();
currentEstimate.print("Current estimate: ");
}
}
/* ************************************************************************* */

148
examples/vSLAMexample/vSFMexample.cpp Normal file
View File

@ -0,0 +1,148 @@
/**
* @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 "vSLAMutils.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: 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);
}
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 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: ");
}
/* ************************************************************************* */

211
examples/vSLAMexample/vSLAMutils.cpp Normal file
View File

@ -0,0 +1,211 @@
#include "vSLAMutils.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;
}
/* ************************************************************************* */
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: " << 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;
}

27
examples/vSLAMexample/vSLAMutils.h Normal file
View File

@ -0,0 +1,27 @@
#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);
std::vector<gtsam::Pose3> readPosesISAM(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);
std::vector< std::vector<Feature2D> > readAllMeasurementsISAM(const std::string& baseFolder, const std::string& measurementsFn);
#endif // LANDMARKUTILS_H