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

moved smart factors for smartFactors_ceres

release/4.3a0
Luca Carlone 2013-10-21 15:35:23 +00:00
parent 7ebce58a69
commit 6e7302283e
6 changed files with 0 additions and 3036 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

362
gtsam_unstable/examples/SmartProjectionFactorExampleBAL.cpp
View File

@ -1,362 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 SmartProjectionFactorTesting.cpp
* @brief Example usage of SmartProjectionFactor using real datasets
* @date August, 2013
* @author Luca Carlone
*/
// Use a map to store landmark/smart factor pairs
#include <gtsam/base/FastMap.h>
// Both relative poses and recovered trajectory poses will be stored as Pose3 objects
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3Bundler.h>
// Each variable in the system (poses and landmarks) must be identified with a unique key.
// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
// Here we will use Symbols
#include <gtsam/inference/Symbol.h>
// We want to use iSAM2 to solve the range-SLAM problem incrementally
#include <gtsam/nonlinear/ISAM2.h>
// iSAM2 requires as input a set set of new factors to be added stored in a factor graph,
// and initial guesses for any new variables used in the added factors
#include <gtsam/nonlinear/Values.h>
// We will use a non-linear solver to batch-initialize from the first 150 frames
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
// In GTSAM, measurement functions are represented as 'factors'. Several common factors
// have been provided with the library for solving robotics SLAM problems.
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/dataset.h>
#include <gtsam_unstable/slam/SmartProjectionFactorsCreator.h>
#include <gtsam_unstable/slam/GenericProjectionFactorsCreator.h>
// Standard headers, added last, so we know headers above work on their own
#include <boost/foreach.hpp>
#include <boost/assign.hpp>
#include <boost/assign/std/vector.hpp>
#include <fstream>
#include <iostream>
using namespace std;
using namespace gtsam;
using namespace boost::assign;
namespace NM = gtsam::noiseModel;
using symbol_shorthand::X;
using symbol_shorthand::L;
typedef PriorFactor<Pose3> Pose3Prior;
typedef FastMap<Key, int> OrderingMap;
typedef SmartProjectionFactorsCreator<Pose3, Point3, Cal3Bundler> SmartFactorsCreator;
typedef GenericProjectionFactorsCreator<Pose3, Point3, Cal3Bundler> ProjectionFactorsCreator;
bool debug = false;
void optimizeGraphLM(NonlinearFactorGraph &graph, gtsam::Values::shared_ptr graphValues, Values &result, boost::shared_ptr<Ordering> &ordering) {
// Optimization parameters
LevenbergMarquardtParams params;
params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
params.verbosity = NonlinearOptimizerParams::ERROR;
params.lambdaInitial = 1;
// Other parameters: if needed
// params.lambdaFactor = 10;
// Profile a single iteration
// params.maxIterations = 1;
// params.relativeErrorTol = 1e-5;
// params.absoluteErrorTol = 1.0;
cout << "==================== Optimization ==================" << endl;
cout << "- Number of factors: " << graph.size() << endl;
cout << "- Number of variables: " << graphValues->size() << endl;
params.print("PARAMETERS FOR LM: \n");
if (debug) {
cout << "\n\n===============================================\n\n";
graph.print("thegraph");
}
cout << "-----------------------------------------------------" << endl;
if (ordering && ordering->size() > 0) {
std::cout << "Starting graph optimization with user specified ordering" << std::endl;
params.ordering = *ordering;
LevenbergMarquardtOptimizer optimizer(graph, *graphValues, params);
gttic_(GenericProjectionFactorExample_kitti);
result = optimizer.optimize();
gttoc_(GenericProjectionFactorExample_kitti);
tictoc_finishedIteration_();
cout << "-----------------------------------------------------" << endl;
std::cout << "Number of outer LM iterations: " << optimizer.state().iterations << std::endl;
std::cout << "Total number of LM iterations (inner and outer): " << optimizer.getInnerIterations() << std::endl;
} else {
std::cout << "Starting graph optimization with COLAMD ordering" << std::endl;
LevenbergMarquardtOptimizer optimizer(graph, *graphValues, params);
params.ordering = Ordering::COLAMD(graph);
gttic_(smartProjectionFactorExample);
result = optimizer.optimize();
gttoc_(smartProjectionFactorExample);
tictoc_finishedIteration_();
cout << "-----------------------------------------------------" << endl;
std::cout << "Number of outer LM iterations: " << optimizer.state().iterations << std::endl;
std::cout << "Total number of LM iterations (inner and outer): " << optimizer.getInnerIterations() << std::endl;
//*ordering = params.ordering;
if (params.ordering) {
if(debug) std::cout << "Graph size: " << graph.size() << " Ordering: " << params.ordering->size() << std::endl;
ordering = boost::make_shared<Ordering>(*(new Ordering()));
*ordering = *params.ordering;
} else {
std::cout << "WARNING: NULL ordering!" << std::endl;
}
}
cout << "======================================================" << endl;
}
void optimizeGraphGN(NonlinearFactorGraph &graph, gtsam::Values::shared_ptr graphValues, Values &result) {
GaussNewtonParams params;
//params.maxIterations = 1;
params.verbosity = NonlinearOptimizerParams::DELTA;
GaussNewtonOptimizer optimizer(graph, *graphValues, params);
gttic_(smartProjectionFactorExample);
result = optimizer.optimize();
gttoc_(smartProjectionFactorExample);
tictoc_finishedIteration_();
}
void optimizeGraphISAM2(NonlinearFactorGraph &graph, gtsam::Values::shared_ptr graphValues, Values &result) {
ISAM2 isam;
gttic_(smartProjectionFactorExample);
isam.update(graph, *graphValues);
result = isam.calculateEstimate();
gttoc_(smartProjectionFactorExample);
tictoc_finishedIteration_();
}
// ************************************************************************************************
// ************************************************************************************************
// main
int main(int argc, char** argv) {
bool useSmartProjectionFactor = true; // default choice is to use the smart projection factors
bool doTriangulation = true; // default choice is to initialize points from triangulation (only for standard projection factors)
bool addNoise = false; // add (fixed) noise to the initial guess of camera poses
bool useLM = true;
// Smart factors settings
double linThreshold = -1.0; // negative is disabled
double rankTolerance = 1.0;
// Get home directory and default dataset
string HOME = getenv("HOME");
string datasetFile = HOME + "/data/SfM/BAL/Ladybug/problem-1031-110968-pre.txt";
// --------------- READ USER INPUTS (main arguments) ------------------------------------
// COMMAND TO RUN (EXAMPLE): ./SmartProjectionFactorExampleBAL smart triangulation=0 /home/aspn/data/SfM/BAL/Ladybug/problem-1031-110968-pre.txt
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 useTriangulationArgument = argv[2];
datasetFile = argv[3];
if(useSmartArgument.compare("smart")==0){
useSmartProjectionFactor=true;
} else{
if(useSmartArgument.compare("standard")==0){
useSmartProjectionFactor=false;
}else{
cout << "Selected wrong option for input argument - useSmartProjectionFactor" << endl;
exit(1);
}
}
if(useTriangulationArgument.compare("triangulation=1")==0){
doTriangulation=true;
} else{
if(useTriangulationArgument.compare("triangulation=0")==0){
doTriangulation=false;
}else{
cout << "Selected wrong option for input argument - doTriangulation - not important for SmartFactors" << endl;
}
}
}
// --------------- PRINT USER's CHOICE ------------------------------------
std::cout << "- useSmartFactor: " << useSmartProjectionFactor << std::endl;
std::cout << "- doTriangulation: " << doTriangulation << std::endl;
std::cout << "- datasetFile: " << datasetFile << std::endl;
if (linThreshold >= 0)
std::cout << "- linThreshold (negative is disabled): " << linThreshold << std::endl;
if(addNoise)
std::cout << "- Noise: " << addNoise << std::endl;
// --------------- READ INPUT DATA ----------------------------------------
std::cout << "- reading dataset from file... " << std::endl;
SfM_data inputData;
readBAL(datasetFile, inputData);
// --------------- CREATE FACTOR GRAPH ------------------------------------
std::cout << "- creating factor graph... " << std::endl;
static SharedNoiseModel pixel_sigma(noiseModel::Unit::Create(2)); // pixel noise
boost::shared_ptr<Ordering> ordering(new Ordering());
NonlinearFactorGraph graphSmart;
gtsam::Values::shared_ptr graphSmartValues(new gtsam::Values());
NonlinearFactorGraph graphProjection;
gtsam::Values::shared_ptr graphProjectionValues(new gtsam::Values());
std::vector< boost::shared_ptr<Cal3Bundler> > K_cameras;
boost::shared_ptr<Cal3Bundler> K(new Cal3Bundler());
SmartFactorsCreator smartCreator(pixel_sigma, K, rankTolerance, linThreshold); // this initial K is not used
ProjectionFactorsCreator projectionCreator(pixel_sigma, K); // this initial K is not used
int numLandmarks=0;
if(debug){
std::cout << "Constructors for factor creators " << std::endl;
std::cout << "inputData.number_cameras() " << inputData.number_cameras() << std::endl;
std::cout << "inputData.number_tracks() " << inputData.number_tracks() << std::endl;
}
// Load graph values
gtsam::Values::shared_ptr loadedValues(new gtsam::Values()); // values we read from file
for (size_t i = 0; i < inputData.number_cameras(); i++){ // for each camera
Pose3 cameraPose = inputData.cameras[i].pose();
if(addNoise){
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);
}
loadedValues->insert(X(i), cameraPose); // this will be used for the graphProjection
graphSmartValues->insert(X(i), cameraPose); // we insert the value for the graphSmart that only contains poses
}
if(debug) std::cout << "Initialized values " << std::endl;
for (size_t j = 0; j < inputData.number_tracks(); j++){ // for each 3D point
Point3 point = inputData.tracks[j].p;
loadedValues->insert(L(j), point);
}
if(debug) std::cout << "Initialized points " << std::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;
for (size_t k = 0; k < track.number_measurements(); k++){ // for each measurement of the point
SfM_Measurement measurement = track.measurements[k];
int i = measurement.first; // camera id
double u = measurement.second.x();
double v = measurement.second.y();
boost::shared_ptr<Cal3Bundler> Ki(new Cal3Bundler(inputData.cameras[i].calibration()));
if (useSmartProjectionFactor) {
// use SMART PROJECTION FACTORS
smartCreator.add(L(j), X(i), Point2(u,v), pixel_sigma, Ki, graphSmart);
numLandmarks = smartCreator.getNumLandmarks();
} else {
// or STANDARD PROJECTION FACTORS
projectionCreator.add(L(j), X(i), Point2(u,v), pixel_sigma, Ki, graphProjection);
numLandmarks = projectionCreator.getNumLandmarks();
}
}
}
if(debug){
cout << "Included measurements in the graph " << endl;
cout << "Number of landmarks " << numLandmarks << endl;
cout << "Before call to update: ------------------ " << endl;
cout << "Poses in SmartGraph values: "<< graphSmartValues->size() << endl;
Values valuesProjPoses = graphProjectionValues->filter<Pose3>();
cout << "Poses in ProjectionGraph values: "<< valuesProjPoses.size() << endl;
Values valuesProjPoints = graphProjectionValues->filter<Point3>();
cout << "Points in ProjectionGraph values: "<< valuesProjPoints.size() << endl;
cout << "---------------------------------------------------------- " << endl;
}
if (!useSmartProjectionFactor) {
projectionCreator.update(graphProjection, loadedValues, graphProjectionValues, doTriangulation);
ordering = projectionCreator.getOrdering();
}
if(debug) {
cout << "After call to update: ------------------ " << endl;
cout << "--------------------------------------------------------- " << endl;
cout << "Poses in SmartGraph values: "<< graphSmartValues->size() << endl;
Values valuesProjPoses = graphProjectionValues->filter<Pose3>();
cout << "Poses in ProjectionGraph values: "<< valuesProjPoses.size() << endl;
Values valuesProjPoints = graphProjectionValues->filter<Point3>();
cout << "Points in ProjectionGraph values: "<< valuesProjPoints.size() << endl;
cout << "---------------------------------------------------------- " << endl;
}
Values result;
if (useSmartProjectionFactor) {
if (useLM)
optimizeGraphLM(graphSmart, graphSmartValues, result, ordering);
else
optimizeGraphISAM2(graphSmart, graphSmartValues, result);
cout << "Initial reprojection error (smart): " << graphSmart.error(*graphSmartValues) << endl;;
cout << "Final reprojection error (smart): " << graphSmart.error(result) << endl;;
} else {
if (useLM)
optimizeGraphLM(graphProjection, graphProjectionValues, result, ordering);
else
optimizeGraphISAM2(graphProjection, graphProjectionValues, result);
cout << "Initial reprojection error (standard): " << graphProjection.error(*graphProjectionValues) << endl;;
cout << "Final reprojection error (standard): " << graphProjection.error(result) << endl;;
}
tictoc_print_();
cout << "===================================================" << endl;
// --------------- WRITE OUTPUT TO BAL FILE ----------------------------------------
if(useSmartProjectionFactor){
smartCreator.computePoints(result);
}
cout << "- writing results to (BAL) file... " << endl;
std::size_t stringCut1 = datasetFile.rfind("/");
std::size_t stringCut2 = datasetFile.rfind(".txt");
string outputFile;
if(useSmartProjectionFactor){
outputFile = "." + datasetFile.substr(stringCut1, stringCut2-stringCut1) + "-optimized-smart.txt";
}else{
if(doTriangulation){
outputFile = "." + datasetFile.substr(stringCut1, stringCut2-stringCut1) + "-optimized-standard-triangulation.txt";
}else{
outputFile = "." + datasetFile.substr(stringCut1, stringCut2-stringCut1) + "-optimized-standard.txt";
}
}
if(debug) cout << outputFile << endl;
writeBALfromValues(outputFile, inputData, result);
cout << "- mission accomplished! " << endl;
exit(0);
}

