Removed obsolete code, including slow Schur-complement versions

gtsam/slam/SmartFactorBase.h

@@ -342,18 +342,6 @@ public:
       F.block<This::ZDim, Dim>(This::ZDim * i, Dim * i) = Fblocks.at(i).second;
   }
 
-  /**
-   *  Compute F, E, and b, where F and E are the stacked derivatives
-   *  with respect to the point. The value of cameras/point are passed as parameters.
-   */
-  double computeJacobians(Matrix& F, Matrix& E, Vector& b,
-      const Cameras& cameras, const Point3& point) const {
-    std::vector<KeyMatrix2D> Fblocks;
-    double f = computeJacobians(Fblocks, E, b, cameras, point);
-    FillDiagonalF(Fblocks, F);
-    return f;
-  }
-
   /// SVD version
   double computeJacobiansSVD(std::vector<KeyMatrix2D>& Fblocks, Matrix& Enull,
       Vector& b, const Cameras& cameras, const Point3& point) const {
@@ -370,16 +358,6 @@ public:
     return f;
   }
 
-  /// Matrix version of SVD
-  // TODO, there should not be a Matrix version, really
-  double computeJacobiansSVD(Matrix& F, Matrix& Enull, Vector& b,
-      const Cameras& cameras, const Point3& point) const {
-    std::vector<KeyMatrix2D> Fblocks;
-    double f = computeJacobiansSVD(Fblocks, Enull, b, cameras, point);
-    FillDiagonalF(Fblocks, F);
-    return f;
-  }
-
   /**
    * Linearize returns a Hessianfactor that is an approximation of error(p)
    */
@@ -394,69 +372,19 @@ public:
     double f = computeJacobians(Fblocks, E, b, cameras, point);
     Matrix3 P = PointCov(E, lambda, diagonalDamping);
 
-//#define HESSIAN_BLOCKS // slower, as internally the Hessian factor will transform the blocks into SymmetricBlockMatrix
+    // we create directly a SymmetricBlockMatrix
-#ifdef HESSIAN_BLOCKS
+    size_t M1 = Dim * m + 1;
-    // Create structures for Hessian Factors
-    std::vector < Matrix > Gs(m * (m + 1) / 2);
-    std::vector < Vector > gs(m);
-
-    sparseSchurComplement(Fblocks, E, P, b, Gs, gs);
-    // schurComplement(Fblocks, E, P, b, Gs, gs);
-
-    //std::vector < Matrix > Gs2(Gs.begin(), Gs.end());
-    //std::vector < Vector > gs2(gs.begin(), gs.end());
-
-    return boost::make_shared < RegularHessianFactor<Dim> > (this->keys_, Gs, gs, f);
-#else // we create directly a SymmetricBlockMatrix
-    size_t n1 = Dim * m + 1;
     std::vector<DenseIndex> dims(m + 1); // this also includes the b term
     std::fill(dims.begin(), dims.end() - 1, Dim);
     dims.back() = 1;
 
-    SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(n1, n1)); // for 10 cameras, size should be (10*Dim+1 x 10*Dim+1)
+    // build augmented hessian
-    sparseSchurComplement(Fblocks, E, P, b, augmentedHessian); // augmentedHessian.matrix().block<Dim,Dim> (i1,i2) = ...
+    SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1));
+    sparseSchurComplement(Fblocks, E, P, b, augmentedHessian);
     augmentedHessian(m, m)(0, 0) = f;
+
     return boost::make_shared<RegularHessianFactor<Dim> >(keys_,
         augmentedHessian);
