Merged in feature/smartCache (pull request #237)

Feature/smartcache

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;
   }
 
   /**

gtsam/slam/SmartFactorBase.h

@@ -50,6 +50,12 @@ private:
   typedef SmartFactorBase<CAMERA> This;
   typedef typename CAMERA::Measurement Z;
 
+public:
+
+  static const int Dim = traits<CAMERA>::dimension; ///< Camera dimension
+  static const int ZDim = traits<Z>::dimension; ///< Measurement dimension
+  typedef Eigen::Matrix<double, ZDim, Dim> 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<Pose3> body_P_sensor_; ///< Pose of the camera in the body frame
   /// @}
 
-  static const int Dim = traits<CAMERA>::dimension; ///< Camera dimension
-  static const int ZDim = traits<Z>::dimension; ///< Measurement dimension
-
-  // Definitions for block matrices used internally
-  typedef Eigen::Matrix<double, Dim, ZDim> MatrixD2; // F'
-  typedef Eigen::Matrix<double, Dim, Dim> MatrixDD; // camera hessian block
-  typedef Eigen::Matrix<double, ZDim, 3> Matrix23;
-  typedef Eigen::Matrix<double, Dim, 1> VectorD;
-  typedef Eigen::Matrix<double, ZDim, ZDim> Matrix2;
+  // Cache for Fblocks, to avoid a malloc ever time we re-linearize
+  mutable std::vector<MatrixZD> Fblocks;
 
 public:
 
-  // Definitions for blocks of F, externally visible
-  typedef Eigen::Matrix<double, ZDim, Dim> 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<Pose3> body_P_sensor = boost::none) :
-          body_P_sensor_(body_P_sensor){
+                  boost::optional<Pose3> 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<MatrixZD> 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<RegularHessianFactor<Dim> >(keys_,
         augmentedHessian);
@@ -307,10 +302,8 @@ public:
       const FastVector<Key> allKeys) const {
     Matrix E;
     Vector b;
-    std::vector<MatrixZD> 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_

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

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(&params);
-  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_();

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_,

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 <gtsam/slam/SmartProjectionFactor.h>
+#include <gtsam/geometry/Cal3Bundler.h>
+#include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/Point3.h>
+
+using namespace std;
+using namespace gtsam;
+
+typedef PinholeCamera<Cal3Bundler> Camera;
+typedef SmartProjectionFactor<Camera> 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<SfmFactor>(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);
+}