430
gtsam_unstable/examples/SmartProjectionFactorExampleKitti.cpp
View File

@ -1,430 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 SmartProjectionFactorExample_kitti_nonbatch.cpp
* @brief Example usage of SmartProjectionFactor using real dataset in a non-batch fashion
* @date August, 2013
* @author Zsolt Kira
* @author Luca Carlone
*/
// Use a map to store landmark/smart factor pairs
#include <gtsam/base/FastMap.h>
// Both relative poses and recovered trajectory poses will be stored as Pose3 objects
#include <gtsam/geometry/Pose3.h>
// Each variable in the system (poses and landmarks) must be identified with a unique key.
// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
// Here we will use Symbols
#include <gtsam/inference/Symbol.h>
// We want to use iSAM2 to solve the range-SLAM problem incrementally
#include <gtsam/nonlinear/ISAM2.h>
// iSAM2 requires as input a set set of new factors to be added stored in a factor graph,
// and initial guesses for any new variables used in the added factors
#include <gtsam/nonlinear/Values.h>
// We will use a non-linear solver to batch-initialize from the first 150 frames
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
// In GTSAM, measurement functions are represented as 'factors'. Several common factors
// have been provided with the library for solving robotics SLAM problems.
#include <gtsam/slam/PriorFactor.h>
#include <gtsam_unstable/slam/SmartProjectionFactorsCreator.h>
#include <gtsam_unstable/slam/GenericProjectionFactorsCreator.h>
// Standard headers, added last, so we know headers above work on their own
#include <boost/foreach.hpp>
#include <boost/assign.hpp>
#include <boost/assign/std/vector.hpp>
#include <fstream>
#include <iostream>
using namespace std;
using namespace gtsam;
using namespace boost::assign;
namespace NM = gtsam::noiseModel;
using symbol_shorthand::X;
using symbol_shorthand::L;
typedef PriorFactor<Pose3> Pose3Prior;
typedef SmartProjectionFactorsCreator<Pose3, Point3, Cal3_S2> SmartFactorsCreator;
typedef GenericProjectionFactorsCreator<Pose3, Point3, Cal3_S2> ProjectionFactorsCreator;
typedef FastMap<Key, int> OrderingMap;
bool debug = false;
//// Helper functions taken from VO code
// Loaded all pose values into list
Values::shared_ptr loadPoseValues(const string& filename) {
Values::shared_ptr values(new Values());
bool addNoise = false;
std::cout << "PARAM Noise: " << addNoise << std::endl;
// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,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));
// read in camera poses
string full_filename = filename;
ifstream fin;
fin.open(full_filename.c_str());
int pose_id;
while (fin >> pose_id) {
double pose_matrix[16];
for (int i = 0; i < 16; i++) {
fin >> pose_matrix[i];
}
if (addNoise) {
values->insert(Symbol('x',pose_id), Pose3(Matrix_(4, 4, pose_matrix)).compose(noise_pose));
} else {
values->insert(Symbol('x',pose_id), Pose3(Matrix_(4, 4, pose_matrix)));
}
}
fin.close();
return values;
}
// Load specific pose values that are in key list
Values::shared_ptr loadPoseValues(const string& filename, list<Key> keys) {
Values::shared_ptr values(new Values());
std::list<Key>::iterator kit;
// read in camera poses
string full_filename = filename;
ifstream fin;
fin.open(full_filename.c_str());
int pose_id;
while (fin >> pose_id) {
double pose_matrix[16];
for (int i = 0; i < 16; i++) {
fin >> pose_matrix[i];
}
kit = find (keys.begin(), keys.end(), X(pose_id));
if (kit != keys.end()) {
//cout << " Adding " << X(pose_id) << endl;
values->insert(Symbol('x',pose_id), Pose3(Matrix_(4, 4, pose_matrix)));
}
}
fin.close();
return values;
}
// Load calibration info
boost::shared_ptr<Cal3_S2> loadCalibration(const string& filename) {
string full_filename = filename;
ifstream fin(full_filename.c_str());
// try loading from parent directory as backup
if(!fin) {
cerr << "Could not load " << full_filename;
exit(1);
}
double fx, fy, s, u, v, b;
fin >> fx >> fy >> s >> u >> v >> b;
return boost::make_shared<Cal3_S2>(fx,fy, s, u, v);
}
// Write key values to file
void writeValues(string directory_, const Values& values){
string filename = directory_ + "out_camera_poses.txt";
ofstream fout;
fout.open(filename.c_str());
fout.precision(20);
// write out camera poses
BOOST_FOREACH(Values::ConstFiltered<Pose3>::value_type key_value, values.filter<Pose3>()) {
fout << Symbol(key_value.key).index();
const gtsam::Matrix& matrix= key_value.value.matrix();
for (size_t row=0; row < 4; ++row) {
for (size_t col=0; col < 4; ++col) {
fout << " " << matrix(row, col);
}
}
fout << endl;
}
fout.close();
if(values.filter<Point3>().size() > 0) {
// write landmarks
filename = directory_ + "landmarks.txt";
fout.open(filename.c_str());
BOOST_FOREACH(Values::ConstFiltered<Point3>::value_type key_value, values.filter<Point3>()) {
fout << Symbol(key_value.key).index();
fout << " " << key_value.value.x();
fout << " " << key_value.value.y();
fout << " " << key_value.value.z();
fout << endl;
}
fout.close();
}
}
void optimizeGraphLM(NonlinearFactorGraph &graph, gtsam::Values::shared_ptr graphValues, Values &result, boost::shared_ptr<Ordering> &ordering) {
// Optimization parameters
LevenbergMarquardtParams params;
params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
params.verbosity = NonlinearOptimizerParams::ERROR;
params.lambdaInitial = 1;
params.lambdaFactor = 10;
// Profile a single iteration
// params.maxIterations = 1;
params.maxIterations = 100;
std::cout << " LM max iterations: " << params.maxIterations << std::endl;
// // params.relativeErrorTol = 1e-5;
params.absoluteErrorTol = 1.0;
params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
params.verbosity = NonlinearOptimizerParams::ERROR;
params.linearSolverType = SuccessiveLinearizationParams::MULTIFRONTAL_CHOLESKY;
cout << "Graph size: " << graph.size() << endl;
cout << "Number of variables: " << graphValues->size() << endl;
std::cout << " OPTIMIZATION " << std::endl;
std::cout << "\n\n=================================================\n\n";
if (debug) {
graph.print("thegraph");
}
std::cout << "\n\n=================================================\n\n";
if (ordering && ordering->size() > 0) {
if (debug) {
std::cout << "Have an ordering\n" << std::endl;
BOOST_FOREACH(const Key& key, *ordering) {
std::cout << key << " ";
}
std::cout << std::endl;
}
params.ordering = *ordering;
//for (int i = 0; i < 3; i++) {
LevenbergMarquardtOptimizer optimizer(graph, *graphValues, params);
gttic_(GenericProjectionFactorExample_kitti);
result = optimizer.optimize();
gttoc_(GenericProjectionFactorExample_kitti);
tictoc_finishedIteration_();
//}
} else {
std::cout << "Using COLAMD ordering\n" << std::endl;
//boost::shared_ptr<Ordering> ordering2(new Ordering()); ordering = ordering2;
//for (int i = 0; i < 3; i++) {
LevenbergMarquardtOptimizer optimizer(graph, *graphValues, params);
params.ordering = Ordering::COLAMD(graph);
gttic_(SmartProjectionFactorExample_kitti);
result = optimizer.optimize();
gttoc_(SmartProjectionFactorExample_kitti);
tictoc_finishedIteration_();
//}
//*ordering = params.ordering;
if (params.ordering) {
std::cout << "Graph size: " << graph.size() << " ORdering: " << params.ordering->size() << std::endl;
ordering = boost::make_shared<Ordering>(*(new Ordering()));
*ordering = *params.ordering;
} else {
std::cout << "WARNING: NULL ordering!" << std::endl;
}
}
}
void optimizeGraphGN(NonlinearFactorGraph &graph, gtsam::Values::shared_ptr graphValues, Values &result) {
GaussNewtonParams params;
//params.maxIterations = 1;
params.verbosity = NonlinearOptimizerParams::DELTA;
GaussNewtonOptimizer optimizer(graph, *graphValues, params);
gttic_(SmartProjectionFactorExample_kitti);
result = optimizer.optimize();
gttoc_(SmartProjectionFactorExample_kitti);
tictoc_finishedIteration_();
}
void optimizeGraphISAM2(NonlinearFactorGraph &graph, gtsam::Values::shared_ptr graphValues, Values &result) {
ISAM2 isam;
gttic_(SmartProjectionFactorExample_kitti);
isam.update(graph, *graphValues);
result = isam.calculateEstimate();
gttoc_(SmartProjectionFactorExample_kitti);
tictoc_finishedIteration_();
}
// main
int main(int argc, char** argv) {
unsigned int maxNumLandmarks = 389007; // 10000; //100000000; // 309393 // (loop_closure_merged) //37106 //(reduced kitti);
unsigned int maxNumPoses = 1e+6;
// Set to true to use SmartProjectionFactor. Otherwise GenericProjectionFactor will be used
bool useSmartProjectionFactor = true;
bool useLM = true;
double KittiLinThreshold = -1.0; // 0.005; //
double KittiRankTolerance = 1.0;
bool incrementalFlag = false;
int optSkip = 200; // we optimize the graph every optSkip poses
std::cout << "PARAM SmartFactor: " << useSmartProjectionFactor << std::endl;
std::cout << "PARAM LM: " << useLM << std::endl;
std::cout << "PARAM KittiLinThreshold (negative is disabled): " << KittiLinThreshold << std::endl;
// Get home directory and dataset
string HOME = getenv("HOME");
//string input_dir = HOME + "/data/KITTI_00_200/";
string input_dir = HOME + "/data/kitti/loop_closures_merged/"; // 399997 landmarks, 4541 poses
//string input_dir = HOME + "/data/kitti_00_full_dirty/";
static SharedNoiseModel pixel_sigma(noiseModel::Unit::Create(2));
NonlinearFactorGraph graphSmart, graphProjection;
// Load calibration
boost::shared_ptr<Cal3_S2> K = loadCalibration(input_dir+"calibration.txt");
K->print("Calibration");
// Read in kitti dataset
ifstream fin;
fin.open((input_dir+"stereo_factors.txt").c_str());
if(!fin) {
cerr << "Could not open stereo_factors.txt" << endl;
exit(1);
}
// Load all values
gtsam::Values::shared_ptr graphSmartValues(new gtsam::Values());
gtsam::Values::shared_ptr graphProjectionValues(new gtsam::Values());
gtsam::Values::shared_ptr loadedValues = loadPoseValues(input_dir+"camera_poses.txt");
// read all measurements tracked by VO stereo
cout << "Loading stereo_factors.txt" << endl;
unsigned int count = 0;
Key currentLandmark = 0;
unsigned int numLandmarks = 0, numPoses = 0;
Key r, l;
double uL, uR, v, x, y, z;
std::vector<Key> landmarkKeys, cameraPoseKeys;
Values values;
Values result;
bool optimized = false;
boost::shared_ptr<Ordering> ordering(new Ordering());
bool breakingCondition;
SmartFactorsCreator smartCreator(pixel_sigma, K, KittiRankTolerance, KittiLinThreshold);
ProjectionFactorsCreator projectionCreator(pixel_sigma, K);
// main loop: reads measurements and adds factors (also performs optimization if desired)
// r >> pose id
// l >> landmark id
// (uL >> uR) >> measurement (xaxis pixel measurement in left and right camera - since we do monocular, we only use uL)
// v >> measurement (yaxis pixel measurement)
// (x >> y >> z) 3D initialization for the landmark (not used in this code)
while (fin >> r >> l >> uL >> uR >> v >> x >> y >> z) {
if (debug) fprintf(stderr,"Landmark %ld\n", l);
if (debug) fprintf(stderr,"Line %d: %d landmarks, (max landmarks %d), %d poses, max poses %d\n", count, numLandmarks, maxNumLandmarks, numPoses, maxNumPoses);
// 1: add values and factors to the graph
// 1.1: add factors
// SMART FACTORS ..
if (useSmartProjectionFactor) {
smartCreator.add(L(l), X(r), Point2(uL,v), 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(uL,v), graphProjection);
numLandmarks = projectionCreator.getNumLandmarks();
optimized = false;
}
breakingCondition = currentLandmark != l && (numPoses > maxNumPoses || numLandmarks > maxNumLandmarks);
// Optimize if have a certain number of poses/landmarks, or we want to do incremental inference
if (breakingCondition || (incrementalFlag && !optimized && ((numPoses+1) % optSkip)==0) ) {
if (debug) fprintf(stderr,"%d: %d > %d, %d > %d\n", count, numLandmarks, maxNumLandmarks, numPoses, maxNumPoses);
if (debug) cout << "Adding triangulated landmarks, graph size: " << graphProjection.size() << endl;
if (useSmartProjectionFactor == false) {
projectionCreator.update(graphProjection, loadedValues, graphProjectionValues);
ordering = projectionCreator.getOrdering();
}
if (debug) cout << "Adding triangulated landmarks, graph size after: " << graphProjection.size() << endl;
if (debug) fprintf(stderr,"%d: %d > %d, %d > %d\n", count, numLandmarks, maxNumLandmarks, numPoses, maxNumPoses);
if (useSmartProjectionFactor) {
if (useLM)
optimizeGraphLM(graphSmart, graphSmartValues, result, ordering);
else
optimizeGraphISAM2(graphSmart, graphSmartValues, result);
} else {
if (useLM)
optimizeGraphLM(graphProjection, graphProjectionValues, result, ordering);
else
optimizeGraphISAM2(graphSmart, graphSmartValues, result);
}
if(incrementalFlag) *graphSmartValues = result; // we use optimized solution as initial guess for the next one
optimized = true;
}
if (debug) fprintf(stderr,"%d %d\n", count, maxNumLandmarks);
if (debug) cout << "CurrentLandmark " << currentLandmark << " Landmark " << l << std::endl;
if (debug) fprintf(stderr,"%d: %d, %d > %d, %d > %d\n", count, currentLandmark != l, numLandmarks, maxNumLandmarks, numPoses, maxNumPoses);
if(breakingCondition){ // reached desired number of landmarks/poses
break;
}
currentLandmark = l;
count++;
if(count==100000) {
cout << "Loading graph smart... " << graphSmart.size() << endl;
cout << "Loading graph projection... " << graphProjection.size() << endl;
}
} // end of while
if (1||debug) fprintf(stderr,"%d: %d > %d, %d > %d\n", count, numLandmarks, maxNumLandmarks, numPoses, maxNumPoses);
cout << "===================================================" << endl;
//graphSmartValues->print("before optimization ");
//result.print("results of kitti optimization ");
tictoc_print_();
cout << "===================================================" << endl;
writeValues("./", result);
if (1||debug) fprintf(stderr,"%d: %d > %d, %d > %d\n", count, numLandmarks, maxNumLandmarks, numPoses, maxNumPoses);
exit(0);
}

