Feature/rolling shutter smart factors

gtsam/base/Lie.h

@@ -17,6 +17,7 @@
  * @author Frank Dellaert
  * @author Mike Bosse
  * @author Duy Nguyen Ta
+ * @author Yotam Stern
  */
 
 
@@ -319,12 +320,28 @@ T expm(const Vector& x, int K=7) {
 }
 
 /**
- * Linear interpolation between X and Y by coefficient t in [0, 1].
+ * Linear interpolation between X and Y by coefficient t. Typically t \in [0,1],
+ * but can also be used to extrapolate before pose X or after pose Y.
  */
 template <typename T>
-T interpolate(const T& X, const T& Y, double t) {
-  assert(t >= 0 && t <= 1);
-  return traits<T>::Compose(X, traits<T>::Expmap(t * traits<T>::Logmap(traits<T>::Between(X, Y))));
+T interpolate(const T& X, const T& Y, double t,
+              typename MakeOptionalJacobian<T, T>::type Hx = boost::none,
+              typename MakeOptionalJacobian<T, T>::type Hy = boost::none) {
+  if (Hx || Hy) {
+    typename MakeJacobian<T, T>::type between_H_x, log_H, exp_H, compose_H_x;
+    const T between =
+        traits<T>::Between(X, Y, between_H_x);  // between_H_y = identity
+    typename traits<T>::TangentVector delta = traits<T>::Logmap(between, log_H);
+    const T Delta = traits<T>::Expmap(t * delta, exp_H);
+    const T result = traits<T>::Compose(
+        X, Delta, compose_H_x);  // compose_H_xinv_y = identity
+
+    if (Hx) *Hx = compose_H_x + t * exp_H * log_H * between_H_x;
+    if (Hy) *Hy = t * exp_H * log_H;
+    return result;
+  }
+  return traits<T>::Compose(
+      X, traits<T>::Expmap(t * traits<T>::Logmap(traits<T>::Between(X, Y))));
 }
 
 /**

gtsam/geometry/CameraSet.h

@@ -147,51 +147,149 @@ class CameraSet : public std::vector<CAMERA, Eigen::aligned_allocator<CAMERA> >
      * 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
-    static SymmetricBlockMatrix SchurComplement(
-        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) {
-
-      // 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, ZDim, ND>& Fi = Fs[i];
-        const auto FiT = Fi.transpose();
-        const Eigen::Matrix<double, ZDim, N> Ei_P = //
-            E.block(ZDim * i, 0, ZDim, N) * P;
-
-        // D = (Dx2) * ZDim
-        augmentedHessian.setOffDiagonalBlock(i, m, FiT * b.segment<ZDim>(ZDim * i) // F' * b
-        - FiT * (Ei_P * (E.transpose() * b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1)
+    */
+  template <int N,
+            int ND>  // N = 2 or 3 (point dimension), ND is the camera dimension
+  static SymmetricBlockMatrix SchurComplement(
+      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) {
+    // 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, ZDim, ND>& Fi = Fs[i];
+      const auto FiT = Fi.transpose();
+      const Eigen::Matrix<double, ZDim, N> Ei_P = //
+          E.block(ZDim * i, 0, ZDim, N) * P;
+
+      // D = (Dx2) * ZDim
+      augmentedHessian.setOffDiagonalBlock(i, m, FiT * b.segment<ZDim>(ZDim * 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(ZDim * i, 0, ZDim, 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, ZDim, ND>& Fj = Fs[j];
+
+        // (DxD) = (Dx2) * ( (2x2) * (2xD) )
+        augmentedHessian.setOffDiagonalBlock(i, j, -FiT
+            * (Ei_P * E.block(ZDim * j, 0, ZDim, N).transpose() * Fj));
+      }
+    } // end of for over cameras
 
-        // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
-        augmentedHessian.setDiagonalBlock(i, FiT
-            * (Fi - Ei_P * E.block(ZDim * i, 0, ZDim, N).transpose() * Fi));
+    augmentedHessian.diagonalBlock(m)(0, 0) += b.squaredNorm();
+    return augmentedHessian;
+  }
 
-        // upper triangular part of the hessian
-        for (size_t j = i + 1; j < m; j++) { // for each camera
-          const Eigen::Matrix<double, ZDim, ND>& Fj = Fs[j];
+  /**
+   * 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)
+   * In this version, we allow for the case where the keys in the Jacobian are
+   * organized differently from the keys in the output SymmetricBlockMatrix In
+   * particular: each diagonal block of the Jacobian F captures 2 poses (useful
+   * for rolling shutter and extrinsic calibration) such that F keeps the block
+   * structure that makes the Schur complement trick fast.
+   *
+   * N = 2 or 3 (point dimension), ND is the Jacobian block dimension, NDD is
+   * the Hessian block dimension
+   */
+  template <int N, int ND, int NDD>
+  static SymmetricBlockMatrix SchurComplementAndRearrangeBlocks(
+      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,
+      const KeyVector& jacobianKeys, const KeyVector& hessianKeys) {
+    size_t nrNonuniqueKeys = jacobianKeys.size();
+    size_t nrUniqueKeys = hessianKeys.size();
+
+    // Marginalize point: note - we reuse the standard SchurComplement function.
+    SymmetricBlockMatrix augmentedHessian = SchurComplement<N, ND>(Fs, E, P, b);
+
+    // Pack into an Hessian factor, allow space for b term.
+    std::vector<DenseIndex> dims(nrUniqueKeys + 1);
+    std::fill(dims.begin(), dims.end() - 1, NDD);
+    dims.back() = 1;
+    SymmetricBlockMatrix augmentedHessianUniqueKeys;
+
+    // Deal with the fact that some blocks may share the same keys.
+    if (nrUniqueKeys == nrNonuniqueKeys) {
+      // Case when there is 1 calibration key per camera:
+      augmentedHessianUniqueKeys = SymmetricBlockMatrix(
+          dims, Matrix(augmentedHessian.selfadjointView()));
+    } else {
+      // When multiple cameras share a calibration we have to rearrange
+      // the results of the Schur complement matrix.
+      std::vector<DenseIndex> nonuniqueDims(nrNonuniqueKeys + 1);  // includes b
+      std::fill(nonuniqueDims.begin(), nonuniqueDims.end() - 1, NDD);
+      nonuniqueDims.back() = 1;
+      augmentedHessian = SymmetricBlockMatrix(
+          nonuniqueDims, Matrix(augmentedHessian.selfadjointView()));
+
+      // 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[hessianKeys[k]] = k;
+      }
 
-          // (DxD) = (Dx2) * ( (2x2) * (2xD) )
-          augmentedHessian.setOffDiagonalBlock(i, j, -FiT
-              * (Ei_P * E.block(ZDim * j, 0, ZDim, N).transpose() * Fj));
+      // Initialize matrix to zero.
+      augmentedHessianUniqueKeys = SymmetricBlockMatrix(
+          dims, Matrix::Zero(NDD * nrUniqueKeys + 1, NDD * 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 = jacobianKeys.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 = jacobianKeys.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());
+            }
+          }
         }
-      } // end of for over cameras
+      }
 
