Merge pull request #865 from borglab/feature/readmes

gtsam/slam/JacobianFactorSVD.h

@@ -9,20 +9,21 @@
 
 namespace gtsam {
 /**
- * JacobianFactor for Schur complement that uses the "Nullspace Trick" by Mourikis
+ * JacobianFactor for Schur complement that uses the "Nullspace Trick" by
+ * Mourikis et al.
  *
  * This trick is equivalent to the Schur complement, but can be faster.
- * In essence, the linear factor |E*dp + F*dX - b|, where p is point and X are poses,
- * is multiplied by Enull, a matrix that spans the left nullspace of E, i.e.,
- * The mx3 matrix is analyzed with SVD as E = [Erange Enull]*S*V (mxm * mx3 * 3x3)
- * where Enull is an m x (m-3) matrix
- * Then Enull'*E*dp = 0, and
+ * In essence, the linear factor |E*dp + F*dX - b|, where p is point and X are
+ * poses, is multiplied by Enull, a matrix that spans the left nullspace of E,
+ * i.e., The mx3 matrix is analyzed with SVD as E = [Erange Enull]*S*V (mxm *
+ * mx3 * 3x3) where Enull is an m x (m-3) matrix Then Enull'*E*dp = 0, and
  *  |Enull'*E*dp + Enull'*F*dX - Enull'*b| == |Enull'*F*dX - Enull'*b|
  * Normally F is m x 6*numKeys, and Enull'*F yields an (m-3) x 6*numKeys matrix.
  *
- * The code below assumes that F is block diagonal and is given as a vector of ZDim*D blocks.
- * Example: m = 4 (2 measurements), Enull = 4*1, F = 4*12 (for D=6)
- * Then Enull'*F = 1*4 * 4*12 = 1*12, but each 1*6 piece can be computed as a 1x2 * 2x6 mult
+ * The code below assumes that F is block diagonal and is given as a vector of
+ * ZDim*D blocks. Example: m = 4 (2 measurements), Enull = 4*1, F = 4*12 (for
+ * D=6) Then Enull'*F = 1*4 * 4*12 = 1*12, but each 1*6 piece can be computed as
+ * a 1x2 * 2x6 multiplication.
  */
 template<size_t D, size_t ZDim>
 class JacobianFactorSVD: public RegularJacobianFactor<D> {
@@ -37,10 +38,10 @@ class JacobianFactorSVD: public RegularJacobianFactor<D> {
   JacobianFactorSVD() {
   }
 
-  /// Empty constructor with keys
-  JacobianFactorSVD(const KeyVector& keys, //
-      const SharedDiagonal& model = SharedDiagonal()) :
-      Base() {
+  /// Empty constructor with keys.
+  JacobianFactorSVD(const KeyVector& keys,
+                    const SharedDiagonal& model = SharedDiagonal())
+      : Base() {
     Matrix zeroMatrix = Matrix::Zero(0, D);
     Vector zeroVector = Vector::Zero(0);
     std::vector<KeyMatrix> QF;
@@ -51,18 +52,21 @@ class JacobianFactorSVD: public RegularJacobianFactor<D> {
   }
 
   /**
-   * @brief Constructor
-   * Takes the CameraSet derivatives (as ZDim*D blocks of block-diagonal F)
-   * and a reduced point derivative, Enull
-   * and creates a reduced-rank Jacobian factor on the CameraSet
+   * @brief Construct a new JacobianFactorSVD object, createing a reduced-rank
+   * Jacobian factor on the CameraSet.
    *
-   * @Fblocks:
+   * @param keys keys associated with F blocks.
+   * @param Fblocks CameraSet derivatives, ZDim*D blocks of block-diagonal F
+   * @param Enull a reduced point derivative
+   * @param b right-hand side
+   * @param model noise model
    */
-  JacobianFactorSVD(const KeyVector& keys,
-      const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& Fblocks, const Matrix& Enull,
-      const Vector& b, //
-      const SharedDiagonal& model = SharedDiagonal()) :
-      Base() {
+  JacobianFactorSVD(
+      const KeyVector& keys,
+      const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& Fblocks,
+      const Matrix& Enull, const Vector& b,
+      const SharedDiagonal& model = SharedDiagonal())
+      : Base() {
     size_t numKeys = Enull.rows() / ZDim;
     size_t m2 = ZDim * numKeys - 3; // TODO: is this not just Enull.rows()?
     // PLAIN nullptr SPACE TRICK
@@ -74,9 +78,8 @@ class JacobianFactorSVD: public RegularJacobianFactor<D> {
     QF.reserve(numKeys);
     for (size_t k = 0; k < Fblocks.size(); ++k) {
       Key key = keys[k];
-      QF.push_back(
-          KeyMatrix(key,
-              (Enull.transpose()).block(0, ZDim * k, m2, ZDim) * Fblocks[k]));
+      QF.emplace_back(
+          key, (Enull.transpose()).block(0, ZDim * k, m2, ZDim) * Fblocks[k]);
     }
     JacobianFactor::fillTerms(QF, Enull.transpose() * b, model);
   }

gtsam/slam/ProjectionFactor.h

@@ -11,7 +11,7 @@
 
 /**
  * @file ProjectionFactor.h
- * @brief Basic bearing factor from 2D measurement
+ * @brief Reprojection of a LANDMARK to a 2D point.
  * @author Chris Beall
  * @author Richard Roberts
  * @author Frank Dellaert
@@ -22,17 +22,21 @@
 
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/Point3.h>
 #include <gtsam/geometry/Cal3_S2.h>
 #include <boost/optional.hpp>
 
 namespace gtsam {
 
   /**
-   * Non-linear factor for a constraint derived from a 2D measurement. The calibration is known here.
-   * i.e. the main building block for visual SLAM.
+   * Non-linear factor for a constraint derived from a 2D measurement. 
+   * The calibration is known here.
+   * The main building block for visual SLAM.
    * @addtogroup SLAM
    */
-  template<class POSE, class LANDMARK, class CALIBRATION = Cal3_S2>
+  template <class POSE = Pose3, class LANDMARK = Point3,
+            class CALIBRATION = Cal3_S2>
   class GenericProjectionFactor: public NoiseModelFactor2<POSE, LANDMARK> {
   protected:
 
@@ -57,9 +61,9 @@ namespace gtsam {
     typedef boost::shared_ptr<This> shared_ptr;
 
     /// Default constructor
-  GenericProjectionFactor() :
-      measured_(0, 0), throwCheirality_(false), verboseCheirality_(false) {
-  }
+    GenericProjectionFactor() :
+        measured_(0, 0), throwCheirality_(false), verboseCheirality_(false) {
+    }
 
     /**
      * Constructor

gtsam/slam/ReadMe.md

@@ -0,0 +1,70 @@
+# SLAM Factors
+
+## GenericProjectionFactor (defined in ProjectionFactor.h)
+
+Non-linear factor that minimizes the re-projection error with respect to a 2D measurement. 
+The calibration is assumed known and passed in the constructor.
+The main building block for visual SLAM.
+
+Templated on
+- `POSE`, default `Pose3`
+- `LANDMARK`, default `Point3`
+- `CALIBRATION`, default `Cal3_S2`
+
+## SmartFactors
+
+These are "structure-less" factors, i.e., rather than introducing a new variable for an observed 3D point or landmark, a single factor is created that provides a multi-view constraint on several poses and/or cameras.
+While one typically adds multiple GenericProjectionFactors (one for each observation of a landmark), a SmartFactor collects all measurements for a landmark, i.e., the factor graph contains 1 smart factor per landmark.
+
+### SmartFactorBase
+
+This is the base class for smart factors, templated on a `CAMERA` type.
+It has no internal point, but it saves the measurements, keeps a noise model, and an optional sensor pose.
+
+### SmartProjectionFactor
+
+Also templated on `CAMERA`. Triangulates a 3D point and keeps an estimate of it around.
+This factor operates with monocular cameras, and is used to optimize the camera pose
+*and* calibration for each camera, and requires variables of type `CAMERA` in values.
+
+If the calibration is fixed use `SmartProjectionPoseFactor` instead!
+
+
+### SmartProjectionPoseFactor
+
+Derives from `SmartProjectionFactor` but is templated on a `CALIBRATION` type, setting `CAMERA = PinholePose<CALIBRATION>`.
+This factor assumes that the camera calibration is fixed and the same for all cameras involved in this factor.
+The factor only constrains poses.
+
+If the calibration should be optimized, as well, use `SmartProjectionFactor` instead!
+
+### SmartProjectionFactorP
+
+Same as `SmartProjectionPoseFactor`, except:
+- it is templated on `CAMERA`, i.e., it allows cameras beyond pinhole;
+- it admits a different calibration for each measurement, i.e., it can model a multi-camera system;
+- it allows multiple observations from the same pose/key, again, to model a multi-camera system.
+
+TODO: DimPose and ZDim are hardcoded. Copy/paste from `SmartProjectionPoseFactor`. Unclear what the use case is.
+
+## Linearized Smart Factors
+
+The factors below are less likely to be relevant to the user, but result from using the non-linear smart factors above.
+
+
+### RegularImplicitSchurFactor
+
+A specialization of a GaussianFactor to structure-less SFM, which is very fast in a conjugate gradient (CG) solver. 
+It is produced by calling `createRegularImplicitSchurFactor` in `SmartFactorBase` or `SmartProjectionFactor`. 
+
+### JacobianFactorQ
+
+A RegularJacobianFactor that uses some badly documented reduction on the Jacobians.
+
+### JacobianFactorQR
+
+A RegularJacobianFactor that eliminates a point using sequential elimination.
+
+### JacobianFactorQR
+
+A RegularJacobianFactor that uses the "Nullspace Trick" by Mourikis et al. See the documentation in the file, which *is* well documented.

gtsam/slam/RegularImplicitSchurFactor.h

@@ -1,6 +1,6 @@
 /**
  * @file    RegularImplicitSchurFactor.h
- * @brief   A new type of linear factor (GaussianFactor), which is subclass of GaussianFactor
+ * @brief   A subclass of GaussianFactor specialized to structureless SFM.
  * @author  Frank Dellaert
  * @author  Luca Carlone
  */
@@ -20,6 +20,20 @@ namespace gtsam {
 
 /**
  * RegularImplicitSchurFactor
+ * 
+ * A specialization of a GaussianFactor to structure-less SFM, which is very
+ * fast in a conjugate gradient (CG) solver. Specifically, as measured in 
+ * timeSchurFactors.cpp, it stays very fast for an increasing number of cameras.
+ * The magic is in multiplyHessianAdd, which does the Hessian-vector multiply at 
+ * the core of CG, and implements
+ *    y += F'*alpha*(I - E*P*E')*F*x
+ * where 
+ *  - F is the 2mx6m Jacobian of the m 2D measurements wrpt m 6DOF poses
+ *  - E is the 2mx3 Jacabian of the m 2D measurements wrpt a 3D point
+ *  - P is the covariance on the point
+ * The equation above implicitly executes the Schur complement by removing the
+ * information E*P*E' from the Hessian. It is also very fast as we do not use 
+ * the full 6m*6m F matrix, but rather only it's m 6x6 diagonal blocks.
  */
 template<class CAMERA>
 class RegularImplicitSchurFactor: public GaussianFactor {
@@ -38,9 +52,10 @@ class RegularImplicitSchurFactor: public GaussianFactor {
   static const int ZDim = traits<Z>::dimension; ///< Measurement dimension
 
   typedef Eigen::Matrix<double, ZDim, D> MatrixZD; ///< type of an F block
-  typedef Eigen::Matrix<double, D, D> MatrixDD; ///< camera hessian
+  typedef Eigen::Matrix<double, D, D> MatrixDD; ///< camera Hessian
+  typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > FBlocks;
 
-  const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > FBlocks_; ///< All ZDim*D F blocks (one for each camera)
+  FBlocks FBlocks_; ///< All ZDim*D F blocks (one for each camera)
   const Matrix PointCovariance_; ///< the 3*3 matrix P = inv(E'E) (2*2 if degenerate)
   const Matrix E_; ///< The 2m*3 E Jacobian with respect to the point
   const Vector b_; ///< 2m-dimensional RHS vector
@@ -52,17 +67,25 @@ class RegularImplicitSchurFactor: public GaussianFactor {
   }
 
   /// Construct from blocks of F, E, inv(E'*E), and RHS vector b
-  RegularImplicitSchurFactor(const KeyVector& keys,
-      const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& FBlocks, const Matrix& E, const Matrix& P,
-      const Vector& b) :
-      GaussianFactor(keys), FBlocks_(FBlocks), PointCovariance_(P), E_(E), b_(b) {
-  }
+
+  /**
+   * @brief Construct a new RegularImplicitSchurFactor object.
+   * 
+   * @param keys keys corresponding to cameras
+   * @param Fs All ZDim*D F blocks (one for each camera)
+   * @param E Jacobian of measurements wrpt point.
+   * @param P point covariance matrix
+   * @param b RHS vector
+   */
+  RegularImplicitSchurFactor(const KeyVector& keys, const FBlocks& Fs,
+                             const Matrix& E, const Matrix& P, const Vector& b)
+      : GaussianFactor(keys), FBlocks_(Fs), PointCovariance_(P), E_(E), b_(b) {}
 
   /// Destructor
   ~RegularImplicitSchurFactor() override {
   }
 
-  std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& FBlocks() const {
+  const FBlocks& Fs() const {
     return FBlocks_;
   }