308
gtsam_unstable/slam/GenericProjectionFactorsCreator.h
View File

@ -1,308 +0,0 @@
/*
* GenericProjectionFactorsCreator.h
*
* Created on: Oct 10, 2013
* @author Zsolt Kira
*/
#ifndef GENERICPROJECTIONFACTORSCREATOR_H_
#define GENERICPROJECTIONFACTORSCREATOR_H_
// Both relative poses and recovered trajectory poses will be stored as Pose3 objects
#include <gtsam/geometry/Pose3.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
// Use a map to store landmark/smart factor pairs
#include <gtsam/base/FastMap.h>
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3Bundler.h>
//#include <boost/foreach.hpp>
//#include <boost/assign.hpp>
//#include <boost/assign/std/vector.hpp>
//#include <fstream>
//#include <iostream>
#include <utility>
namespace gtsam {
template<class POSE, class LANDMARK, class CALIBRATION = Cal3_S2>
class GenericProjectionFactorsCreator {
typedef GenericProjectionFactor<Pose3, Point3, CALIBRATION> ProjectionFactor;
typedef FastMap<Key, std::vector<boost::shared_ptr<ProjectionFactor> > > ProjectionFactorMap;
typedef FastMap<Key, boost::shared_ptr<CALIBRATION> > CalibrationMap;
typedef FastMap<Key, int> OrderingMap;
public:
GenericProjectionFactorsCreator(const SharedNoiseModel& model,
const boost::shared_ptr<CALIBRATION>& K,
boost::optional<POSE> body_P_sensor = boost::none) :
noise_(model), K_(K), body_P_sensor_(body_P_sensor),
orderingMethod(0), totalNumMeasurements(0), numLandmarks(0) {
ordering = boost::make_shared<Ordering>(*(new Ordering()));
};
void add(Key landmarkKey,
Key poseKey, Point2 measurement, NonlinearFactorGraph &graph) {
bool debug = false;
// Create projection factor
boost::shared_ptr<ProjectionFactor> projectionFactor(new ProjectionFactor(measurement, noise_, poseKey, landmarkKey, K_));
// Check if landmark exists in mapping
typename ProjectionFactorMap::iterator pfit = projectionFactors.find(landmarkKey);
if (pfit != projectionFactors.end()) {
if (debug) fprintf(stderr,"Adding measurement to existing landmark\n");
// Add projection factor to list of projection factors associated with this landmark
(*pfit).second.push_back(projectionFactor);
} else {
if (debug) fprintf(stderr,"New landmark (%d)\n", pfit != projectionFactors.end());
// Create a new vector of projection factors
std::vector<boost::shared_ptr<ProjectionFactor> > projectionFactorVector;
projectionFactorVector.push_back(projectionFactor);
// Insert projection factor to NEW list of projection factors associated with this landmark
projectionFactors.insert( std::make_pair(landmarkKey, projectionFactorVector) );
// Add projection factor to graph
//graph.push_back(projectionFactor);
// We have a new landmark
numLandmarks++;
landmarkKeys.push_back( landmarkKey );
}
}
void add(Key landmarkKey,
Key poseKey,
Point2 measurement,
const SharedNoiseModel& model,
const boost::shared_ptr<CALIBRATION>& K,
NonlinearFactorGraph &graph) {
bool debug = false;
// Check if landmark exists in mapping
typename CalibrationMap::iterator calfit = keyCalibrationMap.find(poseKey);
if (calfit == keyCalibrationMap.end()){
keyCalibrationMap.insert( std::make_pair(poseKey, K) );
}
// Create projection factor
typename ProjectionFactor::shared_ptr projectionFactor(new ProjectionFactor(measurement, model, poseKey, landmarkKey, K));
// Check if landmark exists in mapping
typename ProjectionFactorMap::iterator pfit = projectionFactors.find(landmarkKey);
if (pfit != projectionFactors.end()) {
if (debug) fprintf(stderr,"Adding measurement to existing landmark\n");
// Add projection factor to list of projection factors associated with this landmark
(*pfit).second.push_back(projectionFactor);
} else {
if (debug) fprintf(stderr,"New landmark (%d)\n", pfit != projectionFactors.end());
// Create a new vector of projection factors
std::vector<boost::shared_ptr<ProjectionFactor> > projectionFactorVector;
projectionFactorVector.push_back(projectionFactor);
// Insert projection factor to NEW list of projection factors associated with this landmark
projectionFactors.insert( std::make_pair(landmarkKey, projectionFactorVector) );
// Add projection factor to graph
//graph.push_back(projectionFactor);
// We have a new landmark
numLandmarks++;
landmarkKeys.push_back( landmarkKey );
}
}
void update(NonlinearFactorGraph &graph, gtsam::Values::shared_ptr inputValues, gtsam::Values::shared_ptr outputValues, bool doTriangulatePoints = true) {
addTriangulatedLandmarks(graph, inputValues, outputValues, doTriangulatePoints);
updateOrdering(graph);
}
unsigned int getTotalNumMeasurements() { return totalNumMeasurements; }
unsigned int getNumLandmarks() { return numLandmarks; }
unsigned int getNumPoses() { return cameraPoseKeys.size(); }
boost::shared_ptr<Ordering> getOrdering() { return ordering; }
protected:
void updateTriangulations() {
}
void updateOrdering(NonlinearFactorGraph &graph) {
bool debug = false;
if (1||debug) std::cout << "Landmark Keys: " << landmarkKeys.size() << " Pose Keys: " << cameraPoseKeys.size() << std::endl;
if (1||debug) std::cout << "Pose ordering: " << ordering->size() << std::endl;
if (orderingMethod == 1) {
OrderingMap orderingMap;
// Add landmark keys first for ordering
BOOST_FOREACH(const Key& key, landmarkKeys) {
orderingMap.insert( std::make_pair(key, 1) );
}
//Ordering::iterator oit;
BOOST_FOREACH(const Key& key, cameraPoseKeys) {
orderingMap.insert( std::make_pair(key, 2) );
}
*ordering = graph.orderingCOLAMDConstrained(orderingMap);
}
if (1||debug) std::cout << "Optimizing landmark first " << ordering->size() << std::endl;
}
void addTriangulatedLandmarks(NonlinearFactorGraph &graph, gtsam::Values::shared_ptr loadedValues,
gtsam::Values::shared_ptr graphValues, bool doTriangulatePoints) {
bool debug = false;
if(doTriangulatePoints)
std::cout << "Triangulating 3D points" << std::endl;
else
std::cout << "Reading initial guess for 3D points from file" << std::endl;
double rankTolerance=1;
std::cout << "rankTolerance " << rankTolerance << std::endl;
std::vector<boost::shared_ptr<ProjectionFactor> > projectionFactorVector;
typename std::vector<boost::shared_ptr<ProjectionFactor> >::iterator vfit;
Point3 point;
Pose3 cameraPose;
typename ProjectionFactorMap::iterator pfit;
if (debug) graphValues->print("graphValues \n");
if (debug) std::cout << " # END VALUES: " << std::endl;
// Iterate through all landmarks
if (debug) std::cout << " PROJECTION FACTOR GROUPED: " << projectionFactors.size();
int numProjectionFactors = 0;
int numProjectionFactorsAdded = 0;
int numFailures = 0;
for (pfit = projectionFactors.begin(); pfit != projectionFactors.end(); pfit++) { // for each landmark!
projectionFactorVector = (*pfit).second; // all factors connected to a given landmark
std::vector<Pose3> cameraPoses;
std::vector<Point2> measured;
typename std::vector< boost::shared_ptr<CALIBRATION> > Ks;
// Iterate through projection factors
for (vfit = projectionFactorVector.begin(); vfit != projectionFactorVector.end(); vfit++) { // for each factors connected to the landmark
numProjectionFactors++;
if (debug) std::cout << "ProjectionFactor: " << std::endl;
if (debug) (*vfit)->print("ProjectionFactor");
// Iterate through poses // find calibration
Key poseKey = (*vfit)->key1();
typename CalibrationMap::iterator calfit = keyCalibrationMap.find(poseKey);
if (calfit == keyCalibrationMap.end()){ // the pose is not there
std::cout << "ProjectionFactor: " << std::endl;
}else{
Ks.push_back(calfit->second);
}
cameraPoses.push_back( loadedValues->at<Pose3>(poseKey) ); // get poses connected to the landmark
measured.push_back( (*vfit)->measured() ); // get measurements of the landmark
}
// Triangulate landmark based on set of poses and measurements
if (doTriangulatePoints){
// std::cout << "Triangulating points " << std::endl;
try {
point = triangulatePoint3(cameraPoses, measured, Ks, rankTolerance);
if (debug) std::cout << "Triangulation succeeded: " << point << std::endl;
} catch( TriangulationUnderconstrainedException& e) {
if (debug) std::cout << "Triangulation failed because of unconstrained exception" << std::endl;
if (debug) {
BOOST_FOREACH(const Pose3& pose, cameraPoses) {
std::cout << " Pose: " << pose << std::endl;
}
}
numFailures++;
continue;
} catch( TriangulationCheiralityException& e) {
if (debug) std::cout << "Triangulation failed because of unconstrained exception" << std::endl;
if (debug) {
std::cout << "Triangulation failed because of cheirality exception" << std::endl;
BOOST_FOREACH(const Pose3& pose, cameraPoses) {
std::cout << " Pose: " << pose << std::endl;
}
}
numFailures++;
continue;
}
}else{ // we read 3D points from file
if (loadedValues->exists<Point3>((*pfit).first)){ // (*pfit).first) is the key of the landmark
// point
}else{
std::cout << "Trying to read non existing point from file " << std::endl;
}
}
// Add projection factors and pose values
for (vfit = projectionFactorVector.begin(); vfit != projectionFactorVector.end(); vfit++) { // for each proj factor connected to the landmark
numProjectionFactorsAdded++;
if (debug) std::cout << "Adding factor " << std::endl;
if (debug) (*vfit)->print("Projection Factor");
graph.push_back( (*vfit) ); // add factor to the graph
if (!graphValues->exists<Pose3>( (*vfit)->key1()) && loadedValues->exists<Pose3>((*vfit)->key1())) {
graphValues->insert((*vfit)->key1(), loadedValues->at<Pose3>((*vfit)->key1()));
cameraPoseKeys.push_back( (*vfit)->key1() ); // add camera poses, if necessary
// std::cout << "Added camera value " << std::endl;
}
}
// Add landmark value
if (debug) std::cout << "Adding value " << std::endl;
graphValues->insert( projectionFactorVector[0]->key2(), point); // add point
// std::cout << "Added point value " << std::endl;
landmarkKeys.push_back( projectionFactorVector[0]->key2() );
}
if (1||debug) std::cout << " # PROJECTION FACTORS CALCULATED: " << numProjectionFactors;
if (1||debug) std::cout << " # PROJECTION FACTORS ADDED: " << numProjectionFactorsAdded;
if (1||debug) std::cout << " # FAILURES: " << numFailures << std::endl;
}
const SharedNoiseModel noise_; ///< noise model used
///< (important that the order is the same as the keys that we use to create the factor)
boost::shared_ptr<CALIBRATION> K_; ///< shared pointer to calibration object
boost::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame
std::vector<Key> cameraPoseKeys;
std::vector<Key> landmarkKeys;
ProjectionFactorMap projectionFactors;
CalibrationMap keyCalibrationMap;
boost::shared_ptr<Ordering> ordering;
// orderingMethod: 0 - COLAMD, 1 - landmark first, then COLAMD on poses (constrained ordering)
int orderingMethod;
unsigned int totalNumMeasurements;
unsigned int numLandmarks;
unsigned int numPoses;
};
}
#endif /* SMARTPROJECTIONFACTORSCREATOR_H_ */

