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

Updated BAL example with readBAL functionality (work in progress)

release/4.3a0
Luca Carlone 2013-10-19 02:05:51 +00:00
parent de5f8ee354
commit 916d730fce
4 changed files with 94 additions and 145 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

234
gtsam_unstable/examples/SmartProjectionFactorExampleBAL.cpp
View File

@ -43,6 +43,7 @@
// In GTSAM, measurement functions are represented as 'factors'. Several common factors // In GTSAM, measurement functions are represented as 'factors'. Several common factors
// have been provided with the library for solving robotics SLAM problems. // have been provided with the library for solving robotics SLAM problems.
#include <gtsam/slam/PriorFactor.h> #include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/dataset.h>
#include <gtsam_unstable/slam/SmartProjectionFactorsCreator.h> #include <gtsam_unstable/slam/SmartProjectionFactorsCreator.h>
#include <gtsam_unstable/slam/GenericProjectionFactorsCreator.h> #include <gtsam_unstable/slam/GenericProjectionFactorsCreator.h>
@ -58,7 +59,7 @@ using namespace gtsam;
using namespace boost::assign; using namespace boost::assign;
namespace NM = gtsam::noiseModel; namespace NM = gtsam::noiseModel;
// #define USE_BUNDLER #define USE_BUNDLER
using symbol_shorthand::X; using symbol_shorthand::X;
using symbol_shorthand::L; using symbol_shorthand::L;
@ -67,11 +68,11 @@ typedef PriorFactor<Pose3> Pose3Prior;
typedef FastMap<Key, int> OrderingMap; typedef FastMap<Key, int> OrderingMap;
#ifdef USE_BUNDLER #ifdef USE_BUNDLER
typedef SmartProjectionFactorsCreator<Pose3, Point3, Cal3Bundler> SmartFactorsCreator; typedef SmartProjectionFactorsCreator<Pose3, Point3, Cal3Bundler> SmartFactorsCreator;
typedef GenericProjectionFactorsCreator<Pose3, Point3, Cal3Bundler> ProjectionFactorsCreator; typedef GenericProjectionFactorsCreator<Pose3, Point3, Cal3Bundler> ProjectionFactorsCreator;
#else #else
typedef SmartProjectionFactorsCreator<Pose3, Point3, Cal3_S2> SmartFactorsCreator; typedef SmartProjectionFactorsCreator<Pose3, Point3, Cal3_S2> SmartFactorsCreator;
typedef GenericProjectionFactorsCreator<Pose3, Point3, Cal3_S2> ProjectionFactorsCreator; typedef GenericProjectionFactorsCreator<Pose3, Point3, Cal3_S2> ProjectionFactorsCreator;
#endif #endif
bool debug = false; bool debug = false;
@ -121,7 +122,7 @@ void optimizeGraphLM(NonlinearFactorGraph &graph, gtsam::Values::shared_ptr grap
params.lambdaInitial = 1; params.lambdaInitial = 1;
params.lambdaFactor = 10; params.lambdaFactor = 10;
// Profile a single iteration // Profile a single iteration
// params.maxIterations = 1; // params.maxIterations = 1;
params.maxIterations = 100; params.maxIterations = 100;
std::cout << " LM max iterations: " << params.maxIterations << std::endl; std::cout << " LM max iterations: " << params.maxIterations << std::endl;
// // params.relativeErrorTol = 1e-5; // // params.relativeErrorTol = 1e-5;
@ -152,11 +153,11 @@ void optimizeGraphLM(NonlinearFactorGraph &graph, gtsam::Values::shared_ptr grap
params.ordering = *ordering; params.ordering = *ordering;
//for (int i = 0; i < 3; i++) { //for (int i = 0; i < 3; i++) {
LevenbergMarquardtOptimizer optimizer(graph, *graphValues, params); LevenbergMarquardtOptimizer optimizer(graph, *graphValues, params);
gttic_(GenericProjectionFactorExample_kitti); gttic_(GenericProjectionFactorExample_kitti);
result = optimizer.optimize(); result = optimizer.optimize();
gttoc_(GenericProjectionFactorExample_kitti); gttoc_(GenericProjectionFactorExample_kitti);
tictoc_finishedIteration_(); tictoc_finishedIteration_();
//} //}
} else { } else {
std::cout << "Using COLAMD ordering\n" << std::endl; std::cout << "Using COLAMD ordering\n" << std::endl;
@ -174,11 +175,11 @@ void optimizeGraphLM(NonlinearFactorGraph &graph, gtsam::Values::shared_ptr grap
//*ordering = params.ordering; //*ordering = params.ordering;
if (params.ordering) { if (params.ordering) {
std::cout << "Graph size: " << graph.size() << " ORdering: " << params.ordering->size() << std::endl; std::cout << "Graph size: " << graph.size() << " ORdering: " << params.ordering->size() << std::endl;
ordering = boost::make_shared<Ordering>(*(new Ordering())); ordering = boost::make_shared<Ordering>(*(new Ordering()));
*ordering = *params.ordering; *ordering = *params.ordering;
} else { } else {
std::cout << "WARNING: NULL ordering!" << std::endl; std::cout << "WARNING: NULL ordering!" << std::endl;
} }
} }
} }
@ -219,11 +220,15 @@ int main(int argc, char** argv) {
double linThreshold = -1.0; // negative is disabled double linThreshold = -1.0; // negative is disabled
double rankTolerance = 1.0; double rankTolerance = 1.0;
// Get home directory and dataset // Get home directory and default dataset
string HOME = getenv("HOME"); string HOME = getenv("HOME");
string datasetFile = HOME + "/data/SfM/BAL/Ladybug/problem-1031-110968-pre.txt"; string datasetFile = HOME + "/data/SfM/BAL/Ladybug/problem-1031-110968-pre.txt";
// --------------- READ USER INPUTS (main arguments) ------------------------------------
if(argc>1){ // if we have any input arguments if(argc>1){ // if we have any input arguments
// Arg1: "smart" or "standard" (select if to use smart factors or standard projection factors)
// Arg2: "triangulation=0" or "triangulation=1" (select whether to initialize initial guess for points using triangulation)
// Arg3: name of the dataset, e.g., /home/aspn/data/SfM/BAL/Ladybug/problem-1031-110968-pre.txt
string useSmartArgument = argv[1]; string useSmartArgument = argv[1];
string useTriangulationArgument = argv[2]; string useTriangulationArgument = argv[2];
datasetFile = argv[3]; datasetFile = argv[3];
@ -238,160 +243,108 @@ int main(int argc, char** argv) {
exit(1); exit(1);
} }
} }
if(useTriangulationArgument.compare("do")==0){ if(useTriangulationArgument.compare("triangulation=1")==0){
doTriangulation=true; doTriangulation=true;
} else{ } else{
if(useTriangulationArgument.compare("dont")==0){ if(useTriangulationArgument.compare("triangulation=0")==0){
doTriangulation=false; doTriangulation=false;
}else{ }else{
cout << "Selected wrong option for input argument - doTriangulation - not important for SmartFactors" << endl; cout << "Selected wrong option for input argument - doTriangulation - not important for SmartFactors" << endl;
} }
} }
} }
std::cout << "PARAM SmartFactor: " << useSmartProjectionFactor << std::endl;
std::cout << "PARAM doTriangulation: " << doTriangulation << std::endl; // --------------- PRINT USER's CHOICE ------------------------------------
// std::cout << "PARAM LM: " << useLM << std::endl; std::cout << "- useSmartFactor: " << useSmartProjectionFactor << std::endl;
std::cout << "PARAM linThreshold (negative is disabled): " << linThreshold << std::endl; std::cout << "- doTriangulation: " << doTriangulation << std::endl;
std::cout << "- datasetFile: " << datasetFile << std::endl;
if (linThreshold >= 0)
std::cout << "PARAM linThreshold (negative is disabled): " << linThreshold << std::endl;
if(addNoise) if(addNoise)
std::cout << "PARAM Noise: " << addNoise << std::endl; std::cout << "PARAM Noise: " << addNoise << std::endl;
std::cout << "datasetFile: " << datasetFile << std::endl; // --------------- READ INPUT DATA ----------------------------------------
SfM_data inputData;
readBAL(datasetFile, inputData);
std::cout << "read data from file... " << std::endl;
// --------------- CREATE FACTOR GRAPH ------------------------------------
static SharedNoiseModel pixel_sigma(noiseModel::Unit::Create(2)); static SharedNoiseModel pixel_sigma(noiseModel::Unit::Create(2));
NonlinearFactorGraph graphSmart, graphProjection;
gtsam::Values::shared_ptr graphSmartValues(new gtsam::Values());
gtsam::Values::shared_ptr graphProjectionValues(new gtsam::Values());
gtsam::Values::shared_ptr loadedValues(new gtsam::Values()); // values we read from file
Values result;
// Read in kitti dataset
ifstream fin;
fin.open((datasetFile).c_str());
if(!fin) {
cerr << "Could not open dataset" << endl;
exit(1);
}
// read all measurements
cout << "Reading dataset... " << endl;
unsigned int numLandmarks = 0, numPoses = 0;
Key r, l;
double u, v;
double x, y, z, rotx, roty, rotz, f, k1, k2;
std::vector<Key> landmarkKeys, cameraPoseKeys;
bool optimized = false;
boost::shared_ptr<Ordering> ordering(new Ordering()); boost::shared_ptr<Ordering> ordering(new Ordering());
#ifdef USE_BUNDLER NonlinearFactorGraph graphSmart;
std::vector< boost::shared_ptr<Cal3Bundler> > K_cameras; gtsam::Values::shared_ptr graphSmartValues(new gtsam::Values());
boost::shared_ptr<Cal3Bundler> K(new Cal3Bundler());
#else NonlinearFactorGraph graphProjection;
std::vector< boost::shared_ptr<Cal3_S2> > K_cameras; gtsam::Values::shared_ptr graphProjectionValues(new gtsam::Values());
Cal3_S2::shared_ptr K(new Cal3_S2());
#endif #ifdef USE_BUNDLER
std::vector< boost::shared_ptr<Cal3Bundler> > K_cameras;
boost::shared_ptr<Cal3Bundler> K(new Cal3Bundler());
#else
std::vector< boost::shared_ptr<Cal3_S2> > K_cameras;
Cal3_S2::shared_ptr K(new Cal3_S2());
#endif
SmartFactorsCreator smartCreator(pixel_sigma, K, rankTolerance, linThreshold); // this initial K is not used SmartFactorsCreator smartCreator(pixel_sigma, K, rankTolerance, linThreshold); // this initial K is not used
ProjectionFactorsCreator projectionCreator(pixel_sigma, K); // this initial K is not used ProjectionFactorsCreator projectionCreator(pixel_sigma, K); // this initial K is not used
int numLandmarks=0;
// main loop: reads measurements and adds factors (also performs optimization if desired) if(debug){
// r >> pose id std::cout << "Constructors for factor creators " << std::endl;
// l >> landmark id std::cout << "inputData.number_cameras() " << inputData.number_cameras() << std::endl;
// (u >> u) >> measurement std::cout << "inputData.number_tracks() " << inputData.number_tracks() << std::endl;
unsigned int totNumLandmarks=0, totNumPoses=0, totNumMeasurements=0;
fin >> totNumPoses >> totNumLandmarks >> totNumMeasurements;
cout << "Dataset: #poses: " << totNumPoses << " #points: " << totNumLandmarks << " #measurements: " << totNumMeasurements << " " << endl;
std::vector<double> vector_u;
std::vector<double> vector_v;
std::vector<int> vector_r;
std::vector<int> vector_l;
// read measurements
for(unsigned int i = 0; i < totNumMeasurements; i++){
fin >> r >> l >> u >> v;
vector_u.push_back(u);
vector_v.push_back(v);
vector_r.push_back(r);
vector_l.push_back(l);
} }
cout << "last measurement: " << r << " " << l << " " << u << " " << v << endl; // Load graph values
gtsam::Values::shared_ptr loadedValues(new gtsam::Values()); // values we read from file
// create values for (size_t i = 0; i < inputData.number_cameras(); i++){ // for each camera
for(unsigned int i = 0; i < totNumPoses; i++){ Pose3 cameraPose = inputData.cameras[i].pose();
// R,t,f,k1 and k2.
fin >> rotx >> roty >> rotz >> x >> y >> z >> f >> k1 >> k2;
#ifdef USE_BUNDLER
boost::shared_ptr<Cal3Bundler> Kbundler(new Cal3Bundler(f, k1, k2, 0.0, 0.0));
// cout << k1 << " " << k2 << endl;
K_cameras.push_back(Kbundler);
#else
boost::shared_ptr<Cal3_S2> K_S2(new Cal3_S2(f, f, 0.0, 0.0, 0.0));
K_cameras.push_back(K_S2);
#endif
Vector3 rotVect(rotx,roty,rotz);
// FORMAT CONVERSION!! R -> R'
Rot3 R = Rot3::Expmap(rotVect);
Matrix3 R_bal_gtsam_mat = Matrix3::Zero(3,3);
R_bal_gtsam_mat(0,0) = 1.0; R_bal_gtsam_mat(1,1) = -1.0; R_bal_gtsam_mat(2,2) = -1.0;
Rot3 R_bal_gtsam_ = Rot3(R_bal_gtsam_mat);
Pose3 CameraPose((R.inverse()).compose(R_bal_gtsam_), - R.unrotate(Point3(x,y,z)));
if(addNoise){ if(addNoise){
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.3,0.1,0.3)); Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.3,0.1,0.3));
CameraPose = CameraPose.compose(noise_pose); cameraPose = cameraPose.compose(noise_pose);
} }
loadedValues->insert(X(i), CameraPose ); loadedValues->insert(X(i), cameraPose);
graphSmartValues->insert(X(i), cameraPose);
} }
if(debug) std::cout << "Initialized values " << std::endl;
cout << "last pose: " << x << " " << y << " " << z << " " << rotx << " " for (size_t j = 0; j < inputData.number_tracks(); j++){ // for each 3D point
<< roty << " " << rotz << " " << f << " " << k1 << " " << k2 << endl; Point3 point = inputData.tracks[j].p;
loadedValues->insert(L(j), point);
// add landmarks in standard projection factors
if(!useSmartProjectionFactor){
for(unsigned int i = 0; i < totNumLandmarks; i++){
fin >> x >> y >> z;
// FORMAT CONVERSION!! XPOINT
loadedValues->insert(L(i), Point3(x,y,z) );
}
} }
if(debug) std::cout << "Initialized points " << std::endl;
cout << "last point: " << x << " " << y << " " << z << endl; // Create the graph
for (size_t j = 0; j < inputData.number_tracks(); j++){ // for each 3D point
SfM_Track track = inputData.tracks[j];
Point3 point = track.p;
// 1: add values and factors to the graph for (size_t k = 0; k < track.number_measurements(); k++){ // for each measurement of the point
// 1.1: add factors SfM_Measurement measurement = track.measurements[k];
// SMART FACTORS .. int i = measurement.first;
for(size_t i = 0; i < vector_u.size(); i++){ double u = measurement.second.x();
double v = measurement.second.y();
boost::shared_ptr<Cal3Bundler> Ki(new Cal3Bundler(inputData.cameras[i].calibration()));
//boost::shared_ptr<Cal3_S2> Ki(new Cal3_S2());
l = vector_l.at(i); // insert data in a format that can be understood
r = vector_r.at(i); if (useSmartProjectionFactor) {
// FORMAT CONVERSION!! XPOINT // Use smart factors
u = vector_u.at(i); smartCreator.add(L(j), X(i), Point2(u,v), pixel_sigma, Ki, graphSmart);
// FORMAT CONVERSION!! XPOINT numLandmarks = smartCreator.getNumLandmarks();
v = - vector_v.at(i); } else {
// or STANDARD PROJECTION FACTORS
if (useSmartProjectionFactor) { projectionCreator.add(L(j), X(i), Point2(u,v), pixel_sigma, Ki, graphProjection);
numLandmarks = projectionCreator.getNumLandmarks();
smartCreator.add(L(l), X(r), Point2(u,v), pixel_sigma, K_cameras.at(r), graphSmart);
numLandmarks = smartCreator.getNumLandmarks();
// Add initial pose value if pose does not exist
if (!graphSmartValues->exists<Pose3>(X(r)) && loadedValues->exists<Pose3>(X(r))) {
graphSmartValues->insert(X(r), loadedValues->at<Pose3>(X(r)));
numPoses++;
optimized = false;
} }
} else {
// or STANDARD PROJECTION FACTORS
projectionCreator.add(L(l), X(r), Point2(u,v), pixel_sigma, K_cameras.at(r), graphProjection);
numLandmarks = projectionCreator.getNumLandmarks();
optimized = false;
} }
} }
if(debug) std::cout << "Included measurements in the graph " << std::endl;
cout << "Number of landmarks " << numLandmarks << endl;
cout << "Before call to update: ------------------ " << endl; cout << "Before call to update: ------------------ " << endl;
cout << "Poses in SmartGraph values: "<< graphSmartValues->size() << endl; cout << "Poses in SmartGraph values: "<< graphSmartValues->size() << endl;
@ -415,31 +368,26 @@ int main(int argc, char** argv) {
cout << "Points in ProjectionGraph values: "<< valuesProjPoints.size() << endl; cout << "Points in ProjectionGraph values: "<< valuesProjPoints.size() << endl;
cout << "---------------------------------------------------------- " << endl; cout << "---------------------------------------------------------- " << endl;
Values result;
if (useSmartProjectionFactor) { if (useSmartProjectionFactor) {
if (useLM) if (useLM)
optimizeGraphLM(graphSmart, graphSmartValues, result, ordering); optimizeGraphLM(graphSmart, graphSmartValues, result, ordering);
else else
optimizeGraphISAM2(graphSmart, graphSmartValues, result); optimizeGraphISAM2(graphSmart, graphSmartValues, result);
cout << "Final reprojection error (smart): " << graphSmart.error(result); cout << "Final reprojection error (smart): " << graphSmart.error(result);
} else { } else {
if (useLM) if (useLM)
optimizeGraphLM(graphProjection, graphProjectionValues, result, ordering); optimizeGraphLM(graphProjection, graphProjectionValues, result, ordering);
else else
optimizeGraphISAM2(graphProjection, graphProjectionValues, result); // ? optimizeGraphISAM2(graphProjection, graphProjectionValues, result); // ?
cout << "Final reprojection error (standard): " << graphProjection.error(result); cout << "Final reprojection error (standard): " << graphProjection.error(result);
} }
optimized = true;
cout << "===================================================" << endl; cout << "===================================================" << endl;
tictoc_print_(); tictoc_print_();
cout << "===================================================" << endl; cout << "===================================================" << endl;
writeValues("./", result); writeValues("./", result);
if (debug) cout << numLandmarks << " " << numPoses << " " << optimized << endl;
exit(0); exit(0);
} }

