all pass!

gtsam/geometry/CameraSet.h

@@ -142,14 +142,67 @@ public:
     return ErrorVector(project2(point, Fs, E), measured);
   }
 
+  static SymmetricBlockMatrix mySchurComplement(
+        const std::vector< Eigen::Matrix<double, 2, 12>,
+        Eigen::aligned_allocator< Eigen::Matrix<double, 2, 12> > >& Fs,
+        const Matrix& E, const Eigen::Matrix<double, 3, 3>& P, const Vector& b) {
+    return mySchurComplement<2,3,12>(Fs, E, P, b);
+  }
+
+  template<int myZDim, int N, int ND> // N = 2 or 3 (point dimension), ND is the camera dimension
+  static SymmetricBlockMatrix mySchurComplement(
+      const std::vector< Eigen::Matrix<double, myZDim, ND>,
+      Eigen::aligned_allocator< Eigen::Matrix<double, myZDim, ND> > >& Fs,
+      const Matrix& E, const Eigen::Matrix<double, N, N>& P, const Vector& b) {
+
+    // a single point is observed in m cameras
+    size_t m = Fs.size();
+
+    // Create a SymmetricBlockMatrix (augmented hessian, with extra row/column with info vector)
+    size_t M1 = ND * m + 1;
+    std::vector<DenseIndex> dims(m + 1); // this also includes the b term
+    std::fill(dims.begin(), dims.end() - 1, ND);
+    dims.back() = 1;
+    SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1));
+
+    // Blockwise Schur complement
+    for (size_t i = 0; i < m; i++) { // for each camera
+
+      const Eigen::Matrix<double, myZDim, ND>& Fi = Fs[i];
+      const auto FiT = Fi.transpose();
+      const Eigen::Matrix<double, myZDim, N> Ei_P = //
+          E.block(myZDim * i, 0, myZDim, N) * P;
+
+      // D = (Dx2) * ZDim
+      augmentedHessian.setOffDiagonalBlock(i, m, FiT * b.segment<myZDim>(myZDim * i) // F' * b
+                                           - FiT * (Ei_P * (E.transpose() * b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1)
+
+      // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
+      augmentedHessian.setDiagonalBlock(i, FiT
+                                        * (Fi - Ei_P * E.block(myZDim * i, 0, myZDim, N).transpose() * Fi));
+
+      // upper triangular part of the hessian
+      for (size_t j = i + 1; j < m; j++) { // for each camera
+        const Eigen::Matrix<double, myZDim, ND>& Fj = Fs[j];
+
+        // (DxD) = (Dx2) * ( (2x2) * (2xD) )
+        augmentedHessian.setOffDiagonalBlock(i, j, -FiT
+                                             * (Ei_P * E.block(myZDim * j, 0, myZDim, N).transpose() * Fj));
+      }
+    } // end of for over cameras
+
+    augmentedHessian.diagonalBlock(m)(0, 0) += b.squaredNorm();
+    return augmentedHessian;
+  }
+
   /**
      * Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix
      * G = F' * F - F' * E * P * E' * F
      * g = F' * (b - E * P * E' * b)
      * Fixed size version
      */
-    template<int N, int ND> // N = 2 or 3, ND is the camera dimension
+    template<int N, int ND> // N = 2 or 3 (point dimension), ND is the camera dimension
-    static SymmetricBlockMatrix SchurComplement(
+    static SymmetricBlockMatrix SchurComplementWithCustomBlocks(
         const std::vector< Eigen::Matrix<double, ZDim, ND>, Eigen::aligned_allocator< Eigen::Matrix<double, ZDim, ND> > >& Fs,
         const Matrix& E, const Eigen::Matrix<double, N, N>& P, const Vector& b) {
 
@@ -202,11 +255,11 @@ public:
   template<int N> // N = 2 or 3
   static SymmetricBlockMatrix SchurComplement(const FBlocks& Fs,
       const Matrix& E, const Eigen::Matrix<double, N, N>& P, const Vector& b) {
-    return SchurComplement<N,D>(Fs, E, P, b);
+    return SchurComplementWithCustomBlocks<N,D>(Fs, E, P, b);
   }
 
   /// Computes Point Covariance P, with lambda parameter
-  template<int N> // N = 2 or 3
+  template<int N> // N = 2 or 3 (point dimension)
   static void ComputePointCovariance(Eigen::Matrix<double, N, N>& P,
       const Matrix& E, double lambda, bool diagonalDamping = false) {
 
@@ -258,7 +311,7 @@ public:
    * 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.
    */
-  template<int N> // N = 2 or 3
+  template<int N> // N = 2 or 3 (point dimension)
   static void UpdateSchurComplement(const FBlocks& Fs, const Matrix& E,
       const Eigen::Matrix<double, N, N>& P, const Vector& b,
       const KeyVector& allKeys, const KeyVector& keys,

gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h

@@ -17,10 +17,10 @@
 
 #pragma once
 
-#include <gtsam/slam/SmartProjectionFactor.h>
-#include <gtsam/slam/SmartFactorBase.h>
-#include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/CameraSet.h>
+#include <gtsam/geometry/PinholePose.h>
+#include <gtsam/geometry/PinholeSet.h>
+#include <gtsam/slam/SmartProjectionFactor.h>
 
 namespace gtsam {
 /**
@@ -60,11 +60,10 @@ PinholePose<CALIBRATION> > {
  public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
-  typedef PinholePose<CALIBRATION> Camera;
-  // typedef CameraSet<Camera> Cameras;
-
   /// shorthand for base class type
-  typedef SmartProjectionFactor<Camera> Base;
+  typedef SmartProjectionFactor<PinholePose<CALIBRATION> > Base;
+//  typedef PinholePose<CALIBRATION> Camera;
+//  typedef CameraSet<Camera> Cameras;
 
   /// shorthand for this class
   typedef SmartProjectionPoseFactorRollingShutter This;
@@ -312,8 +311,6 @@ PinholePose<CALIBRATION> > {
       throw std::runtime_error("SmartProjectionPoseFactorRollingShutter: "
           "measured_.size() inconsistent with input");
 
-    std::cout << "got this far.." << std::endl;
-
     // triangulate 3D point at given linearization point
     this->triangulateSafe(this->cameras(values));
 
@@ -336,91 +333,95 @@ PinholePose<CALIBRATION> > {
     for (size_t i = 0; i < Fs.size(); i++)
       Fs[i] = this->noiseModel_->Whiten(Fs[i]);
 
+     Matrix3 P = Base::Cameras::PointCov(E, lambda, diagonalDamping);
+
     // build augmented Hessian (with last row/column being the information vector)
-    Matrix3 P;
+    // and marginalize point: note - we reuse the standard SchurComplement function
-    Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping);
+    // does not work, since assumes convensional 6-dim blocks
+    // SymmetricBlockMatrix augmentedHessian =
+    //     Base::Cameras::SchurComplement(Fs, E, b, lambda,diagonalDamping);
 
-    //    // marginalize point: note - we reuse the standard SchurComplement function
+    SymmetricBlockMatrix augmentedHessian =
-    //    SymmetricBlockMatrix augmentedHessian = This::Cameras::SchurComplement<2,DimBlock>(Fs, E, P, b);
+         Base::Cameras::mySchurComplement(Fs, E, P, b);
 
-    //    // now pack into an Hessian factor
+    // now pack into an Hessian factor
-    //    std::vector<DenseIndex> dims(nrUniqueKeys + 1);  // this also includes the b term
+    std::vector<DenseIndex> dims(nrUniqueKeys + 1);  // this also includes the b term
-    //    std::fill(dims.begin(), dims.end() - 1, 6);
+    std::fill(dims.begin(), dims.end() - 1, 6);
-    //    dims.back() = 1;
+    dims.back() = 1;
-    //    SymmetricBlockMatrix augmentedHessianUniqueKeys;
+    SymmetricBlockMatrix augmentedHessianUniqueKeys;
-    //
-    //    // here we have to deal with the fact that some cameras may share the same extrinsic key
-    //    if (nrUniqueKeys == nrNonuniqueKeys) {  // if there is 1 calibration key per camera
-    //      augmentedHessianUniqueKeys = SymmetricBlockMatrix(
-    //          dims, Matrix(augmentedHessian.selfadjointView()));
-    //    } else {  // if multiple cameras share a calibration we have to rearrange
-    //      // the results of the Schur complement matrix
-    //      std::vector<DenseIndex> nonuniqueDims(nrNonuniqueKeys + 1);  // this also includes the b term
-    //      std::fill(nonuniqueDims.begin(), nonuniqueDims.end() - 1, 6);
-    //      nonuniqueDims.back() = 1;
-    //      augmentedHessian = SymmetricBlockMatrix(
-    //          nonuniqueDims, Matrix(augmentedHessian.selfadjointView()));
-    //
-    //      // these are the keys that correspond to the blocks in augmentedHessian (output of SchurComplement)
-    //      KeyVector nonuniqueKeys;
-    //      for (size_t i = 0; i < world_P_body_key_pairs_.size(); i++) {
-    //        nonuniqueKeys.push_back(world_P_body_key_pairs_.at(i));
-    //        nonuniqueKeys.push_back(body_P_cam_ this->keys_.at(i));
-    //      }
-    //
-    //      // get map from key to location in the new augmented Hessian matrix (the one including only unique keys)
-    //      std::map<Key, size_t> keyToSlotMap;
-    //      for (size_t k = 0; k < nrUniqueKeys; k++) {
-    //        keyToSlotMap[ this->keys_[k]] = k;
-    //      }
-    //
-    //      // initialize matrix to zero
-    //      augmentedHessianUniqueKeys = SymmetricBlockMatrix(
-    //          dims, Matrix::Zero(6 * nrUniqueKeys + 1, 6 * nrUniqueKeys + 1));
-    //
-    //      // add contributions for each key: note this loops over the hessian with nonUnique keys (augmentedHessian)
-    //      // and populates an Hessian that only includes the unique keys (that is what we want to return)
-    //      for (size_t i = 0; i < nrNonuniqueKeys; i++) {  // rows
-    //        Key key_i = nonuniqueKeys.at(i);
-    //
-    //        // update information vector
-    //        augmentedHessianUniqueKeys.updateOffDiagonalBlock(
-    //            keyToSlotMap[key_i], nrUniqueKeys,
-    //            augmentedHessian.aboveDiagonalBlock(i, nrNonuniqueKeys));
-    //
-    //        // update blocks
-    //        for (size_t j = i; j < nrNonuniqueKeys; j++) {  // cols
-    //          Key key_j = nonuniqueKeys.at(j);
-    //          if (i == j) {
-    //            augmentedHessianUniqueKeys.updateDiagonalBlock(
-    //                keyToSlotMap[key_i], augmentedHessian.diagonalBlock(i));
-    //          } else {  // (i < j)
-    //            if (keyToSlotMap[key_i] != keyToSlotMap[key_j]) {
-    //              augmentedHessianUniqueKeys.updateOffDiagonalBlock(
-    //                  keyToSlotMap[key_i], keyToSlotMap[key_j],
-    //                  augmentedHessian.aboveDiagonalBlock(i, j));
-    //            } else {
-    //              augmentedHessianUniqueKeys.updateDiagonalBlock(
-    //                  keyToSlotMap[key_i],
-    //                  augmentedHessian.aboveDiagonalBlock(i, j)
-    //                      + augmentedHessian.aboveDiagonalBlock(i, j).transpose());
-    //            }
-    //          }
-    //        }
-    //      }
-    //      // update bottom right element of the matrix
-    //      augmentedHessianUniqueKeys.updateDiagonalBlock(
-    //          nrUniqueKeys, augmentedHessian.diagonalBlock(nrNonuniqueKeys));
-    //    }
-    //    return boost::make_shared < RegularHessianFactor<DimPose>
-    //        > ( this->keys_, augmentedHessianUniqueKeys);
 
-    // TO REMOVE:
+    // here we have to deal with the fact that some cameras may share the same extrinsic key
-    for (Matrix& m : Gs)
+    if (nrUniqueKeys == nrNonuniqueKeys) {  // if there is 1 calibration key per camera
-      m = Matrix::Zero(DimPose, DimPose);
+      augmentedHessianUniqueKeys = SymmetricBlockMatrix(
-    for (Vector& v : gs)
+          dims, Matrix(augmentedHessian.selfadjointView()));
-      v = Vector::Zero(DimPose);
+    } else {  // if multiple cameras share a calibration we have to rearrange
-    return boost::make_shared < RegularHessianFactor<DimPose> > ( this->keys_, Gs, gs, 0.0);
+      // the results of the Schur complement matrix
+      std::vector<DenseIndex> nonuniqueDims(nrNonuniqueKeys + 1);  // this also includes the b term
+      std::fill(nonuniqueDims.begin(), nonuniqueDims.end() - 1, 6); // all are poses ..
+      nonuniqueDims.back() = 1; // except b is a scalar
+      augmentedHessian = SymmetricBlockMatrix(
+          nonuniqueDims, Matrix(augmentedHessian.selfadjointView()));
+
+      // these are the keys that correspond to the blocks in augmentedHessian (output of SchurComplement)
+      KeyVector nonuniqueKeys;
+      for (size_t i = 0; i < world_P_body_key_pairs_.size(); i++) {
+        nonuniqueKeys.push_back(world_P_body_key_pairs_.at(i).first);
+        nonuniqueKeys.push_back(world_P_body_key_pairs_.at(i).second);
+      }
+
+      // get map from key to location in the new augmented Hessian matrix (the one including only unique keys)
+      std::map<Key, size_t> keyToSlotMap;
+      for (size_t k = 0; k < nrUniqueKeys; k++) {
+        keyToSlotMap[ this->keys_[k] ] = k;
+      }
+
+      // initialize matrix to zero
+      augmentedHessianUniqueKeys = SymmetricBlockMatrix(
+          dims, Matrix::Zero(6 * nrUniqueKeys + 1, 6 * nrUniqueKeys + 1));
+
+      // add contributions for each key: note this loops over the hessian with nonUnique keys (augmentedHessian)
+      // and populates an Hessian that only includes the unique keys (that is what we want to return)
+      for (size_t i = 0; i < nrNonuniqueKeys; i++) {  // rows
+        Key key_i = nonuniqueKeys.at(i);
+
+        // update information vector
+        augmentedHessianUniqueKeys.updateOffDiagonalBlock(
+            keyToSlotMap[key_i], nrUniqueKeys,
+            augmentedHessian.aboveDiagonalBlock(i, nrNonuniqueKeys));
+
+        // update blocks
+        for (size_t j = i; j < nrNonuniqueKeys; j++) {  // cols
+          Key key_j = nonuniqueKeys.at(j);
+          if (i == j) {
+            augmentedHessianUniqueKeys.updateDiagonalBlock(
+                keyToSlotMap[key_i], augmentedHessian.diagonalBlock(i));
+          } else {  // (i < j)
+            if (keyToSlotMap[key_i] != keyToSlotMap[key_j]) {
+              augmentedHessianUniqueKeys.updateOffDiagonalBlock(
+                  keyToSlotMap[key_i], keyToSlotMap[key_j],
+                  augmentedHessian.aboveDiagonalBlock(i, j));
+            } else {
+              augmentedHessianUniqueKeys.updateDiagonalBlock(
+                  keyToSlotMap[key_i],
+                  augmentedHessian.aboveDiagonalBlock(i, j)
+                  + augmentedHessian.aboveDiagonalBlock(i, j).transpose());
+            }
+          }
+        }
+      }
+      // update bottom right element of the matrix
+      augmentedHessianUniqueKeys.updateDiagonalBlock(
+          nrUniqueKeys, augmentedHessian.diagonalBlock(nrNonuniqueKeys));
+    }
+    return boost::make_shared < RegularHessianFactor<DimPose>
+    > ( this->keys_, augmentedHessianUniqueKeys);
+
+//    // TO REMOVE:
+//    for (Matrix& m : Gs)
+//      m = Matrix::Zero(DimPose, DimPose);
+//    for (Vector& v : gs)
+//      v = Vector::Zero(DimPose);
+//    return boost::make_shared < RegularHessianFactor<DimPose> > ( this->keys_, Gs, gs, 0.0);
   }
 
   /**
@@ -470,7 +471,7 @@ PinholePose<CALIBRATION> > {
     // depending on flag set on construction we may linearize to different linear factors
     switch (this->params_.linearizationMode) {
       case HESSIAN:
-        return createHessianFactor(values, lambda);
+        return this->createHessianFactor(values, lambda);
       default:
         throw std::runtime_error(
             "SmartProjectionPoseFactorRollingShutter: unknown linearization mode");
@@ -480,7 +481,7 @@ PinholePose<CALIBRATION> > {
   /// linearize
   boost::shared_ptr<GaussianFactor> linearize(const Values& values) const
           override {
-    return linearizeDamped(values);
+    return this->linearizeDamped(values);
   }
 
  private:

gtsam_unstable/slam/SmartStereoProjectionFactorPP.h

@@ -250,7 +250,7 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
 
     // marginalize point: note - we reuse the standard SchurComplement function
     SymmetricBlockMatrix augmentedHessian =
-        Cameras::SchurComplement<3, Dim>(Fs, E, P, b);
+        Cameras::SchurComplementWithCustomBlocks<3, Dim>(Fs, E, P, b);
 
     // now pack into an Hessian factor
     std::vector<DenseIndex> dims(nrUniqueKeys + 1);  // this also includes the b term