179
gtsam_unstable/slam/SmartProjectionFactorsCreator.h
View File

@ -1,179 +0,0 @@
/*
* SmartProjectionFactorsCreator.h
*
* Created on: Oct 8, 2013
* @author Zsolt Kira
*/
#ifndef SMARTPROJECTIONFACTORSCREATOR_H_
#define SMARTPROJECTIONFACTORSCREATOR_H_
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
// Use a map to store landmark/smart factor pairs
#include <gtsam/base/FastMap.h>
// #include <gtsam_unstable/slam/SmartProjectionFactor.h>
#include <gtsam_unstable/slam/SmartProjectionHessianFactor.h>
#include <boost/foreach.hpp>
#include <boost/assign/std/vector.hpp>
#include <fstream>
#include <iostream>
#include <utility>
namespace gtsam {
template<class POSE, class LANDMARK, class CALIBRATION = Cal3_S2>
class SmartProjectionFactorsCreator {
typedef SmartProjectionHessianFactor<Pose3, Point3, CALIBRATION> SmartFactor;
typedef FastMap<Key, boost::shared_ptr<SmartProjectionHessianFactorState> > SmartFactorToStateMap;
typedef FastMap<Key, boost::shared_ptr<SmartFactor> > SmartFactorMap;
public:
SmartProjectionFactorsCreator(const SharedNoiseModel& model,
const boost::shared_ptr<CALIBRATION>& K,
const double rankTol,
const double linThreshold,
boost::optional<POSE> body_P_sensor = boost::none) :
noise_(model), K_(K), rankTolerance_(rankTol),
linearizationThreshold_(linThreshold), body_P_sensor_(body_P_sensor),
totalNumMeasurements(0), numLandmarks(0) {};
void computePoints(Values& values) {
typename SmartFactorMap::iterator fit;
// Check if landmark exists in mapping
for(fit = smartFactors.begin(); fit != smartFactors.end(); fit++) {
Key pointKey = (*fit).first;
Point3 currentPoint;
if((*fit).second->point() && !(*fit).second->isDegenerate()){
currentPoint = *((*fit).second->point());
}else{
currentPoint = Point3(); // if we cannot the smartFactor is degenerate
}
values.insert(pointKey, currentPoint);
}
}
void add(Key landmarkKey,
Key poseKey, Point2 measurement, NonlinearFactorGraph &graph) {
std::vector<Key> views;
std::vector<Point2> measurements;
bool debug = false;
// Check if landmark exists in mapping
SmartFactorToStateMap::iterator fsit = smartFactorStates.find(landmarkKey);
typename SmartFactorMap::iterator fit = smartFactors.find(landmarkKey);
if (fsit != smartFactorStates.end() && fit != smartFactors.end()) {
if (debug) fprintf(stderr,"Adding measurement to existing landmark\n");
// Add measurement to smart factor
(*fit).second->add(measurement, poseKey, noise_, K_);
totalNumMeasurements++;
if (debug) (*fit).second->print();
} else {
if (debug) fprintf(stderr,"New landmark (%d,%d)\n", fsit != smartFactorStates.end(), fit != smartFactors.end());
views.push_back(poseKey);
measurements.push_back(measurement);
// This is a new landmark, create a new factor and add to mapping
boost::shared_ptr<SmartProjectionHessianFactorState> smartFactorState(new SmartProjectionHessianFactorState());
boost::shared_ptr<SmartFactor> smartFactor(new SmartFactor(rankTolerance_, linearizationThreshold_));
smartFactor->add(measurement, poseKey, noise_, K_);
smartFactorStates.insert( std::make_pair(landmarkKey, smartFactorState) );
smartFactors.insert( std::make_pair(landmarkKey, smartFactor) );
graph.push_back(smartFactor);
numLandmarks++;
//landmarkKeys.push_back( L(l) );
totalNumMeasurements++;
views.clear();
measurements.clear();
}
}
void add(Key landmarkKey, Key poseKey,
Point2 measurement,
const SharedNoiseModel& model,
const boost::shared_ptr<CALIBRATION>& Ki,
NonlinearFactorGraph &graph) {
std::vector<Key> views;
std::vector<Point2> measurements;
// std::cout << "matrix : " << K->K() << std::endl;
bool debug = false;
// Check if landmark exists in mapping
SmartFactorToStateMap::iterator fsit = smartFactorStates.find(landmarkKey);
typename SmartFactorMap::iterator fit = smartFactors.find(landmarkKey);
if (fsit != smartFactorStates.end() && fit != smartFactors.end()) {
if (debug) fprintf(stderr,"Adding measurement to existing landmark\n");
// (*fit).second->print("before: ");
// Add measurement to smart factor
(*fit).second->add(measurement, poseKey, model, Ki);
// (*fit).second->print("after: ");
totalNumMeasurements++;
if (debug) (*fit).second->print();
} else {
if (debug) std::cout <<"landmark " << DefaultKeyFormatter(landmarkKey) << "pose " << DefaultKeyFormatter(poseKey) << std::endl;
// if (debug) fprintf(stderr,"landmarkKey %d poseKey %d measurement\n", landmarkKey, fit != smartFactors.end());
// This is a new landmark, create a new factor and add to mapping
boost::shared_ptr<SmartProjectionHessianFactorState> smartFactorState(new SmartProjectionHessianFactorState());
boost::shared_ptr<SmartFactor> smartFactor(new SmartFactor(rankTolerance_, linearizationThreshold_));
smartFactor->add(measurement, poseKey, model, Ki);
// smartFactor->print("created: ");
// smartFactor->print(" ");
smartFactorStates.insert( std::make_pair(landmarkKey, smartFactorState) );
smartFactors.insert( std::make_pair(landmarkKey, smartFactor) );
graph.push_back(smartFactor);
if (debug) std::cout <<" graph size " << graph.size() << std::endl;
numLandmarks++;
totalNumMeasurements++;
}
}
unsigned int getTotalNumMeasurements() { return totalNumMeasurements; }
unsigned int getNumLandmarks() { return numLandmarks; }
protected:
const SharedNoiseModel noise_; ///< noise model used
///< (important that the order is the same as the keys that we use to create the factor)
boost::shared_ptr<CALIBRATION> K_; ///< shared pointer to calibration object
double rankTolerance_; ///< threshold to decide whether triangulation is degenerate
double linearizationThreshold_; ///< threshold to decide whether to re-linearize
boost::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame
SmartFactorToStateMap smartFactorStates;
SmartFactorMap smartFactors;
unsigned int totalNumMeasurements;
//landmarkKeys.push_back( L(l) );
unsigned int numLandmarks;
};
}
#endif /* SMARTPROJECTIONFACTORSCREATOR_H_ */