1
gtsam_unstable/examples/SmartProjectionFactorExampleKitti.cpp
View File

@ -14,6 +14,7 @@
* @brief Example usage of SmartProjectionFactor using real dataset in a non-batch fashion * @brief Example usage of SmartProjectionFactor using real dataset in a non-batch fashion
* @date August, 2013 * @date August, 2013
* @author Zsolt Kira * @author Zsolt Kira
* @author Luca Carlone
*/ */
// Use a map to store landmark/smart factor pairs // Use a map to store landmark/smart factor pairs

2
gtsam_unstable/slam/GenericProjectionFactorsCreator.h
View File

@ -2,7 +2,7 @@
* GenericProjectionFactorsCreator.h * GenericProjectionFactorsCreator.h
* *
* Created on: Oct 10, 2013 * Created on: Oct 10, 2013
* Author: zkira * @author Zsolt Kira
*/ */
#ifndef GENERICPROJECTIONFACTORSCREATOR_H_ #ifndef GENERICPROJECTIONFACTORSCREATOR_H_

2
gtsam_unstable/slam/SmartProjectionFactorsCreator.h
View File

@ -2,7 +2,7 @@
* SmartProjectionFactorsCreator.h * SmartProjectionFactorsCreator.h
* *
* Created on: Oct 8, 2013 * Created on: Oct 8, 2013
* Author: aspn * @author Zsolt Kira
*/ */
#ifndef SMARTPROJECTIONFACTORSCREATOR_H_ #ifndef SMARTPROJECTIONFACTORSCREATOR_H_