-      augmentedHessian.diagonalBlock(m)(0, 0) += b.squaredNorm();
-      return augmentedHessian;
+      // Update bottom right element of the matrix.
+      augmentedHessianUniqueKeys.updateDiagonalBlock(
+          nrUniqueKeys, augmentedHessian.diagonalBlock(nrNonuniqueKeys));
     }
+    return augmentedHessianUniqueKeys;
+  }
 
   /**
    * Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix
@@ -206,7 +304,7 @@ class CameraSet : public std::vector<CAMERA, Eigen::aligned_allocator<CAMERA> >
   }
 
   /// 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 +356,7 @@ class CameraSet : public std::vector<CAMERA, Eigen::aligned_allocator<CAMERA> >
    * 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/geometry/tests/testCameraSet.cpp

@@ -17,6 +17,7 @@
  */
 
 #include <gtsam/geometry/CameraSet.h>
+#include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <CppUnitLite/TestHarness.h>
@@ -125,6 +126,89 @@ TEST(CameraSet, Pinhole) {
   EXPECT(assert_equal(actualE, E));
 }
 
+/* ************************************************************************* */
+TEST(CameraSet, SchurComplementAndRearrangeBlocks) {
+  typedef PinholePose<Cal3Bundler> Camera;
+  typedef CameraSet<Camera> Set;
+
+  // this is the (block) Jacobian with respect to the nonuniqueKeys
+  std::vector<Eigen::Matrix<double, 2, 12>,
+      Eigen::aligned_allocator<Eigen::Matrix<double, 2, 12> > > Fs;
+  Fs.push_back(1 * Matrix::Ones(2, 12));  // corresponding to key pair (0,1)
+  Fs.push_back(2 * Matrix::Ones(2, 12));  // corresponding to key pair (1,2)
+  Fs.push_back(3 * Matrix::Ones(2, 12));  // corresponding to key pair (2,0)
+  Matrix E = 4 * Matrix::Identity(6, 3) + Matrix::Ones(6, 3);
+  E(0, 0) = 3;
+  E(1, 1) = 2;
+  E(2, 2) = 5;
+  Matrix Et = E.transpose();
+  Matrix P = (Et * E).inverse();
+  Vector b = 5 * Vector::Ones(6);
+
+  {  // SchurComplement
+     // Actual
+    SymmetricBlockMatrix augmentedHessianBM = Set::SchurComplement<3, 12>(Fs, E,
+                                                                          P, b);
+    Matrix actualAugmentedHessian = augmentedHessianBM.selfadjointView();
+
+    // Expected
+    Matrix F = Matrix::Zero(6, 3 * 12);
+    F.block<2, 12>(0, 0) = 1 * Matrix::Ones(2, 12);
+    F.block<2, 12>(2, 12) = 2 * Matrix::Ones(2, 12);
+    F.block<2, 12>(4, 24) = 3 * Matrix::Ones(2, 12);
+
+    Matrix Ft = F.transpose();
+    Matrix H = Ft * F - Ft * E * P * Et * F;
+    Vector v = Ft * (b - E * P * Et * b);
+    Matrix expectedAugmentedHessian = Matrix::Zero(3 * 12 + 1, 3 * 12 + 1);
+    expectedAugmentedHessian.block<36, 36>(0, 0) = H;
+    expectedAugmentedHessian.block<36, 1>(0, 36) = v;
+    expectedAugmentedHessian.block<1, 36>(36, 0) = v.transpose();
+    expectedAugmentedHessian(36, 36) = b.squaredNorm();
+
+    EXPECT(assert_equal(expectedAugmentedHessian, actualAugmentedHessian));
+  }
+
+  {  // SchurComplementAndRearrangeBlocks
+    KeyVector nonuniqueKeys;
+    nonuniqueKeys.push_back(0);
+    nonuniqueKeys.push_back(1);
+    nonuniqueKeys.push_back(1);
+    nonuniqueKeys.push_back(2);
+    nonuniqueKeys.push_back(2);
+    nonuniqueKeys.push_back(0);
+
+    KeyVector uniqueKeys;
+    uniqueKeys.push_back(0);
+    uniqueKeys.push_back(1);
+    uniqueKeys.push_back(2);
+
+    // Actual
+    SymmetricBlockMatrix augmentedHessianBM =
+        Set::SchurComplementAndRearrangeBlocks<3, 12, 6>(
+            Fs, E, P, b, nonuniqueKeys, uniqueKeys);
+    Matrix actualAugmentedHessian = augmentedHessianBM.selfadjointView();
+
+    // Expected
+    // we first need to build the Jacobian F according to unique keys
+    Matrix F = Matrix::Zero(6, 18);
+    F.block<2, 6>(0, 0) = Fs[0].block<2, 6>(0, 0);
+    F.block<2, 6>(0, 6) = Fs[0].block<2, 6>(0, 6);
+    F.block<2, 6>(2, 6) = Fs[1].block<2, 6>(0, 0);
+    F.block<2, 6>(2, 12) = Fs[1].block<2, 6>(0, 6);
+    F.block<2, 6>(4, 12) = Fs[2].block<2, 6>(0, 0);
+    F.block<2, 6>(4, 0) = Fs[2].block<2, 6>(0, 6);
+
+    Matrix Ft = F.transpose();
+    Vector v = Ft * (b - E * P * Et * b);
+    Matrix H = Ft * F - Ft * E * P * Et * F;
+    Matrix expectedAugmentedHessian(19, 19);
+    expectedAugmentedHessian << H, v, v.transpose(), b.squaredNorm();
+
+    EXPECT(assert_equal(expectedAugmentedHessian, actualAugmentedHessian));
+  }
+}
+
 /* ************************************************************************* */
 #include <gtsam/geometry/StereoCamera.h>
 TEST(CameraSet, Stereo) {