Documented linear factors better.

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,
+ * In essence, the linear factor |E*dp + F*dX - b|, where p is point and X are
- * is multiplied by Enull, a matrix that spans the left nullspace of E, i.e.,
+ * poses, is multiplied by Enull, a matrix that spans the left nullspace of E,
- * The mx3 matrix is analyzed with SVD as E = [Erange Enull]*S*V (mxm * mx3 * 3x3)
+ * i.e., The mx3 matrix is analyzed with SVD as E = [Erange Enull]*S*V (mxm *
- * where Enull is an m x (m-3) matrix
+ * mx3 * 3x3) where Enull is an m x (m-3) matrix Then Enull'*E*dp = 0, and
- * 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.
+ * The code below assumes that F is block diagonal and is given as a vector of
- * Example: m = 4 (2 measurements), Enull = 4*1, F = 4*12 (for D=6)
+ * ZDim*D blocks. Example: m = 4 (2 measurements), Enull = 4*1, F = 4*12 (for
- * Then Enull'*F = 1*4 * 4*12 = 1*12, but each 1*6 piece can be computed as a 1x2 * 2x6 mult
+ * 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 @@ public:
   JacobianFactorSVD() {
   }
 
-  /// Empty constructor with keys
+  /// Empty constructor with keys.
-  JacobianFactorSVD(const KeyVector& keys, //
+  JacobianFactorSVD(const KeyVector& keys,
-      const SharedDiagonal& model = SharedDiagonal()) :
+                    const SharedDiagonal& model = SharedDiagonal())
-      Base() {
+      : Base() {
     Matrix zeroMatrix = Matrix::Zero(0, D);
     Vector zeroVector = Vector::Zero(0);
     std::vector<KeyMatrix> QF;
@@ -51,18 +52,21 @@ public:
   }
 
   /**
-   * @brief Constructor
+   * @brief Construct a new JacobianFactorSVD object, createing a reduced-rank
-   * Takes the CameraSet derivatives (as ZDim*D blocks of block-diagonal F)
+   * Jacobian factor on the CameraSet.
-   * and a reduced point derivative, Enull
-   * and creates 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,
+  JacobianFactorSVD(
-      const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& Fblocks, const Matrix& Enull,
+      const KeyVector& keys,
-      const Vector& b, //
+      const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& Fblocks,
-      const SharedDiagonal& model = SharedDiagonal()) :
+      const Matrix& Enull, const Vector& b,
-      Base() {
+      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 @@ public:
     QF.reserve(numKeys);
     for (size_t k = 0; k < Fblocks.size(); ++k) {
       Key key = keys[k];
-      QF.push_back(
+      QF.emplace_back(
-          KeyMatrix(key,
+          key, (Enull.transpose()).block(0, ZDim * k, m2, ZDim) * Fblocks[k]);
-              (Enull.transpose()).block(0, ZDim * k, m2, ZDim) * Fblocks[k]));
     }
     JacobianFactor::fillTerms(QF, Enull.transpose() * b, model);
   }

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 @@ protected:
   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 @@ public:
   }
 
   /// 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) :
+   * @brief Construct a new RegularImplicitSchurFactor object.
-      GaussianFactor(keys), FBlocks_(FBlocks), PointCovariance_(P), E_(E), b_(b) {
+   * 
-  }
+   * @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_;
   }