611
gtsam_unstable/slam/SmartProjectionHessianFactor.h
View File

@ -1,611 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 ProjectionFactor.h
* @brief Basic bearing factor from 2D measurement
* @author Chris Beall
* @author Luca Carlone
* @author Zsolt Kira
*/
#pragma once
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/linear/HessianFactor.h>
#include <vector>
#include <gtsam_unstable/geometry/triangulation.h>
#include <boost/optional.hpp>
//#include <boost/assign.hpp>
static bool isDebug=false;
namespace gtsam {
// default threshold for selective relinearization
static double defaultLinThreshold = -1; // 1e-7; // 0.01
// default threshold for retriangulation
static double defaultTriangThreshold = 1e-5;
// default threshold for rank deficient triangulation
static double defaultRankTolerance = 1; // this value may be scenario-dependent and has to be larger in presence of larger noise
// if set to true will use the rotation-only version for degenerate cases
static bool manageDegeneracy = true;
/**
* Structure for storing some state memory, used to speed up optimization
* @addtogroup SLAM
*/
class SmartProjectionHessianFactorState {
public:
static int lastID;
int ID;
SmartProjectionHessianFactorState() {
ID = lastID++;
calculatedHessian = false;
}
// Linearization point
Values values;
std::vector<Pose3> cameraPosesLinearization;
// Triangulation at current linearization point
Point3 point;
std::vector<Pose3> cameraPosesTriangulation;
bool degenerate;
bool cheiralityException;
// Overall reprojection error
double overallError;
std::vector<Pose3> cameraPosesError;
// Hessian representation (after Schur complement)
bool calculatedHessian;
Matrix H;
Vector gs_vector;
std::vector<Matrix> Gs;
std::vector<Vector> gs;
double f;
// C = Hl'Hl
// Cinv = inv(Hl'Hl)
// Matrix3 Cinv;
// E = Hx'Hl
// w = Hl'b
};
int SmartProjectionHessianFactorState::lastID = 0;
/**
* The calibration is known here.
* @addtogroup SLAM
*/
template<class POSE, class LANDMARK, class CALIBRATION = Cal3_S2>
class SmartProjectionHessianFactor: public NonlinearFactor {
protected:
// Keep a copy of measurement and calibration for I/O
std::vector<Point2> measured_; ///< 2D measurement for each of the m views
std::vector< SharedNoiseModel > noise_; ///< noise model used
///< (important that the order is the same as the keys that we use to create the factor)
std::vector< boost::shared_ptr<CALIBRATION> > K_all_; ///< shared pointer to calibration object (one for each camera)
double retriangulationThreshold; ///< threshold to decide whether to re-triangulate
double rankTolerance; ///< threshold to decide whether triangulation is degenerate
double linearizationThreshold; ///< threshold to decide whether to re-linearize
boost::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame (one for each camera)
boost::shared_ptr<SmartProjectionHessianFactorState> state_;
// verbosity handling for Cheirality Exceptions
bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false)
bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false)
public:
/// shorthand for base class type
typedef NonlinearFactor Base;
/// shorthand for this class
typedef SmartProjectionHessianFactor<POSE, LANDMARK, CALIBRATION> This;
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;
/// shorthand for smart projection factor state variable
typedef boost::shared_ptr<SmartProjectionHessianFactorState> SmartFactorStatePtr;
/**
* Constructor
* @param poseKeys is the set of indices corresponding to the cameras observing the same landmark
* @param measured is the 2m dimensional location of the projection of a single landmark in the m views (the measurements)
* @param model is the standard deviation (current version assumes that the uncertainty is the same for all views)
* @param K shared pointer to the constant calibration
* @param body_P_sensor is the transform from body to sensor frame (default identity)
*/
SmartProjectionHessianFactor(
const double rankTol = defaultRankTolerance,
const double linThreshold = defaultLinThreshold,
boost::optional<POSE> body_P_sensor = boost::none,
SmartFactorStatePtr state = SmartFactorStatePtr(new SmartProjectionHessianFactorState())) :
retriangulationThreshold(defaultTriangThreshold), rankTolerance(rankTol),
linearizationThreshold(linThreshold), body_P_sensor_(body_P_sensor),
state_(state), throwCheirality_(false), verboseCheirality_(false) {}
/** Virtual destructor */
virtual ~SmartProjectionHessianFactor() {}
/**
* add a new measurement and pose key
* @param measured is the 2m dimensional location of the projection of a single landmark in the m view (the measurement)
* @param poseKey is the index corresponding to the camera observing the same landmark
*/
void add(const Point2 measured_i, const Key poseKey_i, const SharedNoiseModel noise_i,
const boost::shared_ptr<CALIBRATION> K_i) {
measured_.push_back(measured_i);
keys_.push_back(poseKey_i);
noise_.push_back(noise_i);
K_all_.push_back(K_i);
}
void add(std::vector< Point2 > measurements, std::vector< Key > poseKeys, std::vector< SharedNoiseModel > noises,
std::vector< boost::shared_ptr<CALIBRATION> > Ks) {
for(size_t i = 0; i < measurements.size(); i++) {
measured_.push_back(measurements.at(i));
keys_.push_back(poseKeys.at(i));
noise_.push_back(noises.at(i));
K_all_.push_back(Ks.at(i));
}
}
void add(std::vector< Point2 > measurements, std::vector< Key > poseKeys, const SharedNoiseModel noise,
const boost::shared_ptr<CALIBRATION> K) {
for(size_t i = 0; i < measurements.size(); i++) {
measured_.push_back(measurements.at(i));
keys_.push_back(poseKeys.at(i));
noise_.push_back(noise);
K_all_.push_back(K);
}
}
// This function checks if the new linearization point is the same as the one used for previous triangulation
// (if not, a new triangulation is needed)
static bool decideIfTriangulate(std::vector<Pose3> cameraPoses, std::vector<Pose3> oldPoses, double retriangulationThreshold) {
// several calls to linearize will be done from the same linearization point, hence it is not needed to re-triangulate
// Note that this is not yet "selecting linearization", that will come later, and we only check if the
// current linearization is the "same" (up to tolerance) w.r.t. the last time we triangulated the point
// if we do not have a previous linearization point or the new linearization point includes more poses
if(oldPoses.empty() || (cameraPoses.size() != oldPoses.size()))
return true;
for(size_t i = 0; i < cameraPoses.size(); i++) {
if (!cameraPoses[i].equals(oldPoses[i], retriangulationThreshold)) {
return true; // at least two poses are different, hence we retriangulate
}
}
return false; // if we arrive to this point all poses are the same and we don't need re-triangulation
}
// This function checks if the new linearization point is 'close' to the previous one used for linearization
// (if not, a new linearization is needed)
static bool decideIfLinearize(std::vector<Pose3> cameraPoses, std::vector<Pose3> oldPoses, double linearizationThreshold) {
// "selective linearization"
// The function evaluates how close are the old and the new poses, transformed in the ref frame of the first pose
// (we only care about the "rigidity" of the poses, not about their absolute pose)
// if we do not have a previous linearization point or the new linearization point includes more poses
if(oldPoses.empty() || (cameraPoses.size() != oldPoses.size()))
return true;
Pose3 firstCameraPose;
Pose3 firstCameraPoseOld;
for(size_t i = 0; i < cameraPoses.size(); i++) {
if(i==0){ // we store the initial pose, this is useful for selective re-linearization
firstCameraPose = cameraPoses[i];
firstCameraPoseOld = oldPoses[i];
continue;
}
// we compare the poses in the frame of the first pose
Pose3 localCameraPose = firstCameraPose.between(cameraPoses[i]);
Pose3 localCameraPoseOld = firstCameraPoseOld.between(oldPoses[i]);
if (!localCameraPose.equals(localCameraPoseOld, linearizationThreshold)) {
return true; // at least two "relative" poses are different, hence we re-linerize
}
}
return false; // if we arrive to this point all poses are the same and we don't need re-linerize
}
/**
* print
* @param s optional string naming the factor
* @param keyFormatter optional formatter useful for printing Symbols
*/
void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "SmartProjectionHessianFactor, z = \n ";
BOOST_FOREACH(const Point2& p, measured_) {
std::cout << "measurement, p = "<< p << std::endl;
}
BOOST_FOREACH(const SharedNoiseModel& noise_i, noise_) {
noise_i->print("noise model = ");
}
BOOST_FOREACH(const boost::shared_ptr<CALIBRATION>& K, K_all_) {
K->print("calibration = ");
}
if(this->body_P_sensor_){
this->body_P_sensor_->print(" sensor pose in body frame: ");
}
Base::print("", keyFormatter);
}
/// equals
virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
const This *e = dynamic_cast<const This*>(&p);
bool areMeasurementsEqual = true;
for(size_t i = 0; i < measured_.size(); i++) {
if(this->measured_.at(i).equals(e->measured_.at(i), tol) == false)
areMeasurementsEqual = false;
break;
}
return e
&& Base::equals(p, tol)
&& areMeasurementsEqual
//&& this->K_->equals(*e->K_all_, tol);
&& ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
}
/// get the dimension of the factor (number of rows on linearization)
virtual size_t dim() const {
return 6*keys_.size();
}
/// linearize returns a Hessianfactor that is an approximation of error(p)
virtual boost::shared_ptr<GaussianFactor> linearize(const Values& values) const {
bool blockwise = false; // the full matrix version in faster
int dim_landmark = 3; // for degenerate instances this will become 2 (direction-only information)
// Create structures for Hessian Factors
unsigned int numKeys = keys_.size();
if(isDebug) {std::cout<< " numKeys = "<< numKeys<<std::endl; }
std::vector<Index> js;
std::vector<Matrix> Gs(numKeys*(numKeys+1)/2);
std::vector<Vector> gs(numKeys);
double f=0;
// Collect all poses (Cameras)
std::vector<Pose3> cameraPoses;
BOOST_FOREACH(const Key& k, keys_) {
Pose3 cameraPose;
if(body_P_sensor_) { cameraPose = values.at<Pose3>(k).compose(*body_P_sensor_);}
else { cameraPose = values.at<Pose3>(k);}
cameraPoses.push_back(cameraPose);
}
if(cameraPoses.size() < 2){ // if we have a single pose the corresponding factor is uninformative
state_->degenerate = true;
BOOST_FOREACH(gtsam::Matrix& m, Gs) m = zeros(6, 6);
BOOST_FOREACH(Vector& v, gs) v = zero(6);
return HessianFactor::shared_ptr(new HessianFactor(keys_, Gs, gs, f)); // TODO: Debug condition, uncomment when fixed
}
bool retriangulate = decideIfTriangulate(cameraPoses, state_->cameraPosesTriangulation, retriangulationThreshold);
if(retriangulate) {// we store the current poses used for triangulation
state_->cameraPosesTriangulation = cameraPoses;
}
if (retriangulate) {
// We triangulate the 3D position of the landmark
try {
// std::cout << "triangulatePoint3 i \n" << rankTolerance << std::endl;
state_->point = triangulatePoint3(cameraPoses, measured_, K_all_, rankTolerance);
state_->degenerate = false;
state_->cheiralityException = false;
} catch( TriangulationUnderconstrainedException& e) {
// if TriangulationUnderconstrainedException can be
// 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before
// 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark)
// in the second case we want to use a rotation-only smart factor
//std::cout << "Triangulation failed " << e.what() << std::endl; // point triangulated at infinity
state_->degenerate = true;
state_->cheiralityException = false;
} catch( TriangulationCheiralityException& e) {
// point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers
// we manage this case by either discarding the smart factor, or imposing a rotation-only constraint
//std::cout << e.what() << std::end;
state_->cheiralityException = true;
}
}
if (!manageDegeneracy && (state_->cheiralityException || state_->degenerate) ){
// std::cout << "In linearize: exception" << std::endl;
BOOST_FOREACH(gtsam::Matrix& m, Gs) m = zeros(6, 6);
BOOST_FOREACH(Vector& v, gs) v = zero(6);
return HessianFactor::shared_ptr(new HessianFactor(keys_, Gs, gs, f));
}
if (state_->cheiralityException || state_->degenerate){ // if we want to manage the exceptions with rotation-only factors
state_->degenerate = true;
dim_landmark = 2;
}
bool doLinearize;
if (linearizationThreshold >= 0){//by convention if linearizationThreshold is negative we always relinearize
// std::cout << "Temporary disabled" << std::endl;
doLinearize = decideIfLinearize(cameraPoses, state_->cameraPosesLinearization, linearizationThreshold);
}
else{
doLinearize = true;
}
if (doLinearize) {
state_->cameraPosesLinearization = cameraPoses;
}
if(!doLinearize){ // return the previous Hessian factor
// std::cout << "Using stored factors :) " << std::endl;
return HessianFactor::shared_ptr(new HessianFactor(keys_, state_->Gs, state_->gs, state_->f));
}
if (blockwise == false){ // version with full matrix multiplication
// ==========================================================================================================
Matrix Hx2 = zeros(2 * numKeys, 6 * numKeys);
Matrix Hl2 = zeros(2 * numKeys, dim_landmark);
Vector b2 = zero(2 * numKeys);
if(state_->degenerate){
for(size_t i = 0; i < measured_.size(); i++) {
Pose3 pose = cameraPoses.at(i);
PinholeCamera<CALIBRATION> camera(pose, *K_all_.at(i));
if(i==0){ // first pose
state_->point = camera.backprojectPointAtInfinity(measured_.at(i));
// 3D parametrization of point at infinity: [px py 1]
// std::cout << "point_ " << state_->point<< std::endl;
}
Matrix Hxi, Hli;
Vector bi = -( camera.projectPointAtInfinity(state_->point,Hxi,Hli) - measured_.at(i) ).vector();
// std::cout << "Hxi \n" << Hxi<< std::endl;
// std::cout << "Hli \n" << Hli<< std::endl;
noise_.at(i)-> WhitenSystem(Hxi, Hli, bi);
f += bi.squaredNorm();
Hx2.block( 2*i, 6*i, 2, 6 ) = Hxi;
Hl2.block( 2*i, 0, 2, 2 ) = Hli;
subInsert(b2,bi,2*i);
}
// std::cout << "Hx2 \n" << Hx2<< std::endl;
// std::cout << "Hl2 \n" << Hl2<< std::endl;
}
else{
for(size_t i = 0; i < measured_.size(); i++) {
Pose3 pose = cameraPoses.at(i);
PinholeCamera<CALIBRATION> camera(pose, *K_all_.at(i));
Matrix Hxi, Hli;
Vector bi;
try {
bi = -( camera.project(state_->point,Hxi,Hli) - measured_.at(i) ).vector();
} catch ( CheiralityException& e) {
std::cout << "Cheirality exception " << state_->ID << std::endl;
exit(EXIT_FAILURE);
}
noise_.at(i)-> WhitenSystem(Hxi, Hli, bi);
f += bi.squaredNorm();
Hx2.block( 2*i, 6*i, 2, 6 ) = Hxi;
Hl2.block( 2*i, 0, 2, 3 ) = Hli;
subInsert(b2,bi,2*i);
}
}
// Shur complement trick
Matrix H(6 * numKeys, 6 * numKeys);
Matrix C2;
Vector gs_vector;
C2.noalias() = (Hl2.transpose() * Hl2).inverse();
H.noalias() = Hx2.transpose() * (Hx2 - (Hl2 * (C2 * (Hl2.transpose() * Hx2))));
gs_vector.noalias() = Hx2.transpose() * (b2 - (Hl2 * (C2 * (Hl2.transpose() * b2))));
// Populate Gs and gs
int GsCount2 = 0;
for(size_t i1 = 0; i1 < numKeys; i1++) {
gs.at(i1) = sub(gs_vector, 6*i1, 6*i1 + 6);
for(size_t i2 = 0; i2 < numKeys; i2++) {
if (i2 >= i1) {
Gs.at(GsCount2) = H.block(6*i1, 6*i2, 6, 6);
GsCount2++;
}
}
}
}
// ==========================================================================================================
if(linearizationThreshold >= 0){ // if we do not use selective relinearization we don't need to store these variables
state_->calculatedHessian = true;
state_->Gs = Gs;
state_->gs = gs;
state_->f = f;
}
return HessianFactor::shared_ptr(new HessianFactor(keys_, Gs, gs, f));
}
/**
* Calculate the error of the factor.
* This is the log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/\sigma^2 \f$ in case of Gaussian.
* In this class, we take the raw prediction error \f$ h(x)-z \f$, ask the noise model
* to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5.
*/
virtual double error(const Values& values) const {
if (this->active(values)) {
double overallError=0;
// Collect all poses (Cameras)
std::vector<Pose3> cameraPoses;
BOOST_FOREACH(const Key& k, keys_) {
Pose3 cameraPose;
if(body_P_sensor_) { cameraPose = values.at<Pose3>(k).compose(*body_P_sensor_);}
else { cameraPose = values.at<Pose3>(k);}
cameraPoses.push_back(cameraPose);
if(0&& isDebug) {cameraPose.print("cameraPose = "); }
}
if(cameraPoses.size() < 2){ // if we have a single pose the corresponding factor is uninformative
return 0.0;
}
bool retriangulate = decideIfTriangulate(cameraPoses, state_->cameraPosesTriangulation, retriangulationThreshold);
if(retriangulate) {// we store the current poses used for triangulation
state_->cameraPosesTriangulation = cameraPoses;
}
if (retriangulate) {
// We triangulate the 3D position of the landmark
try {
state_->point = triangulatePoint3(cameraPoses, measured_, K_all_, rankTolerance);
state_->degenerate = false;
state_->cheiralityException = false;
} catch( TriangulationUnderconstrainedException& e) {
// if TriangulationUnderconstrainedException can be
// 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before
// 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark)
// in the second case we want to use a rotation-only smart factor
//std::cout << "Triangulation failed " << e.what() << std::endl; // point triangulated at infinity
state_->degenerate = true;
state_->cheiralityException = false;
} catch( TriangulationCheiralityException& e) {
// point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers
// we manage this case by either discarding the smart factor, or imposing a rotation-only constraint
//std::cout << e.what() << std::end;
state_->cheiralityException = true;
}
}
if (!manageDegeneracy && (state_->cheiralityException || state_->degenerate) ){
// if we don't want to manage the exceptions we discard the factor
// std::cout << "In error evaluation: exception" << std::endl;
return 0.0;
}
if (state_->cheiralityException || state_->degenerate){ // if we want to manage the exceptions with rotation-only factors
state_->degenerate = true;
}
if(state_->degenerate){
// return 0.0; // TODO: this maybe should be zero?
for(size_t i = 0; i < measured_.size(); i++) {
Pose3 pose = cameraPoses.at(i);
PinholeCamera<CALIBRATION> camera(pose, *K_all_.at(i));
if(i==0){ // first pose
state_->point = camera.backprojectPointAtInfinity(measured_.at(i)); // 3D parametrization of point at infinity
}
Point2 reprojectionError(camera.projectPointAtInfinity(state_->point) - measured_.at(i));
overallError += 0.5 * noise_.at(i)->distance( reprojectionError.vector() );
//overallError += reprojectionError.vector().norm();
}
return overallError;
}
else{
for(size_t i = 0; i < measured_.size(); i++) {
Pose3 pose = cameraPoses.at(i);
PinholeCamera<CALIBRATION> camera(pose, *K_all_.at(i));
try {
Point2 reprojectionError(camera.project(state_->point) - measured_.at(i));
//std::cout << "Reprojection error: " << reprojectionError << std::endl;
overallError += 0.5 * noise_.at(i)->distance( reprojectionError.vector() );
//overallError += reprojectionError.vector().norm();
} catch ( CheiralityException& e) {
std::cout << "Cheirality exception " << state_->ID << std::endl;
exit(EXIT_FAILURE);
}
}
return overallError;
}
} else { // else of active flag
return 0.0;
}
}
/** return the measurements */
const Vector& measured() const {
return measured_;
}
/** return the noise model */
const SharedNoiseModel& noise() const {
return noise_;
}
/** return the landmark */
boost::optional<Point3> point() const {
return state_->point;
}
/** return the calibration object */
inline const boost::shared_ptr<CALIBRATION> calibration() const {
return K_all_;
}
/** return the calibration object */
inline bool isDegenerate() const {
return (state_->cheiralityException || state_->degenerate);
}
/** return verbosity */
inline bool verboseCheirality() const { return verboseCheirality_; }
/** return flag for throwing cheirality exceptions */
inline bool throwCheirality() const { return throwCheirality_; }
private:
/// Serialization function
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(measured_);
ar & BOOST_SERIALIZATION_NVP(K_all_);
ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
}
};
} // \ namespace gtsam

1146
gtsam_unstable/slam/tests/testSmartProjectionHessianFactor.cpp
View File

File diff suppressed because it is too large Load Diff