diff --git a/gtsam_unstable/examples/SmartProjectionFactorExampleBAL.cpp b/gtsam_unstable/examples/SmartProjectionFactorExampleBAL.cpp deleted file mode 100644 index 01e5ca8b0..000000000 --- a/gtsam_unstable/examples/SmartProjectionFactorExampleBAL.cpp +++ /dev/null @@ -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 - -// Both relative poses and recovered trajectory poses will be stored as Pose3 objects -#include -#include -#include - -// 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 - -// We want to use iSAM2 to solve the range-SLAM problem incrementally -#include - -// 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 - -// We will use a non-linear solver to batch-initialize from the first 150 frames -#include -#include - -// In GTSAM, measurement functions are represented as 'factors'. Several common factors -// have been provided with the library for solving robotics SLAM problems. -#include -#include -#include -#include - -// Standard headers, added last, so we know headers above work on their own -#include -#include -#include -#include -#include - -using namespace std; -using namespace gtsam; -using namespace boost::assign; -namespace NM = gtsam::noiseModel; - -using symbol_shorthand::X; -using symbol_shorthand::L; - -typedef PriorFactor Pose3Prior; -typedef FastMap OrderingMap; - -typedef SmartProjectionFactorsCreator SmartFactorsCreator; -typedef GenericProjectionFactorsCreator ProjectionFactorsCreator; - -bool debug = false; - - -void optimizeGraphLM(NonlinearFactorGraph &graph, gtsam::Values::shared_ptr graphValues, Values &result, boost::shared_ptr &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(*(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(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 > K_cameras; - boost::shared_ptr 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 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(); - cout << "Poses in ProjectionGraph values: "<< valuesProjPoses.size() << endl; - Values valuesProjPoints = graphProjectionValues->filter(); - 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(); - cout << "Poses in ProjectionGraph values: "<< valuesProjPoses.size() << endl; - Values valuesProjPoints = graphProjectionValues->filter(); - 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); -} - diff --git a/gtsam_unstable/examples/SmartProjectionFactorExampleKitti.cpp b/gtsam_unstable/examples/SmartProjectionFactorExampleKitti.cpp deleted file mode 100644 index 27b88cc4b..000000000 --- a/gtsam_unstable/examples/SmartProjectionFactorExampleKitti.cpp +++ /dev/null @@ -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 - -// Both relative poses and recovered trajectory poses will be stored as Pose3 objects -#include - -// 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 - -// We want to use iSAM2 to solve the range-SLAM problem incrementally -#include - -// 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 - -// We will use a non-linear solver to batch-initialize from the first 150 frames -#include -#include - -// In GTSAM, measurement functions are represented as 'factors'. Several common factors -// have been provided with the library for solving robotics SLAM problems. -#include -#include -#include - -// Standard headers, added last, so we know headers above work on their own -#include -#include -#include -#include -#include - -using namespace std; -using namespace gtsam; -using namespace boost::assign; -namespace NM = gtsam::noiseModel; - -using symbol_shorthand::X; -using symbol_shorthand::L; - -typedef PriorFactor Pose3Prior; -typedef SmartProjectionFactorsCreator SmartFactorsCreator; -typedef GenericProjectionFactorsCreator ProjectionFactorsCreator; -typedef FastMap 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 keys) { - Values::shared_ptr values(new Values()); - std::list::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 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(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::value_type key_value, values.filter()) { - 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().size() > 0) { - // write landmarks - filename = directory_ + "landmarks.txt"; - fout.open(filename.c_str()); - - BOOST_FOREACH(Values::ConstFiltered::value_type key_value, values.filter()) { - 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) { - // 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 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(*(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 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 landmarkKeys, cameraPoseKeys; - Values values; - Values result; - bool optimized = false; - boost::shared_ptr 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(X(r)) && loadedValues->exists(X(r))) { - graphSmartValues->insert(X(r), loadedValues->at(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); -} diff --git a/gtsam_unstable/slam/GenericProjectionFactorsCreator.h b/gtsam_unstable/slam/GenericProjectionFactorsCreator.h deleted file mode 100644 index d07281ea1..000000000 --- a/gtsam_unstable/slam/GenericProjectionFactorsCreator.h +++ /dev/null @@ -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 - -#include - -#include - -// Use a map to store landmark/smart factor pairs -#include -#include - - -#include -#include -//#include -//#include -//#include -//#include -//#include -#include - -namespace gtsam { - - - template - class GenericProjectionFactorsCreator { - - typedef GenericProjectionFactor ProjectionFactor; - typedef FastMap > > ProjectionFactorMap; - typedef FastMap > CalibrationMap; - typedef FastMap OrderingMap; - - public: - GenericProjectionFactorsCreator(const SharedNoiseModel& model, - const boost::shared_ptr& K, - boost::optional 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(*(new Ordering())); - }; - - void add(Key landmarkKey, - Key poseKey, Point2 measurement, NonlinearFactorGraph &graph) { - bool debug = false; - - // Create projection factor - boost::shared_ptr 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 > 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& 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 > 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 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 > projectionFactorVector; - typename std::vector >::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 cameraPoses; - std::vector measured; - typename std::vector< boost::shared_ptr > 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(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((*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( (*vfit)->key1()) && loadedValues->exists((*vfit)->key1())) { - graphValues->insert((*vfit)->key1(), loadedValues->at((*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 K_; ///< shared pointer to calibration object - - boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame - - std::vector cameraPoseKeys; - std::vector landmarkKeys; - ProjectionFactorMap projectionFactors; - CalibrationMap keyCalibrationMap; - boost::shared_ptr 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_ */ diff --git a/gtsam_unstable/slam/SmartProjectionFactorsCreator.h b/gtsam_unstable/slam/SmartProjectionFactorsCreator.h deleted file mode 100644 index 67c852542..000000000 --- a/gtsam_unstable/slam/SmartProjectionFactorsCreator.h +++ /dev/null @@ -1,179 +0,0 @@ -/* - * SmartProjectionFactorsCreator.h - * - * Created on: Oct 8, 2013 - * @author Zsolt Kira - */ - -#ifndef SMARTPROJECTIONFACTORSCREATOR_H_ -#define SMARTPROJECTIONFACTORSCREATOR_H_ - -#include -#include -#include - -// Use a map to store landmark/smart factor pairs -#include -// #include -#include - -#include -#include - -#include -#include -#include - -namespace gtsam { - - template - class SmartProjectionFactorsCreator { - - typedef SmartProjectionHessianFactor SmartFactor; - typedef FastMap > SmartFactorToStateMap; - typedef FastMap > SmartFactorMap; - - public: - SmartProjectionFactorsCreator(const SharedNoiseModel& model, - const boost::shared_ptr& K, - const double rankTol, - const double linThreshold, - boost::optional 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 views; - std::vector 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 smartFactorState(new SmartProjectionHessianFactorState()); - boost::shared_ptr 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& Ki, - NonlinearFactorGraph &graph) { - - std::vector views; - std::vector 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 smartFactorState(new SmartProjectionHessianFactorState()); - boost::shared_ptr 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 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 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_ */ diff --git a/gtsam_unstable/slam/SmartProjectionHessianFactor.h b/gtsam_unstable/slam/SmartProjectionHessianFactor.h deleted file mode 100644 index 510af56ea..000000000 --- a/gtsam_unstable/slam/SmartProjectionHessianFactor.h +++ /dev/null @@ -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 -#include -#include -#include -#include -#include -#include -#include -//#include - -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 cameraPosesLinearization; - - // Triangulation at current linearization point - Point3 point; - std::vector cameraPosesTriangulation; - bool degenerate; - bool cheiralityException; - - // Overall reprojection error - double overallError; - std::vector cameraPosesError; - - // Hessian representation (after Schur complement) - bool calculatedHessian; - Matrix H; - Vector gs_vector; - std::vector Gs; - std::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 SmartProjectionHessianFactor: public NonlinearFactor { - protected: - - // Keep a copy of measurement and calibration for I/O - std::vector 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 > 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 body_P_sensor_; ///< The pose of the sensor in the body frame (one for each camera) - - boost::shared_ptr 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 This; - - /// shorthand for a smart pointer to a factor - typedef boost::shared_ptr shared_ptr; - - /// shorthand for smart projection factor state variable - typedef boost::shared_ptr 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 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 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 > 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 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 cameraPoses, std::vector 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 cameraPoses, std::vector 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& 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(&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 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< js; - std::vector Gs(numKeys*(numKeys+1)/2); - std::vector gs(numKeys); - double f=0; - - // Collect all poses (Cameras) - std::vector cameraPoses; - BOOST_FOREACH(const Key& k, keys_) { - Pose3 cameraPose; - if(body_P_sensor_) { cameraPose = values.at(k).compose(*body_P_sensor_);} - else { cameraPose = values.at(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 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 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 cameraPoses; - BOOST_FOREACH(const Key& k, keys_) { - Pose3 cameraPose; - if(body_P_sensor_) { cameraPose = values.at(k).compose(*body_P_sensor_);} - else { cameraPose = values.at(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 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 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 point() const { - return state_->point; - } - - /** return the calibration object */ - inline const boost::shared_ptr 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 - 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 diff --git a/gtsam_unstable/slam/tests/testSmartProjectionHessianFactor.cpp b/gtsam_unstable/slam/tests/testSmartProjectionHessianFactor.cpp deleted file mode 100644 index bb3e58cee..000000000 --- a/gtsam_unstable/slam/tests/testSmartProjectionHessianFactor.cpp +++ /dev/null @@ -1,1146 +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 TestSmartProjectionHessianFactor.cpp - * @brief Unit tests for ProjectionFactor Class - * @author Frank Dellaert - * @date Nov 2009 - */ - -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -using namespace std; -using namespace boost::assign; -using namespace gtsam; - -static bool isDebugTest = false; - -// make a realistic calibration matrix -static double fov = 60; // degrees -static size_t w=640,h=480; - -static Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h)); -static Cal3_S2::shared_ptr K2(new Cal3_S2(1500, 1200, 0, 640, 480)); -static boost::shared_ptr Kbundler(new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); - -static double rankTol = 1.0; -static double linThreshold = -1.0; -// Create a noise model for the pixel error -static SharedNoiseModel model(noiseModel::Unit::Create(2)); - -// Convenience for named keys -using symbol_shorthand::X; -using symbol_shorthand::L; - -// tests data -Symbol x1('X', 1); -Symbol x2('X', 2); -Symbol x3('X', 3); - -static Key poseKey1(x1); -static Point2 measurement1(323.0, 240.0); -static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); - -typedef SmartProjectionHessianFactor SmartFactor; -typedef SmartProjectionHessianFactor SmartFactorBundler; - -/* ************************************************************************* */ -TEST( SmartProjectionHessianFactor, Constructor) { - SmartFactor::shared_ptr factor1(new SmartFactor()); -} - -/* ************************************************************************* */ -TEST( SmartProjectionHessianFactor, Constructor2) { - SmartFactor factor1(rankTol, linThreshold); -} - -/* ************************************************************************* */ -TEST( SmartProjectionHessianFactor, Constructor3) { - SmartFactor::shared_ptr factor1(new SmartFactor()); - factor1->add(measurement1, poseKey1, model, K); -} - -/* ************************************************************************* */ -TEST( SmartProjectionHessianFactor, Constructor4) { - SmartFactor factor1(rankTol, linThreshold); - factor1.add(measurement1, poseKey1, model, K); -} - -/* ************************************************************************* */ -TEST( SmartProjectionHessianFactor, ConstructorWithTransform) { - SmartFactor factor1(rankTol, linThreshold, body_P_sensor1); - factor1.add(measurement1, poseKey1, model, K); -} - -/* ************************************************************************* */ -TEST( SmartProjectionHessianFactor, Equals ) { - SmartFactor::shared_ptr factor1(new SmartFactor()); - factor1->add(measurement1, poseKey1, model, K); - - SmartFactor::shared_ptr factor2(new SmartFactor()); - factor2->add(measurement1, poseKey1, model, K); - - CHECK(assert_equal(*factor1, *factor2)); -} - -/* *************************************************************************/ -TEST( SmartProjectionHessianFactor, noiseless ){ - // cout << " ************************ SmartProjectionHessianFactor: noisy ****************************" << endl; - - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); - SimpleCamera level_camera(level_pose, *K2); - - // create second camera 1 meter to the right of first camera - Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0)); - SimpleCamera level_camera_right(level_pose_right, *K2); - - // landmark ~5 meters infront of camera - Point3 landmark(5, 0.5, 1.2); - - // 1. Project two landmarks into two cameras and triangulate - Point2 level_uv = level_camera.project(landmark); - Point2 level_uv_right = level_camera_right.project(landmark); - - Values values; - values.insert(x1, level_pose); - values.insert(x2, level_pose_right); - - SmartFactor::shared_ptr factor1(new SmartFactor()); - factor1->add(level_uv, x1, model, K); - factor1->add(level_uv_right, x2, model, K); - - double actualError = factor1->error(values); - double expectedError = 0.0; - DOUBLES_EQUAL(expectedError, actualError, 1e-7); -} - -/* *************************************************************************/ -TEST( SmartProjectionHessianFactor, noisy ){ - // cout << " ************************ SmartProjectionHessianFactor: noisy ****************************" << endl; - - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); - SimpleCamera level_camera(level_pose, *K2); - - // create second camera 1 meter to the right of first camera - Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0)); - SimpleCamera level_camera_right(level_pose_right, *K2); - - // landmark ~5 meters infront of camera - Point3 landmark(5, 0.5, 1.2); - - // 1. Project two landmarks into two cameras and triangulate - Point2 pixelError(0.2,0.2); - Point2 level_uv = level_camera.project(landmark) + pixelError; - Point2 level_uv_right = level_camera_right.project(landmark); - - Values values; - values.insert(x1, level_pose); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); - values.insert(x2, level_pose_right.compose(noise_pose)); - - SmartFactor::shared_ptr factor1(new SmartFactor()); - factor1->add(level_uv, x1, model, K); - factor1->add(level_uv_right, x2, model, K); - - double actualError1= factor1->error(values); - - SmartFactor::shared_ptr factor2(new SmartFactor()); - vector measurements; - measurements.push_back(level_uv); - measurements.push_back(level_uv_right); - - std::vector< SharedNoiseModel > noises; - noises.push_back(model); - noises.push_back(model); - - std::vector< boost::shared_ptr > Ks; ///< shared pointer to calibration object (one for each camera) - Ks.push_back(K); - Ks.push_back(K); - - std::vector views; - views.push_back(x1); - views.push_back(x2); - - factor2->add(measurements, views, noises, Ks); - - double actualError2= factor2->error(values); - - DOUBLES_EQUAL(actualError1, actualError2, 1e-7); -} - - -/* *************************************************************************/ -TEST( SmartProjectionHessianFactor, 3poses_smart_projection_factor ){ - // cout << " ************************ SmartProjectionHessianFactor: 3 cams + 3 landmarks **********************" << endl; - - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); - SimpleCamera cam1(pose1, *K2); - - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); - SimpleCamera cam2(pose2, *K2); - - // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); - SimpleCamera cam3(pose3, *K2); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); - Point3 landmark3(3, 0, 3.0); - - vector measurements_cam1, measurements_cam2, measurements_cam3; - - // 1. Project three landmarks into three cameras and triangulate - Point2 cam1_uv1 = cam1.project(landmark1); - Point2 cam2_uv1 = cam2.project(landmark1); - Point2 cam3_uv1 = cam3.project(landmark1); - measurements_cam1.push_back(cam1_uv1); - measurements_cam1.push_back(cam2_uv1); - measurements_cam1.push_back(cam3_uv1); - - Point2 cam1_uv2 = cam1.project(landmark2); - Point2 cam2_uv2 = cam2.project(landmark2); - Point2 cam3_uv2 = cam3.project(landmark2); - measurements_cam2.push_back(cam1_uv2); - measurements_cam2.push_back(cam2_uv2); - measurements_cam2.push_back(cam3_uv2); - - Point2 cam1_uv3 = cam1.project(landmark3); - Point2 cam2_uv3 = cam2.project(landmark3); - Point2 cam3_uv3 = cam3.project(landmark3); - measurements_cam3.push_back(cam1_uv3); - measurements_cam3.push_back(cam2_uv3); - measurements_cam3.push_back(cam3_uv3); - - std::vector views; - views.push_back(x1); - views.push_back(x2); - views.push_back(x3); - - SmartFactor::shared_ptr smartFactor1(new SmartFactor()); - smartFactor1->add(measurements_cam1, views, model, K2); - - SmartFactor::shared_ptr smartFactor2(new SmartFactor()); - smartFactor2->add(measurements_cam2, views, model, K2); - - SmartFactor::shared_ptr smartFactor3(new SmartFactor()); - smartFactor3->add(measurements_cam3, views, model, K2); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PriorFactor(x2, pose2, noisePrior)); - - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise - Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - // initialize third pose with some noise, we expect it to move back to original pose3 - values.insert(x3, pose3*noise_pose); - if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); - - LevenbergMarquardtParams params; - if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; - if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; - - Values result; - gttic_(SmartProjectionHessianFactor); - LevenbergMarquardtOptimizer optimizer(graph, values, params); - result = optimizer.optimize(); - gttoc_(SmartProjectionHessianFactor); - tictoc_finishedIteration_(); - -// GaussianFactorGraph::shared_ptr GFG = graph.linearize(values); -// VectorValues delta = GFG->optimize(); - - // result.print("results of 3 camera, 3 landmark optimization \n"); - if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); - EXPECT(assert_equal(pose3,result.at(x3))); - if(isDebugTest) tictoc_print_(); -} - -/* *************************************************************************/ -TEST( SmartProjectionHessianFactor, 3poses_iterative_smart_projection_factor ){ - // cout << " ************************ SmartProjectionHessianFactor: 3 cams + 3 landmarks **********************" << endl; - - std::vector views; - views.push_back(x1); - views.push_back(x2); - views.push_back(x3); - - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); - SimpleCamera cam1(pose1, *K); - - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); - SimpleCamera cam2(pose2, *K); - - // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); - SimpleCamera cam3(pose3, *K); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); - Point3 landmark3(3, 0, 3.0); - - vector measurements_cam1, measurements_cam2, measurements_cam3; - - // 1. Project three landmarks into three cameras and triangulate - Point2 cam1_uv1 = cam1.project(landmark1); - Point2 cam2_uv1 = cam2.project(landmark1); - Point2 cam3_uv1 = cam3.project(landmark1); - measurements_cam1.push_back(cam1_uv1); - measurements_cam1.push_back(cam2_uv1); - measurements_cam1.push_back(cam3_uv1); - - Point2 cam1_uv2 = cam1.project(landmark2); - Point2 cam2_uv2 = cam2.project(landmark2); - Point2 cam3_uv2 = cam3.project(landmark2); - measurements_cam2.push_back(cam1_uv2); - measurements_cam2.push_back(cam2_uv2); - measurements_cam2.push_back(cam3_uv2); - - Point2 cam1_uv3 = cam1.project(landmark3); - Point2 cam2_uv3 = cam2.project(landmark3); - Point2 cam3_uv3 = cam3.project(landmark3); - measurements_cam3.push_back(cam1_uv3); - measurements_cam3.push_back(cam2_uv3); - measurements_cam3.push_back(cam3_uv3); - - SmartFactor::shared_ptr smartFactor1(new SmartFactor()); - smartFactor1->add(cam1_uv1, views[0], model, K); - smartFactor1->add(cam2_uv1, views[1], model, K); - smartFactor1->add(cam3_uv1, views[2], model, K); - - SmartFactor::shared_ptr smartFactor2(new SmartFactor()); - smartFactor2->add(cam1_uv2, views[0], model, K); - smartFactor2->add(cam2_uv2, views[1], model, K); - smartFactor2->add(cam3_uv2, views[2], model, K); - - SmartFactor::shared_ptr smartFactor3(new SmartFactor()); - smartFactor3->add(cam1_uv3, views[0], model, K); - smartFactor3->add(cam2_uv3, views[1], model, K); - smartFactor3->add(cam3_uv3, views[2], model, K); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PriorFactor(x2, pose2, noisePrior)); - - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise - Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - // initialize third pose with some noise, we expect it to move back to original pose3 - values.insert(x3, pose3*noise_pose); - if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); - - LevenbergMarquardtParams params; - if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; - if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; - - Values result; - gttic_(SmartProjectionHessianFactor); - LevenbergMarquardtOptimizer optimizer(graph, values, params); - result = optimizer.optimize(); - gttoc_(SmartProjectionHessianFactor); - tictoc_finishedIteration_(); - - // result.print("results of 3 camera, 3 landmark optimization \n"); - if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); - EXPECT(assert_equal(pose3,result.at(x3))); - if(isDebugTest) tictoc_print_(); -} - -/* *************************************************************************/ -TEST( SmartProjectionHessianFactor, 3poses_projection_factor ){ - // cout << " ************************ Normal ProjectionFactor: 3 cams + 3 landmarks **********************" << endl; - - std::vector views; - views.push_back(x1); - views.push_back(x2); - views.push_back(x3); - - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); - SimpleCamera cam1(pose1, *K2); - - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); - SimpleCamera cam2(pose2, *K2); - - // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); - SimpleCamera cam3(pose3, *K2); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); - Point3 landmark3(3, 0, 3.0); - - typedef GenericProjectionFactor ProjectionFactor; - NonlinearFactorGraph graph; - - // 1. Project three landmarks into three cameras and triangulate - graph.push_back(ProjectionFactor(cam1.project(landmark1), model, x1, L(1), K2)); - graph.push_back(ProjectionFactor(cam2.project(landmark1), model, x2, L(1), K2)); - graph.push_back(ProjectionFactor(cam3.project(landmark1), model, x3, L(1), K2)); - - graph.push_back(ProjectionFactor(cam1.project(landmark2), model, x1, L(2), K2)); - graph.push_back(ProjectionFactor(cam2.project(landmark2), model, x2, L(2), K2)); - graph.push_back(ProjectionFactor(cam3.project(landmark2), model, x3, L(2), K2)); - - graph.push_back(ProjectionFactor(cam1.project(landmark3), model, x1, L(3), K2)); - graph.push_back(ProjectionFactor(cam2.project(landmark3), model, x2, L(3), K2)); - graph.push_back(ProjectionFactor(cam3.project(landmark3), model, x3, L(3), K2)); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PriorFactor(x2, pose2, noisePrior)); - - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); - Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - values.insert(x3, pose3* noise_pose); - values.insert(L(1), landmark1); - values.insert(L(2), landmark2); - values.insert(L(3), landmark3); - if(isDebugTest) values.at(x3).print("Pose3 before optimization: "); - - LevenbergMarquardtParams params; - if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; - if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; - LevenbergMarquardtOptimizer optimizer(graph, values, params); - Values result = optimizer.optimize(); - - if(isDebugTest) result.at(x3).print("Pose3 after optimization: "); - EXPECT(assert_equal(pose3,result.at(x3))); -} - -/* *************************************************************************/ -TEST( SmartProjectionHessianFactor, CheckHessian){ - - std::vector views; - views.push_back(x1); - views.push_back(x2); - views.push_back(x3); - - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); - SimpleCamera cam1(pose1, *K); - - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); - SimpleCamera cam2(pose2, *K); - - // create third camera 1 meter above the first camera - Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); - SimpleCamera cam3(pose3, *K); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); - Point3 landmark3(3, 0, 3.0); - - vector measurements_cam1, measurements_cam2, measurements_cam3; - - // 1. Project three landmarks into three cameras and triangulate - Point2 cam1_uv1 = cam1.project(landmark1); - Point2 cam2_uv1 = cam2.project(landmark1); - Point2 cam3_uv1 = cam3.project(landmark1); - measurements_cam1.push_back(cam1_uv1); - measurements_cam1.push_back(cam2_uv1); - measurements_cam1.push_back(cam3_uv1); - - Point2 cam1_uv2 = cam1.project(landmark2); - Point2 cam2_uv2 = cam2.project(landmark2); - Point2 cam3_uv2 = cam3.project(landmark2); - measurements_cam2.push_back(cam1_uv2); - measurements_cam2.push_back(cam2_uv2); - measurements_cam2.push_back(cam3_uv2); - - Point2 cam1_uv3 = cam1.project(landmark3); - Point2 cam2_uv3 = cam2.project(landmark3); - Point2 cam3_uv3 = cam3.project(landmark3); - measurements_cam3.push_back(cam1_uv3); - measurements_cam3.push_back(cam2_uv3); - measurements_cam3.push_back(cam3_uv3); - - double rankTol = 10; - - SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol)); - smartFactor1->add(measurements_cam1, views, model, K); - - SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol)); - smartFactor2->add(measurements_cam2, views, model, K); - - SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol)); - smartFactor3->add(measurements_cam3, views, model, K); - - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(smartFactor3); - - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise - Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - // initialize third pose with some noise, we expect it to move back to original pose3 - values.insert(x3, pose3*noise_pose); - if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); - - boost::shared_ptr hessianFactor1 = smartFactor1->linearize(values); - boost::shared_ptr hessianFactor2 = smartFactor2->linearize(values); - boost::shared_ptr hessianFactor3 = smartFactor3->linearize(values); - - Matrix CumulativeInformation = hessianFactor1->information() + hessianFactor2->information() + hessianFactor3->information(); - - boost::shared_ptr GaussianGraph = graph.linearize(values); - Matrix GraphInformation = GaussianGraph->hessian().first; - - // Check Hessian - EXPECT(assert_equal(GraphInformation, CumulativeInformation, 1e-8)); - - Matrix AugInformationMatrix = hessianFactor1->augmentedInformation() + - hessianFactor2->augmentedInformation() + hessianFactor3->augmentedInformation(); - - // Check Information vector - cout << AugInformationMatrix.size() << endl; - Vector InfoVector = AugInformationMatrix.block(0,18,18,1); // 18x18 Hessian + information vector - - // Check Hessian - EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-8)); -} - -/* *************************************************************************/ -TEST( SmartProjectionHessianFactor, 3poses_2land_rotation_only_smart_projection_factor ){ - // cout << " ************************ SmartProjectionHessianFactor: 3 cams + 2 landmarks: Rotation Only**********************" << endl; - - std::vector views; - views.push_back(x1); - views.push_back(x2); - views.push_back(x3); - - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); - SimpleCamera cam1(pose1, *K2); - - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); - SimpleCamera cam2(pose2, *K2); - - // create third camera 1 meter above the first camera - Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); - SimpleCamera cam3(pose3, *K2); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); - - vector measurements_cam1, measurements_cam2, measurements_cam3; - - // 1. Project three landmarks into three cameras and triangulate - Point2 cam1_uv1 = cam1.project(landmark1); - Point2 cam2_uv1 = cam2.project(landmark1); - Point2 cam3_uv1 = cam3.project(landmark1); - measurements_cam1.push_back(cam1_uv1); - measurements_cam1.push_back(cam2_uv1); - measurements_cam1.push_back(cam3_uv1); - - Point2 cam1_uv2 = cam1.project(landmark2); - Point2 cam2_uv2 = cam2.project(landmark2); - Point2 cam3_uv2 = cam3.project(landmark2); - measurements_cam2.push_back(cam1_uv2); - measurements_cam2.push_back(cam2_uv2); - measurements_cam2.push_back(cam3_uv2); - - double rankTol = 50; - - SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol)); - smartFactor1->add(measurements_cam1, views, model, K2); - - SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol)); - smartFactor2->add(measurements_cam2, views, model, K2); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10); - Point3 positionPrior = gtsam::Point3(0,0,1); - - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); - graph.push_back(PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); - - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.1,0.1,0.1)); // smaller noise - Values values; - values.insert(x1, pose1); - values.insert(x2, pose2*noise_pose); - // initialize third pose with some noise, we expect it to move back to original pose3 - values.insert(x3, pose3*noise_pose*noise_pose); - if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); - - LevenbergMarquardtParams params; - if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYDELTA; - if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; - - Values result; - gttic_(SmartProjectionHessianFactor); - LevenbergMarquardtOptimizer optimizer(graph, values, params); - result = optimizer.optimize(); - gttoc_(SmartProjectionHessianFactor); - tictoc_finishedIteration_(); - - // result.print("results of 3 camera, 3 landmark optimization \n"); - if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); - EXPECT(assert_equal(pose3,result.at(x3))); - if(isDebugTest) tictoc_print_(); -} - -/* *************************************************************************/ -TEST( SmartProjectionHessianFactor, 3poses_rotation_only_smart_projection_factor ){ - // cout << " ************************ SmartProjectionHessianFactor: 3 cams + 3 landmarks: Rotation Only**********************" << endl; - - std::vector views; - views.push_back(x1); - views.push_back(x2); - views.push_back(x3); - - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); - SimpleCamera cam1(pose1, *K); - - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); - SimpleCamera cam2(pose2, *K); - - // create third camera 1 meter above the first camera - Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); - SimpleCamera cam3(pose3, *K); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); - Point3 landmark3(3, 0, 3.0); - - vector measurements_cam1, measurements_cam2, measurements_cam3; - - // 1. Project three landmarks into three cameras and triangulate - Point2 cam1_uv1 = cam1.project(landmark1); - Point2 cam2_uv1 = cam2.project(landmark1); - Point2 cam3_uv1 = cam3.project(landmark1); - measurements_cam1.push_back(cam1_uv1); - measurements_cam1.push_back(cam2_uv1); - measurements_cam1.push_back(cam3_uv1); - - Point2 cam1_uv2 = cam1.project(landmark2); - Point2 cam2_uv2 = cam2.project(landmark2); - Point2 cam3_uv2 = cam3.project(landmark2); - measurements_cam2.push_back(cam1_uv2); - measurements_cam2.push_back(cam2_uv2); - measurements_cam2.push_back(cam3_uv2); - - Point2 cam1_uv3 = cam1.project(landmark3); - Point2 cam2_uv3 = cam2.project(landmark3); - Point2 cam3_uv3 = cam3.project(landmark3); - measurements_cam3.push_back(cam1_uv3); - measurements_cam3.push_back(cam2_uv3); - measurements_cam3.push_back(cam3_uv3); - - double rankTol = 10; - - SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol)); - smartFactor1->add(measurements_cam1, views, model, K); - - SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol)); - smartFactor2->add(measurements_cam2, views, model, K); - - SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol)); - smartFactor3->add(measurements_cam3, views, model, K); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10); - Point3 positionPrior = gtsam::Point3(0,0,1); - - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); - graph.push_back(PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); - - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise - Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - // initialize third pose with some noise, we expect it to move back to original pose3 - values.insert(x3, pose3*noise_pose); - if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); - - LevenbergMarquardtParams params; - if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYDELTA; - if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; - - Values result; - gttic_(SmartProjectionHessianFactor); - LevenbergMarquardtOptimizer optimizer(graph, values, params); - result = optimizer.optimize(); - gttoc_(SmartProjectionHessianFactor); - tictoc_finishedIteration_(); - - // result.print("results of 3 camera, 3 landmark optimization \n"); - if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); - EXPECT(assert_equal(pose3,result.at(x3))); - if(isDebugTest) tictoc_print_(); -} - -/* *************************************************************************/ -TEST( SmartProjectionHessianFactor, Hessian ){ - // cout << " ************************ SmartProjectionHessianFactor: Hessian **********************" << endl; - - std::vector views; - views.push_back(x1); - views.push_back(x2); - - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); - SimpleCamera cam1(pose1, *K2); - - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); - SimpleCamera cam2(pose2, *K2); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - - // 1. Project three landmarks into three cameras and triangulate - Point2 cam1_uv1 = cam1.project(landmark1); - Point2 cam2_uv1 = cam2.project(landmark1); - vector measurements_cam1; - measurements_cam1.push_back(cam1_uv1); - measurements_cam1.push_back(cam2_uv1); - - SmartFactor::shared_ptr smartFactor1(new SmartFactor()); - smartFactor1->add(measurements_cam1,views, model, K2); - - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); - Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - - boost::shared_ptr hessianFactor = smartFactor1->linearize(values); - if(isDebugTest) hessianFactor->print("Hessian factor \n"); - - // compute triangulation from linearization point - // compute reprojection errors (sum squared) - // compare with hessianFactor.info(): the bottom right element is the squared sum of the reprojection errors (normalized by the covariance) - // check that it is correctly scaled when using noiseProjection = [1/4 0; 0 1/4] -} - - -/* *************************************************************************/ -TEST( SmartProjectionHessianFactor, HessianWithRotation ){ - // cout << " ************************ SmartProjectionHessianFactor: rotated Hessian **********************" << endl; - - std::vector views; - views.push_back(x1); - views.push_back(x2); - views.push_back(x3); - - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); - SimpleCamera cam1(pose1, *K); - - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); - SimpleCamera cam2(pose2, *K); - - // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); - SimpleCamera cam3(pose3, *K); - - Point3 landmark1(5, 0.5, 1.2); - - vector measurements_cam1, measurements_cam2, measurements_cam3; - - // 1. Project three landmarks into three cameras and triangulate - Point2 cam1_uv1 = cam1.project(landmark1); - Point2 cam2_uv1 = cam2.project(landmark1); - Point2 cam3_uv1 = cam3.project(landmark1); - measurements_cam1.push_back(cam1_uv1); - measurements_cam1.push_back(cam2_uv1); - measurements_cam1.push_back(cam3_uv1); - - SmartFactor::shared_ptr smartFactorInstance(new SmartFactor()); - smartFactorInstance->add(cam1_uv1, views[0], model, K); - smartFactorInstance->add(cam2_uv1, views[1], model, K); - smartFactorInstance->add(cam3_uv1, views[2], model, K); - - Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - values.insert(x3, pose3); - - boost::shared_ptr hessianFactor = smartFactorInstance->linearize(values); - // hessianFactor->print("Hessian factor \n"); - - Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,0)); - - Values rotValues; - rotValues.insert(x1, poseDrift.compose(pose1)); - rotValues.insert(x2, poseDrift.compose(pose2)); - rotValues.insert(x3, poseDrift.compose(pose3)); - - boost::shared_ptr hessianFactorRot = smartFactorInstance->linearize(rotValues); - // hessianFactorRot->print("Hessian factor \n"); - - // Hessian is invariant to rotations in the nondegenerate case - EXPECT(assert_equal(hessianFactor->information(), hessianFactorRot->information(), 1e-8) ); - - Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI/2, -M_PI/3, -M_PI/2), gtsam::Point3(10,-4,5)); - - Values tranValues; - tranValues.insert(x1, poseDrift2.compose(pose1)); - tranValues.insert(x2, poseDrift2.compose(pose2)); - tranValues.insert(x3, poseDrift2.compose(pose3)); - - boost::shared_ptr hessianFactorRotTran = smartFactorInstance->linearize(tranValues); - - // Hessian is invariant to rotations and translations in the nondegenerate case - EXPECT(assert_equal(hessianFactor->information(), hessianFactorRotTran->information(), 1e-8) ); -} - -/* *************************************************************************/ -TEST( SmartProjectionHessianFactor, HessianWithRotationDegenerate ){ - // cout << " ************************ SmartProjectionHessianFactor: rotated Hessian (degenerate) **********************" << endl; - - std::vector views; - views.push_back(x1); - views.push_back(x2); - views.push_back(x3); - - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); - SimpleCamera cam1(pose1, *K2); - - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(0,0,0)); - SimpleCamera cam2(pose2, *K2); - - // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,0,0)); - SimpleCamera cam3(pose3, *K2); - - Point3 landmark1(5, 0.5, 1.2); - - vector measurements_cam1, measurements_cam2, measurements_cam3; - - // 1. Project three landmarks into three cameras and triangulate - Point2 cam1_uv1 = cam1.project(landmark1); - Point2 cam2_uv1 = cam2.project(landmark1); - Point2 cam3_uv1 = cam3.project(landmark1); - measurements_cam1.push_back(cam1_uv1); - measurements_cam1.push_back(cam2_uv1); - measurements_cam1.push_back(cam3_uv1); - - SmartFactor::shared_ptr smartFactor(new SmartFactor()); - smartFactor->add(cam1_uv1, views[0], model, K2); - smartFactor->add(cam2_uv1, views[1], model, K2); - smartFactor->add(cam3_uv1, views[2], model, K2); - - Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - values.insert(x3, pose3); - - boost::shared_ptr hessianFactor = smartFactor->linearize(values); - if(isDebugTest) hessianFactor->print("Hessian factor \n"); - - Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,0)); - - Values rotValues; - rotValues.insert(x1, poseDrift.compose(pose1)); - rotValues.insert(x2, poseDrift.compose(pose2)); - rotValues.insert(x3, poseDrift.compose(pose3)); - - boost::shared_ptr hessianFactorRot = smartFactor->linearize(rotValues); - if(isDebugTest) hessianFactorRot->print("Hessian factor \n"); - - // Hessian is invariant to rotations in the nondegenerate case - EXPECT(assert_equal(hessianFactor->information(), hessianFactorRot->information(), 1e-8) ); - - Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI/2, -M_PI/3, -M_PI/2), gtsam::Point3(10,-4,5)); - - Values tranValues; - tranValues.insert(x1, poseDrift2.compose(pose1)); - tranValues.insert(x2, poseDrift2.compose(pose2)); - tranValues.insert(x3, poseDrift2.compose(pose3)); - - boost::shared_ptr hessianFactorRotTran = smartFactor->linearize(tranValues); - - // Hessian is invariant to rotations and translations in the nondegenerate case - EXPECT(assert_equal(hessianFactor->information(), hessianFactorRotTran->information(), 1e-8) ); -} - -/* ************************************************************************* */ -TEST( SmartProjectionHessianFactor, ConstructorWithCal3Bundler) { - SmartProjectionHessianFactor factor1(rankTol, linThreshold); - boost::shared_ptr Kbundler(new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); - factor1.add(measurement1, poseKey1, model, Kbundler); -} - -/* *************************************************************************/ -TEST( SmartProjectionHessianFactor, Cal3Bundler ){ - // cout << " ************************ SmartProjectionHessianFactor: Cal3Bundler **********************" << endl; - - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); - PinholeCamera cam1(pose1, *Kbundler); - - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); - PinholeCamera cam2(pose2, *Kbundler); - - // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); - PinholeCamera cam3(pose3, *Kbundler); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); - Point3 landmark3(3, 0, 3.0); - - vector measurements_cam1, measurements_cam2, measurements_cam3; - - // 1. Project three landmarks into three cameras and triangulate - Point2 cam1_uv1 = cam1.project(landmark1); - Point2 cam2_uv1 = cam2.project(landmark1); - Point2 cam3_uv1 = cam3.project(landmark1); - measurements_cam1.push_back(cam1_uv1); - measurements_cam1.push_back(cam2_uv1); - measurements_cam1.push_back(cam3_uv1); - - Point2 cam1_uv2 = cam1.project(landmark2); - Point2 cam2_uv2 = cam2.project(landmark2); - Point2 cam3_uv2 = cam3.project(landmark2); - measurements_cam2.push_back(cam1_uv2); - measurements_cam2.push_back(cam2_uv2); - measurements_cam2.push_back(cam3_uv2); - - Point2 cam1_uv3 = cam1.project(landmark3); - Point2 cam2_uv3 = cam2.project(landmark3); - Point2 cam3_uv3 = cam3.project(landmark3); - measurements_cam3.push_back(cam1_uv3); - measurements_cam3.push_back(cam2_uv3); - measurements_cam3.push_back(cam3_uv3); - - std::vector views; - views.push_back(x1); - views.push_back(x2); - views.push_back(x3); - - SmartProjectionHessianFactor::shared_ptr smartFactor1(new SmartFactorBundler()); - smartFactor1->add(measurements_cam1, views, model, Kbundler); - - SmartProjectionHessianFactor::shared_ptr smartFactor2(new SmartFactorBundler()); - smartFactor2->add(measurements_cam2, views, model, Kbundler); - - SmartProjectionHessianFactor::shared_ptr smartFactor3(new SmartFactorBundler()); - smartFactor3->add(measurements_cam3, views, model, Kbundler); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PriorFactor(x2, pose2, noisePrior)); - - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise - Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - // initialize third pose with some noise, we expect it to move back to original pose3 - values.insert(x3, pose3*noise_pose); - if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); - - LevenbergMarquardtParams params; - if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; - if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; - - Values result; - gttic_(SmartProjectionHessianFactor); - LevenbergMarquardtOptimizer optimizer(graph, values, params); - result = optimizer.optimize(); - gttoc_(SmartProjectionHessianFactor); - tictoc_finishedIteration_(); - - // result.print("results of 3 camera, 3 landmark optimization \n"); - if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); - EXPECT(assert_equal(pose3,result.at(x3), 1e-6)); - if(isDebugTest) tictoc_print_(); - } - -/* *************************************************************************/ -TEST( SmartProjectionHessianFactor, Cal3BundlerRotationOnly ){ - - std::vector views; - views.push_back(x1); - views.push_back(x2); - views.push_back(x3); - - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); - PinholeCamera cam1(pose1, *Kbundler); - - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); - PinholeCamera cam2(pose2, *Kbundler); - - // create third camera 1 meter above the first camera - Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); - PinholeCamera cam3(pose3, *Kbundler); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); - Point3 landmark3(3, 0, 3.0); - - vector measurements_cam1, measurements_cam2, measurements_cam3; - - // 1. Project three landmarks into three cameras and triangulate - Point2 cam1_uv1 = cam1.project(landmark1); - Point2 cam2_uv1 = cam2.project(landmark1); - Point2 cam3_uv1 = cam3.project(landmark1); - measurements_cam1.push_back(cam1_uv1); - measurements_cam1.push_back(cam2_uv1); - measurements_cam1.push_back(cam3_uv1); - - Point2 cam1_uv2 = cam1.project(landmark2); - Point2 cam2_uv2 = cam2.project(landmark2); - Point2 cam3_uv2 = cam3.project(landmark2); - measurements_cam2.push_back(cam1_uv2); - measurements_cam2.push_back(cam2_uv2); - measurements_cam2.push_back(cam3_uv2); - - Point2 cam1_uv3 = cam1.project(landmark3); - Point2 cam2_uv3 = cam2.project(landmark3); - Point2 cam3_uv3 = cam3.project(landmark3); - measurements_cam3.push_back(cam1_uv3); - measurements_cam3.push_back(cam2_uv3); - measurements_cam3.push_back(cam3_uv3); - - double rankTol = 10; - - SmartFactorBundler::shared_ptr smartFactor1(new SmartFactorBundler(rankTol)); - smartFactor1->add(measurements_cam1, views, model, Kbundler); - - SmartFactorBundler::shared_ptr smartFactor2(new SmartFactorBundler(rankTol)); - smartFactor2->add(measurements_cam2, views, model, Kbundler); - - SmartFactorBundler::shared_ptr smartFactor3(new SmartFactorBundler(rankTol)); - smartFactor3->add(measurements_cam3, views, model, Kbundler); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10); - Point3 positionPrior = gtsam::Point3(0,0,1); - - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); - graph.push_back(PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); - - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise - Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - // initialize third pose with some noise, we expect it to move back to original pose3 - values.insert(x3, pose3*noise_pose); - if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); - - LevenbergMarquardtParams params; - if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYDELTA; - if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; - - Values result; - gttic_(SmartProjectionHessianFactor); - LevenbergMarquardtOptimizer optimizer(graph, values, params); - result = optimizer.optimize(); - gttoc_(SmartProjectionHessianFactor); - tictoc_finishedIteration_(); - - // result.print("results of 3 camera, 3 landmark optimization \n"); - if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); - EXPECT(assert_equal(pose3,result.at(x3))); - if(isDebugTest) tictoc_print_(); -} - - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ - -