diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index 899e6227c..0df85d3d2 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -220,17 +220,15 @@ public: static SymmetricBlockMatrix SchurComplement(const FBlocks& Fblocks, const Matrix& E, const Vector& b, const double lambda = 0.0, bool diagonalDamping = false) { - SymmetricBlockMatrix augmentedHessian; if (E.cols() == 2) { Matrix2 P; ComputePointCovariance(P, E, lambda, diagonalDamping); - augmentedHessian = SchurComplement(Fblocks, E, P, b); + return SchurComplement(Fblocks, E, P, b); } else { Matrix3 P; ComputePointCovariance(P, E, lambda, diagonalDamping); - augmentedHessian = SchurComplement(Fblocks, E, P, b); + return SchurComplement(Fblocks, E, P, b); } - return augmentedHessian; } /** diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 01a8fcf8d..eca7f531b 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -50,6 +50,12 @@ private: typedef SmartFactorBase This; typedef typename CAMERA::Measurement Z; +public: + + static const int Dim = traits::dimension; ///< Camera dimension + static const int ZDim = traits::dimension; ///< Measurement dimension + typedef Eigen::Matrix MatrixZD; // F blocks (derivatives wrpt camera) + protected: /** * As of Feb 22, 2015, the noise model is the same for all measurements and @@ -70,21 +76,11 @@ protected: const boost::optional body_P_sensor_; ///< Pose of the camera in the body frame /// @} - static const int Dim = traits::dimension; ///< Camera dimension - static const int ZDim = traits::dimension; ///< Measurement dimension - - // Definitions for block matrices used internally - typedef Eigen::Matrix MatrixD2; // F' - typedef Eigen::Matrix MatrixDD; // camera hessian block - typedef Eigen::Matrix Matrix23; - typedef Eigen::Matrix VectorD; - typedef Eigen::Matrix Matrix2; + // Cache for Fblocks, to avoid a malloc ever time we re-linearize + mutable std::vector Fblocks; public: - // Definitions for blocks of F, externally visible - typedef Eigen::Matrix MatrixZD; // F - EIGEN_MAKE_ALIGNED_OPERATOR_NEW /// shorthand for a smart pointer to a factor @@ -98,8 +94,9 @@ public: /// Constructor SmartFactorBase(const SharedNoiseModel& sharedNoiseModel, - boost::optional body_P_sensor = boost::none) : - body_P_sensor_(body_P_sensor){ + boost::optional body_P_sensor = boost::none, + size_t expectedNumberCameras = 10) + : body_P_sensor_(body_P_sensor), Fblocks(expectedNumberCameras) { if (!sharedNoiseModel) throw std::runtime_error("SmartFactorBase: sharedNoiseModel is required"); @@ -283,14 +280,12 @@ public: const Cameras& cameras, const Point3& point, const double lambda = 0.0, bool diagonalDamping = false) const { - std::vector Fblocks; Matrix E; Vector b; computeJacobians(Fblocks, E, b, cameras, point); // build augmented hessian - SymmetricBlockMatrix augmentedHessian = Cameras::SchurComplement(Fblocks, E, - b); + SymmetricBlockMatrix augmentedHessian = Cameras::SchurComplement(Fblocks, E, b); return boost::make_shared >(keys_, augmentedHessian); @@ -307,10 +302,8 @@ public: const FastVector allKeys) const { Matrix E; Vector b; - std::vector Fblocks; computeJacobians(Fblocks, E, b, cameras, point); - Cameras::UpdateSchurComplement(Fblocks, E, b, allKeys, keys_, - augmentedHessian); + Cameras::UpdateSchurComplement(Fblocks, E, b, allKeys, keys_, augmentedHessian); } /// Whiten the Jacobians computed by computeJacobians using noiseModel_ diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 8636138f2..14192fcaa 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -658,10 +658,11 @@ bool readBundler(const string& filename, SfM_data &data) { Pose3 pose = openGL2gtsam(R, tx, ty, tz); - data.cameras.push_back(SfM_Camera(pose, K)); + data.cameras.emplace_back(pose, K); } // Get the information for the 3D points + data.tracks.reserve(nrPoints); for (size_t j = 0; j < nrPoints; j++) { SfM_Track track; @@ -681,12 +682,14 @@ bool readBundler(const string& filename, SfM_data &data) { size_t nvisible = 0; is >> nvisible; + track.measurements.reserve(nvisible); + track.siftIndices.reserve(nvisible); for (size_t k = 0; k < nvisible; k++) { size_t cam_idx = 0, point_idx = 0; float u, v; is >> cam_idx >> point_idx >> u >> v; - track.measurements.push_back(make_pair(cam_idx, Point2(u, -v))); - track.siftIndices.push_back(make_pair(cam_idx, point_idx)); + track.measurements.emplace_back(cam_idx, Point2(u, -v)); + track.siftIndices.emplace_back(cam_idx, point_idx); } data.tracks.push_back(track); @@ -716,7 +719,7 @@ bool readBAL(const string& filename, SfM_data &data) { size_t i = 0, j = 0; float u, v; is >> i >> j >> u >> v; - data.tracks[j].measurements.push_back(make_pair(i, Point2(u, -v))); + data.tracks[j].measurements.emplace_back(i, Point2(u, -v)); } // Get the information for the camera poses @@ -737,7 +740,7 @@ bool readBAL(const string& filename, SfM_data &data) { is >> f >> k1 >> k2; Cal3Bundler K(f, k1, k2); - data.cameras.push_back(SfM_Camera(pose, K)); + data.cameras.emplace_back(pose, K); } // Get the information for the 3D points diff --git a/timing/timeSFMBAL.h b/timing/timeSFMBAL.h index cd859326c..1642af7e8 100644 --- a/timing/timeSFMBAL.h +++ b/timing/timeSFMBAL.h @@ -54,7 +54,7 @@ SfM_data preamble(int argc, char* argv[]) { filename = argv[argc - 1]; else filename = findExampleDataFile("dubrovnik-16-22106-pre"); - bool success = readBAL(argv[argc - 1], db); + bool success = readBAL(filename, db); if (!success) throw runtime_error("Could not access file!"); return db; } @@ -67,7 +67,7 @@ int optimize(const SfM_data& db, const NonlinearFactorGraph& graph, // Set parameters to be similar to ceres LevenbergMarquardtParams params; LevenbergMarquardtParams::SetCeresDefaults(¶ms); - params.setVerbosityLM("SUMMARY"); +// params.setVerbosityLM("SUMMARY"); if (gUseSchur) { // Create Schur-complement ordering @@ -81,8 +81,11 @@ int optimize(const SfM_data& db, const NonlinearFactorGraph& graph, } // Optimize - LevenbergMarquardtOptimizer lm(graph, initial, params); - Values result = lm.optimize(); + { + gttic_(optimize); + LevenbergMarquardtOptimizer lm(graph, initial, params); + Values result = lm.optimize(); + } tictoc_finishedIteration_(); tictoc_print_(); diff --git a/timing/timeSFMBALnavTcam.cpp b/timing/timeSFMBALnavTcam.cpp index e370a5a67..0d0eb4f65 100644 --- a/timing/timeSFMBALnavTcam.cpp +++ b/timing/timeSFMBALnavTcam.cpp @@ -35,12 +35,12 @@ int main(int argc, char* argv[]) { // Build graph using conventional GeneralSFMFactor NonlinearFactorGraph graph; for (size_t j = 0; j < db.number_tracks(); j++) { + Point3_ nav_point_(P(j)); BOOST_FOREACH (const SfM_Measurement& m, db.tracks[j].measurements) { size_t i = m.first; Point2 z = m.second; Pose3_ navTcam_(C(i)); Cal3Bundler_ calibration_(K(i)); - Point3_ nav_point_(P(j)); graph.addExpressionFactor( gNoiseModel, z, uncalibrate(calibration_, diff --git a/timing/timeSFMBALsmart.cpp b/timing/timeSFMBALsmart.cpp new file mode 100644 index 000000000..708c25695 --- /dev/null +++ b/timing/timeSFMBALsmart.cpp @@ -0,0 +1,55 @@ +/* ---------------------------------------------------------------------------- + + * 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 timeSFMBALsmart.cpp + * @brief time SFM with BAL file, SmartProjectionFactor + * @author Frank Dellaert + * @date Feb 26, 2016 + */ + +#include "timeSFMBAL.h" + +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +typedef PinholeCamera Camera; +typedef SmartProjectionFactor SfmFactor; + +int main(int argc, char* argv[]) { + // parse options and read BAL file + SfM_data db = preamble(argc, argv); + + // Add smart factors to graph + NonlinearFactorGraph graph; + for (size_t j = 0; j < db.number_tracks(); j++) { + auto smartFactor = boost::make_shared(gNoiseModel); + for (const SfM_Measurement& m : db.tracks[j].measurements) { + size_t i = m.first; + Point2 z = m.second; + smartFactor->add(z, C(i)); + } + graph.push_back(smartFactor); + } + + Values initial; + size_t i = 0; + gUseSchur = false; + for (const SfM_Camera& camera : db.cameras) + initial.insert(C(i++), camera); + + return optimize(db, graph, initial); +}