-#endif
-  }
-
-  /**
-   * Do Schur complement, given Jacobian as F,E,P.
-   * Slow version - works on full matrices
-   */
-  void schurComplement(const std::vector<KeyMatrix2D>& Fblocks, const Matrix& E,
-      const Matrix3& P, const Vector& b,
-      /*output ->*/std::vector<Matrix>& Gs, std::vector<Vector>& gs) const {
-    // Schur complement trick
-    // Gs = F' * F - F' * E * inv(E'*E) * E' * F
-    // gs = F' * (b - E * inv(E'*E) * E' * b)
-    // This version uses full matrices
-
-    int numKeys = this->keys_.size();
-
-    /// Compute Full F ????
-    Matrix F;
-    FillDiagonalF(Fblocks, F);
-
-    Matrix H(Dim * numKeys, Dim * numKeys);
-    Vector gs_vector;
-
-    H.noalias() = F.transpose() * (F - (E * (P * (E.transpose() * F))));
-    gs_vector.noalias() = F.transpose() * (b - (E * (P * (E.transpose() * b))));
-
-    // Populate Gs and gs
-    int GsCount2 = 0;
-    for (DenseIndex i1 = 0; i1 < numKeys; i1++) { // for each camera
-      DenseIndex i1D = i1 * Dim;
-      gs.at(i1) = gs_vector.segment<Dim>(i1D);
-      for (DenseIndex i2 = 0; i2 < numKeys; i2++) {
-        if (i2 >= i1) {
-          Gs.at(GsCount2) = H.block<Dim, Dim>(i1D, i2 * Dim);
-          GsCount2++;
-        }
-      }
-    }
   }
 
   /**
@@ -500,55 +428,6 @@ public:
     } // end of for over cameras
   }
 
-  /**
-   * Do Schur complement, given Jacobian as F,E,P, return Gs/gs
-   * Fast version - works on with sparsity
-   */
-  void sparseSchurComplement(const std::vector<KeyMatrix2D>& Fblocks,
-      const Matrix& E, const Matrix3& P /*Point Covariance*/, const Vector& b,
-      /*output ->*/std::vector<Matrix>& Gs, std::vector<Vector>& gs) const {
-    // Schur complement trick
-    // Gs = F' * F - F' * E * P * E' * F
-    // gs = F' * (b - E * P * E' * b)
-
-    // a single point is observed in numKeys cameras
-    size_t numKeys = this->keys_.size();
-
-    int GsIndex = 0;
-    // Blockwise Schur complement
-    for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera
-      // GsIndex points to the upper triangular blocks
-      // 0  1  2  3  4
-      // X  5  6  7  8
-      // X  X  9 10 11
-      // X  X  X 12 13
-      // X  X  X X  14
-      const Matrix2D& Fi1 = Fblocks.at(i1).second;
-
-      const Matrix23 Ei1_P = E.block<ZDim, 3>(ZDim * i1, 0) * P;
-
-      { // for i1 = i2
-        // Dim = (Dx2) * (2)
-        gs.at(i1) = Fi1.transpose() * b.segment<ZDim>(ZDim * i1) // F' * b
-        - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // Dim = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1)
-
-        // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
-        Gs.at(GsIndex) = Fi1.transpose()
-            * (Fi1 - Ei1_P * E.block<ZDim, 3>(ZDim * i1, 0).transpose() * Fi1);
-        GsIndex++;
-      }
-      // upper triangular part of the hessian
-      for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera
-        const Matrix2D& Fi2 = Fblocks.at(i2).second;
-
-        // (DxD) = (Dx2) * ( (2x2) * (2xD) )
-        Gs.at(GsIndex) = -Fi1.transpose()
-            * (Ei1_P * E.block<ZDim, 3>(ZDim * i2, 0).transpose() * Fi2);
-        GsIndex++;
-      }
-    } // end of for over cameras
-  }
-
   /**
    * Applies Schur complement (exploiting block structure) to get a smart factor on cameras,
    * and adds the contribution of the smart factor to a pre-allocated augmented Hessian.

gtsam/slam/tests/testSmartProjectionCameraFactor.cpp

@@ -746,7 +746,7 @@ TEST( SmartProjectionCameraFactor, computeImplicitJacobian ) {
   SmartFactorBundler::shared_ptr factor1(new SmartFactorBundler());
   factor1->add(level_uv, c1, unit2);
   factor1->add(level_uv_right, c2, unit2);
-  Matrix expectedF, expectedE;
+  Matrix expectedE;
   Vector expectedb;
 
   CameraSet<Camera> cameras;
@@ -757,7 +757,8 @@ TEST( SmartProjectionCameraFactor, computeImplicitJacobian ) {
   Point3 point;
   if (factor1->point())
     point = *(factor1->point());
-  factor1->computeJacobians(expectedF, expectedE, expectedb, cameras, point);
+  vector<SmartFactorBundler::KeyMatrix2D> Fblocks;
+  factor1->computeJacobians(Fblocks, expectedE, expectedb, cameras, point);
 
   NonlinearFactorGraph generalGraph;
   SFMFactor sfm1(level_uv, unit2, c1, l1);

gtsam_unstable/slam/SmartStereoProjectionFactor.h

@@ -406,9 +406,11 @@ public:
     }
 
     // ==================================================================
+    std::vector<typename Base::KeyMatrix2D> Fblocks;
     Matrix F, E;
     Vector b;
-    double f = computeJacobians(F, E, b, cameras);
+    double f = computeJacobians(Fblocks, E, b, cameras);
+    Base::FillDiagonalF(Fblocks,F); // expensive !!!
 
     // Schur complement trick
     // Frank says: should be possible to do this more efficiently?
@@ -591,12 +593,6 @@ public:
     return true;
   }
 
-  /// Returns Matrix, TODO: maybe should not exist -> not sparse !
-  double computeJacobians(Matrix& F, Matrix& E, Vector& b,
-      const Cameras& cameras) const {
-    return Base::computeJacobians(F, E, b, cameras, point_);
-  }
-
   /// Calculate vector of re-projection errors, before applying noise model
   Vector reprojectionErrorAfterTriangulation(const Values& values) const {
     Cameras cameras;