From 81c183199ace7c2eaf90380372ced021267101f1 Mon Sep 17 00:00:00 2001 From: Luca Carlone Date: Fri, 18 Oct 2013 23:14:34 +0000 Subject: [PATCH] cleaning and removing useless things --- ...pp => SmartProjectionFactorExampleBAL.cpp} | 11 +- ... => SmartProjectionFactorExampleKitti.cpp} | 0 .../SmartProjectionFactorExample_kitti.cpp | 351 --------- .../slam/GenericProjectionFactorsCreator.h | 10 +- gtsam_unstable/slam/SmartProjectionFactor.h | 691 ------------------ 5 files changed, 6 insertions(+), 1057 deletions(-) rename gtsam_unstable/examples/{SmartProjectionFactorTesting.cpp => SmartProjectionFactorExampleBAL.cpp} (99%) rename gtsam_unstable/examples/{SmartProjectionFactorExample_kitti_nonbatch.cpp => SmartProjectionFactorExampleKitti.cpp} (100%) delete mode 100644 gtsam_unstable/examples/SmartProjectionFactorExample_kitti.cpp delete mode 100644 gtsam_unstable/slam/SmartProjectionFactor.h diff --git a/gtsam_unstable/examples/SmartProjectionFactorTesting.cpp b/gtsam_unstable/examples/SmartProjectionFactorExampleBAL.cpp similarity index 99% rename from gtsam_unstable/examples/SmartProjectionFactorTesting.cpp rename to gtsam_unstable/examples/SmartProjectionFactorExampleBAL.cpp index 969a42217..d04dbf886 100644 --- a/gtsam_unstable/examples/SmartProjectionFactorTesting.cpp +++ b/gtsam_unstable/examples/SmartProjectionFactorExampleBAL.cpp @@ -16,8 +16,6 @@ * @author Luca Carlone */ -#ifdef DEVELOP - // Use a map to store landmark/smart factor pairs #include @@ -60,7 +58,7 @@ using namespace gtsam; using namespace boost::assign; namespace NM = gtsam::noiseModel; - #define USE_BUNDLER +// #define USE_BUNDLER using symbol_shorthand::X; using symbol_shorthand::L; @@ -445,10 +443,3 @@ int main(int argc, char** argv) { exit(0); } -#endif - -int main(){ - return 1; -} - - diff --git a/gtsam_unstable/examples/SmartProjectionFactorExample_kitti_nonbatch.cpp b/gtsam_unstable/examples/SmartProjectionFactorExampleKitti.cpp similarity index 100% rename from gtsam_unstable/examples/SmartProjectionFactorExample_kitti_nonbatch.cpp rename to gtsam_unstable/examples/SmartProjectionFactorExampleKitti.cpp diff --git a/gtsam_unstable/examples/SmartProjectionFactorExample_kitti.cpp b/gtsam_unstable/examples/SmartProjectionFactorExample_kitti.cpp deleted file mode 100644 index 7bf22e9d1..000000000 --- a/gtsam_unstable/examples/SmartProjectionFactorExample_kitti.cpp +++ /dev/null @@ -1,351 +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.cpp - * @brief Example usage of SmartProjectionFactor using real dataset - * @date August, 2013 - * @author Zsolt Kira - */ - -// Both relative poses and recovered trajectory poses will be stored as Pose2 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 -#include - -// We will use a non-liear solver to batch-inituialize from the first 150 frames -#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; - -//// 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)); - - // 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; -} - -// Loaded 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 -Cal3_S2::shared_ptr loadCalibration(const string& filename) { - string full_filename = filename; - ifstream fin; - fin.open(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; - fin.close(); - - Cal3_S2::shared_ptr K(new Cal3_S2(fx, fy, s, u, v)); - return K; -} - -void writeValues(string directory_, const Values& values){ - string filename = directory_ + "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(); - - } -} - -// main -int main(int argc, char** argv) { - - bool debug = false; - - // Set to true to use SmartProjectionFactor. Otherwise GenericProjectionFactor will be used - bool useSmartProjectionFactor = true; - std::cout << "PARAM SmartFactor: " << useSmartProjectionFactor << std::endl; - - // Minimum number of views of a landmark before it is added to the graph (SmartProjectionFactor case only) - unsigned int minimumNumViews = 1; - - string HOME = getenv("HOME"); - //string input_dir = HOME + "/data/kitti/loop_closures_merged/"; - string input_dir = HOME + "/data/KITTI_00_200/"; - - typedef SmartProjectionFactor SmartFactor; - typedef GenericProjectionFactor ProjectionFactor; - static SharedNoiseModel pixel_sigma(noiseModel::Unit::Create(2)); - static SharedNoiseModel prior_model(noiseModel::Diagonal::Sigmas(Vector_(6, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01))); - NonlinearFactorGraph graph; - - // Load calibration - //Cal3_S2::shared_ptr K(new Cal3_S2(718.856, 718.856, 0.0, 607.1928, 185.2157)); - boost::shared_ptr K = loadCalibration(input_dir+"calibration.txt"); - K->print("Calibration"); - - // Load values from VO camera poses output - gtsam::Values::shared_ptr loaded_values = loadPoseValues(input_dir+"camera_poses.txt"); - graph.push_back(Pose3Prior(X(0),loaded_values->at(X(0)), prior_model)); - graph.push_back(Pose3Prior(X(1),loaded_values->at(X(1)), prior_model)); - //graph.print("thegraph"); - - // 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); - } - - // read all measurements tracked by VO stereo - cout << "Loading stereo_factors.txt" << endl; - int count = 0; - Key currentLandmark = 0; - int numLandmarks = 0; - Key r, l; - double uL, uR, v, x, y, z; - std::list allViews; - std::vector views; - std::vector measurements; - Values values; - while (fin >> r >> l >> uL >> uR >> v >> x >> y >> z) { - if (debug) cout << "CurrentLandmark " << currentLandmark << " Landmark " << l << std::endl; - - if (useSmartProjectionFactor == false) { - if (loaded_values->exists(L(l)) == boost::none) { - Pose3 camera = loaded_values->at(X(r)); - Point3 worldPoint = camera.transform_from(Point3(x, y, z)); - loaded_values->insert(L(l), worldPoint); // add point; - } - - ProjectionFactor::shared_ptr projectionFactor(new ProjectionFactor(Point2(uL,v), pixel_sigma, X(r), L(l), K)); - graph.push_back(projectionFactor); - } - - if (currentLandmark != l && views.size() < minimumNumViews) { - // New landmark. Not enough views for previous landmark so move on. - if (debug) cout << "New landmark " << l << " with not enough view for previous one" << std::endl; - currentLandmark = l; - views.clear(); - measurements.clear(); - } else if (currentLandmark != l) { - // New landmark. Add previous landmark and associated views to new factor - if (debug) cout << "New landmark " << l << " with "<< views.size() << " views for previous landmark " << currentLandmark << std::endl; - - if (debug) cout << "Keys "; - BOOST_FOREACH(const Key& k, views) { - allViews.push_back(k); - if (debug) cout << k << " "; - } - if (debug) cout << endl; - - if (debug) { - cout << "Measurements "; - BOOST_FOREACH(const Point2& p, measurements) { - cout << p << " "; - } - cout << endl; - } - - if (useSmartProjectionFactor) { - SmartFactor::shared_ptr smartFactor(new SmartFactor(views, measurements, pixel_sigma, K)); - graph.push_back(smartFactor); - } - - numLandmarks++; - - currentLandmark = l; - views.clear(); - measurements.clear(); - } else { - // We have new view for current landmark, so add it to the list later - if (debug) cout << "New view for landmark " << l << " (" << views.size() << " total)" << std::endl; - } - - // Add view for new landmark - views += X(r); - measurements += Point2(uL,v); - - count++; - if(count==100000) { - cout << "Loading graph... " << graph.size() << endl; - count=0; - } - } - // Add last measurements - if (useSmartProjectionFactor) { - SmartFactor::shared_ptr smartFactor(new SmartFactor(views, measurements, pixel_sigma, K)); - graph.push_back(smartFactor); - } - - cout << "Graph size: " << graph.size() << endl; - - /* - - // If using only subset of graph, only read in values for keys that are used - - // Get all view in the graph and populate poses from VO output - // TODO: Handle loop closures properly - cout << "All Keys (" << allViews.size() << ")" << endl; - allViews.unique(); - cout << "All Keys (" << allViews.size() << ")" << endl; - - values = *loadPoseValues(input_dir+"camera_poses.txt", allViews); - BOOST_FOREACH(const Key& k, allViews) { - if (debug) cout << k << " "; - } - cout << endl; - */ - - cout << "Optimizing... " << endl; - - // Optimize! - LevenbergMarquardtParams params; - params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; - params.verbosity = NonlinearOptimizerParams::ERROR; - - params.lambdaInitial = 1; - params.lambdaFactor = 10; - params.maxIterations = 100; - params.relativeErrorTol = 1e-5; - params.absoluteErrorTol = 1.0; - params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; - params.verbosity = NonlinearOptimizerParams::ERROR; - params.linearSolverType = SuccessiveLinearizationParams::MULTIFRONTAL_CHOLESKY; - - - Values result; - for (int i = 0; i < 1; i++) { - std::cout << " OPTIMIZATION " << i << std::endl; - LevenbergMarquardtOptimizer optimizer(graph, *loaded_values, params); - gttic_(SmartProjectionFactorExample_kitti); - result = optimizer.optimize(); - gttoc_(SmartProjectionFactorExample_kitti); - tictoc_finishedIteration_(); - } - - cout << "===================================================" << endl; - loaded_values->print("before optimization "); - result.print("results of kitti optimization "); - tictoc_print_(); - cout << "===================================================" << endl; - writeValues("./", result); - - exit(0); -} diff --git a/gtsam_unstable/slam/GenericProjectionFactorsCreator.h b/gtsam_unstable/slam/GenericProjectionFactorsCreator.h index 7616a81da..6f9454393 100644 --- a/gtsam_unstable/slam/GenericProjectionFactorsCreator.h +++ b/gtsam_unstable/slam/GenericProjectionFactorsCreator.h @@ -127,8 +127,8 @@ namespace gtsam { } } - void update(NonlinearFactorGraph &graph, gtsam::Values::shared_ptr inputValues, gtsam::Values::shared_ptr outputValues, bool doTriangualatePoints = true) { - addTriangulatedLandmarks(graph, inputValues, outputValues, doTriangualatePoints); + void update(NonlinearFactorGraph &graph, gtsam::Values::shared_ptr inputValues, gtsam::Values::shared_ptr outputValues, bool doTriangulatePoints = true) { + addTriangulatedLandmarks(graph, inputValues, outputValues, doTriangulatePoints); updateOrdering(graph); } @@ -165,11 +165,11 @@ namespace gtsam { } void addTriangulatedLandmarks(NonlinearFactorGraph &graph, gtsam::Values::shared_ptr loadedValues, - gtsam::Values::shared_ptr graphValues, bool doTriangualatePoints) { + gtsam::Values::shared_ptr graphValues, bool doTriangulatePoints) { bool debug = false; - if(doTriangualatePoints) + if(doTriangulatePoints) std::cout << "Triangulating 3D points" << std::endl; else std::cout << "Reading initial guess for 3D points from file" << std::endl; @@ -222,7 +222,7 @@ namespace gtsam { } // Triangulate landmark based on set of poses and measurements - if (doTriangualatePoints){ + if (doTriangulatePoints){ // std::cout << "Triangulating points " << std::endl; try { point = triangulatePoint3(cameraPoses, measured, Ks, rankTolerance); diff --git a/gtsam_unstable/slam/SmartProjectionFactor.h b/gtsam_unstable/slam/SmartProjectionFactor.h deleted file mode 100644 index 18535f7ec..000000000 --- a/gtsam_unstable/slam/SmartProjectionFactor.h +++ /dev/null @@ -1,691 +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 - -namespace gtsam { - - // default threshold for selective relinearization - static double defaultLinThreshold = -1; // 1e-7; // 0.01 - // default threshold for retriangulation - static double defaultTriangThreshold = 1e-7; - // 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 SmartProjectionFactorState { - public: - - static int lastID; - int ID; - - SmartProjectionFactorState() { - 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 SmartProjectionFactorState::lastID = 0; - - /** - * The calibration is known here. - * @addtogroup SLAM - */ - template - class SmartProjectionFactor: public NonlinearFactor { - protected: - - // Keep a copy of measurement and calibration for I/O - std::vector measured_; ///< 2D measurement for each of the m views - 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 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 - 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 SmartProjectionFactor 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; - - /// Default constructor - SmartProjectionFactor() : retriangulationThreshold(defaultTriangThreshold), - rankTolerance(defaultRankTolerance), linearizationThreshold(defaultLinThreshold), - throwCheirality_(false), verboseCheirality_(false) {} - - /** - * 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) - */ - SmartProjectionFactor(std::vector poseKeys, // camera poses - const std::vector measured, // pixel measurements - const SharedNoiseModel& model, // noise model (same for all measurements) - const boost::shared_ptr& K, // calibration matrix (same for all measurements) - boost::optional body_P_sensor = boost::none, - SmartFactorStatePtr state = SmartFactorStatePtr(new SmartProjectionFactorState())) : - measured_(measured), noise_(model), K_(K), - retriangulationThreshold(defaultTriangThreshold), rankTolerance(defaultRankTolerance), - linearizationThreshold(defaultLinThreshold), body_P_sensor_(body_P_sensor), - state_(state), throwCheirality_(false), verboseCheirality_(false) { - keys_.assign(poseKeys.begin(), poseKeys.end()); - } - - /** - * 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) - */ - SmartProjectionFactor(std::vector poseKeys, // camera poses - const std::vector measured, // pixel measurements - const SharedNoiseModel& model, // noise model (same for all measurements) - const boost::shared_ptr& K, // calibration matrix (same for all measurements) - const double rankTol, - const double linThreshold = defaultLinThreshold, - boost::optional body_P_sensor = boost::none, - SmartFactorStatePtr state = SmartFactorStatePtr(new SmartProjectionFactorState())) : - measured_(measured), noise_(model), K_(K), - retriangulationThreshold(defaultTriangThreshold), rankTolerance(rankTol), - linearizationThreshold(linThreshold), body_P_sensor_(body_P_sensor), - state_(state), throwCheirality_(false), verboseCheirality_(false) { - keys_.assign(poseKeys.begin(), poseKeys.end()); - } - - /** - * Constructor with exception-handling flags - * TODO: Mark argument order standard (keys, measurement, parameters) - * @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 poseKeys is the set of indices corresponding to the cameras observing the same landmark - * @param K shared pointer to the constant calibration - * @param throwCheirality determines whether Cheirality exceptions are rethrown - * @param verboseCheirality determines whether exceptions are printed for Cheirality - * @param body_P_sensor is the transform from body to sensor frame (default identity) - */ - SmartProjectionFactor(std::vector poseKeys, - const std::vector measured, - const SharedNoiseModel& model, - const boost::shared_ptr& K, - const double rankTol, - const double linThreshold, - bool throwCheirality, bool verboseCheirality, - boost::optional body_P_sensor = boost::none, - SmartFactorStatePtr state = SmartFactorStatePtr(new SmartProjectionFactorState())) : - measured_(measured), noise_(model), K_(K), - retriangulationThreshold(defaultTriangThreshold), rankTolerance(rankTol), - linearizationThreshold(linThreshold), body_P_sensor_(body_P_sensor), - state_(state), throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) { - } - - /** - * Constructor with exception-handling flags - * @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 - */ - SmartProjectionFactor(const SharedNoiseModel& model, const boost::shared_ptr& K, - boost::optional body_P_sensor = boost::none, - SmartFactorStatePtr state = SmartFactorStatePtr(new SmartProjectionFactorState())) : - noise_(model), K_(K), retriangulationThreshold(defaultTriangThreshold), rankTolerance(defaultRankTolerance), - linearizationThreshold(defaultLinThreshold), body_P_sensor_(body_P_sensor), state_(state) { - } - - /** Virtual destructor */ - virtual ~SmartProjectionFactor() {} - - /** - * 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, const Key poseKey) { - measured_.push_back(measured); - keys_.push_back(poseKey); - } - - // 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 << "SmartProjectionFactor, z = "; - BOOST_FOREACH(const Point2& p, measured_) { - std::cout << "measurement, p = "<< p << std::endl; - } - 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_, 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(); - std::vector 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 { - state_->point = triangulatePoint3(cameraPoses, measured_, *K_, 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)); - } - - //otherwise redo linearization - if (blockwise){ - // ========================================================================================================== - std::cout << "Deprecated use of blockwise version. This is slower and no longer supported" << std::endl; - blockwise = false; - // std::vector Hx(numKeys); - // std::vector Hl(numKeys); - // std::vector b(numKeys); - // - // for(size_t i = 0; i < measured_.size(); i++) { - // Pose3 pose = cameraPoses.at(i); - // PinholeCamera camera(pose, *K_); - // b.at(i) = - ( camera.project(state_->point,Hx.at(i),Hl.at(i)) - measured_.at(i) ).vector(); - // noise_-> WhitenSystem(Hx.at(i), Hl.at(i), b.at(i)); - // f += b.at(i).squaredNorm(); - // } - // - // // Shur complement trick - // - // // Allocate m^2 matrix blocks - // std::vector< std::vector > Hxl(keys_.size(), std::vector( keys_.size())); - // - // // Allocate inv(Hl'Hl) - // Matrix3 C = zeros(3,3); - // for(size_t i1 = 0; i1 < keys_.size(); i1++) { - // C.noalias() += Hl.at(i1).transpose() * Hl.at(i1); - // } - // - // Matrix3 Cinv = C.inverse(); // this is very important: without eval, because of eigen aliasing the results will be incorrect - // - // // Calculate sub blocks - // for(size_t i1 = 0; i1 < keys_.size(); i1++) { - // for(size_t i2 = 0; i2 < keys_.size(); i2++) { - // // we only need the upper triangular entries - // Hxl[i1][i2].noalias() = Hx.at(i1).transpose() * Hl.at(i1) * Cinv * Hl.at(i2).transpose(); - // } - // } - // // Populate Gs and gs - // int GsCount = 0; - // for(size_t i1 = 0; i1 < numKeys; i1++) { - // gs.at(i1).noalias() = Hx.at(i1).transpose() * b.at(i1); - // - // for(size_t i2 = 0; i2 < numKeys; i2++) { - // gs.at(i1).noalias() -= Hxl[i1][i2] * b.at(i2); - // - // if (i2 == i1){ - // Gs.at(GsCount).noalias() = Hx.at(i1).transpose() * Hx.at(i1) - Hxl[i1][i2] * Hx.at(i2); - // GsCount++; - // } - // if (i2 > i1) { - // Gs.at(GsCount).noalias() = - Hxl[i1][i2] * Hx.at(i2); - // GsCount++; - // } - // } - // } - } - - 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_); - 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_-> 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_); - 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_-> 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(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_, 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 want to manage the exceptions with rotation-only factors - // 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){ - for(size_t i = 0; i < measured_.size(); i++) { - Pose3 pose = cameraPoses.at(i); - PinholeCamera camera(pose, *K_); - 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_->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_); - - try { - Point2 reprojectionError(camera.project(state_->point) - measured_.at(i)); - //std::cout << "Reprojection error: " << reprojectionError << std::endl; - overallError += 0.5 * noise_->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_; - } - - /** 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_); - ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); - ar & BOOST_SERIALIZATION_NVP(throwCheirality_); - ar & BOOST_SERIALIZATION_NVP(verboseCheirality_); - } - - }; - -} // \ namespace gtsam