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,11 +320,33 @@ 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 in [0, 1.5] (t>1 implies extrapolation), with optional jacobians.
  */
 template <typename T>
-T interpolate(const T& X, const T& Y, double t) {
-  assert(t >= 0 && t <= 1);
+T interpolate(const T& X, const T& Y, double t,
+              OptionalJacobian< traits<T>::dimension, traits<T>::dimension > Hx = boost::none,
+              OptionalJacobian< traits<T>::dimension, traits<T>::dimension > Hy = boost::none) {
+  assert(t >= 0.0 && t <= 1.5);
+  if (Hx || Hy) {
+    typedef Eigen::Matrix<double, traits<T>::dimension, traits<T>::dimension> Jacobian;
+    typename traits<T>::TangentVector log_Xinv_Y;
+    Jacobian Hx_tmp, Hy_tmp, H1, H2;
+
+    T Xinv_Y = traits<T>::Between(X, Y, Hx_tmp, Hy_tmp);
+    log_Xinv_Y = traits<T>::Logmap(Xinv_Y, H1);
+    Hx_tmp = H1 * Hx_tmp;
+    Hy_tmp = H1 * Hy_tmp;
+    Xinv_Y = traits<T>::Expmap(t * log_Xinv_Y, H1);
+    Hx_tmp = t * H1 * Hx_tmp;
+    Hy_tmp = t * H1 * Hy_tmp;
+    Xinv_Y = traits<T>::Compose(X, Xinv_Y, H1, H2);
+    Hx_tmp = H1 + H2 * Hx_tmp;
+    Hy_tmp = H2 * Hy_tmp;
+
+    if(Hx) *Hx = Hx_tmp;
+    if(Hy) *Hy = Hy_tmp;
+    return Xinv_Y;
+  }
   return traits<T>::Compose(X, traits<T>::Expmap(t * traits<T>::Logmap(traits<T>::Between(X, Y))));
 }
 

gtsam/geometry/CameraSet.h

@@ -148,7 +148,7 @@ class CameraSet : public std::vector<CAMERA, Eigen::aligned_allocator<CAMERA> >
      * 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(
         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) {
@@ -193,6 +193,105 @@ class CameraSet : public std::vector<CAMERA, Eigen::aligned_allocator<CAMERA> >
       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)
+   * In this version, we allow for the case where the keys in the Jacobian are organized
+   * differents from the keys in the output SymmetricBlockMatrix
+   * In particular: each block of the Jacobian captures 2 poses (useful for rolling shutter and extrinsic calibration)
+   */
+  template<int N, int ND, int NDD>  // N = 2 or 3 (point dimension), ND is the Jacobian block dimension, NDD is the Hessian block dimension
+  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);
+
+    // now pack into an Hessian factor
+    std::vector<DenseIndex> dims(nrUniqueKeys + 1);  // this also includes the b term
+    std::fill(dims.begin(), dims.end() - 1, NDD);
+    dims.back() = 1;
+    SymmetricBlockMatrix augmentedHessianUniqueKeys;
+
+    // here we have to deal with the fact that some blocks may share the same keys
+    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, 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;
+      }
+
+      // 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());
+            }
+          }
+        }
+      }
+      // update bottom right element of the matrix
+      augmentedHessianUniqueKeys.updateDiagonalBlock(
+          nrUniqueKeys, augmentedHessian.diagonalBlock(nrNonuniqueKeys));
+    }
+    return augmentedHessianUniqueKeys;
+  }
+
+  /**
+   * non-templated version of function above
+   */
+  static SymmetricBlockMatrix SchurComplementAndRearrangeBlocks_3_12_6(
+      const std::vector<Eigen::Matrix<double,ZDim, 12>,
+          Eigen::aligned_allocator<Eigen::Matrix<double,ZDim,12> > >& Fs,
+      const Matrix& E, const Eigen::Matrix<double,3,3>& P, const Vector& b,
+      const KeyVector jacobianKeys, const KeyVector hessianKeys) {
+    return SchurComplementAndRearrangeBlocks<3,12,6>(Fs, E, P, b,
+                                                       jacobianKeys,
+                                                       hessianKeys);
+  }
+
   /**
    * Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix
    * G = F' * F - F' * E * P * E' * F
@@ -206,7 +305,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 +357,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,90 @@ 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) {

gtsam/geometry/tests/testPose3.cpp

@@ -1046,6 +1046,68 @@ TEST(Pose3, interpolate) {
   EXPECT(assert_equal(expected2, T2.interpolateRt(T3, t)));
 }
 
+/* ************************************************************************* */
+Pose3 testing_interpolate(const Pose3& t1, const Pose3& t2, double gamma) { return interpolate(t1,t2,gamma); }
+
+TEST(Pose3, interpolateJacobians) {
+  {
+    Pose3 X = Pose3::identity();
+    Pose3 Y(Rot3::Rz(M_PI_2), Point3(1, 0, 0));
+    double t = 0.5;
+    Pose3 expectedPoseInterp(Rot3::Rz(M_PI_4), Point3(0.5, -0.207107, 0)); // note: different from test above: this is full Pose3 interpolation
+    Matrix actualJacobianX, actualJacobianY;
+    EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY), 1e-5));
+
+    Matrix expectedJacobianX = numericalDerivative31<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
+    EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6));
+
+    Matrix expectedJacobianY = numericalDerivative32<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
+    EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6));
+  }
+  {
+    Pose3 X = Pose3::identity();
+    Pose3 Y(Rot3::identity(), Point3(1, 0, 0));
+    double t = 0.3;
+    Pose3 expectedPoseInterp(Rot3::identity(), Point3(0.3, 0, 0));
+    Matrix actualJacobianX, actualJacobianY;
+    EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY), 1e-5));
+
+    Matrix expectedJacobianX = numericalDerivative31<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
+    EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6));
+
+    Matrix expectedJacobianY = numericalDerivative32<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
+    EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6));
+  }
+  {
+    Pose3 X = Pose3::identity();
+    Pose3 Y(Rot3::Rz(M_PI_2), Point3(0, 0, 0));
+    double t = 0.5;
+    Pose3 expectedPoseInterp(Rot3::Rz(M_PI_4), Point3(0, 0, 0));
+    Matrix actualJacobianX, actualJacobianY;
+    EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY), 1e-5));
+
+    Matrix expectedJacobianX = numericalDerivative31<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
+    EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6));
+
+    Matrix expectedJacobianY = numericalDerivative32<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
+    EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6));
+  }
+  {
+    Pose3 X(Rot3::Ypr(0.1,0.2,0.3), Point3(10, 5, -2));
+    Pose3 Y(Rot3::Ypr(1.1,-2.2,-0.3), Point3(-5, 1, 1));
+    double t = 0.3;
+    Pose3 expectedPoseInterp(Rot3::Rz(M_PI_4), Point3(0, 0, 0));
+    Matrix actualJacobianX, actualJacobianY;
+    interpolate(X, Y, t, actualJacobianX, actualJacobianY);
+
+    Matrix expectedJacobianX = numericalDerivative31<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
+    EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6));
+
+    Matrix expectedJacobianY = numericalDerivative32<Pose3,Pose3,Pose3,double>(testing_interpolate, X, Y, t);
+    EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6));
+  }
+}
+
 /* ************************************************************************* */
 TEST(Pose3, Create) {
   Matrix63 actualH1, actualH2;

gtsam/slam/SmartFactorBase.h

@@ -178,7 +178,7 @@ class SmartFactorBase: public NonlinearFactor {
       DefaultKeyFormatter) const override {
     std::cout << s << "SmartFactorBase, z = \n";
     for (size_t k = 0; k < measured_.size(); ++k) {
-      std::cout << "measurement, p = " << measured_[k] << "\t";
+      std::cout << "measurement " << k<<", px = \n" << measured_[k] << "\n";
       noiseModel_->print("noise model = ");
     }
     if(body_P_sensor_)

gtsam/slam/SmartProjectionFactor.h

@@ -101,7 +101,7 @@ class SmartProjectionFactor: public SmartFactorBase<CAMERA> {
   void print(const std::string& s = "", const KeyFormatter& keyFormatter =
       DefaultKeyFormatter) const override {
     std::cout << s << "SmartProjectionFactor\n";
-    std::cout << "linearizationMode:\n" << params_.linearizationMode
+    std::cout << "linearizationMode: " << params_.linearizationMode
         << std::endl;
     std::cout << "triangulationParameters:\n" << params_.triangulation
         << std::endl;

gtsam/slam/tests/testSmartProjectionPoseFactor.cpp

@@ -50,7 +50,7 @@ static Point2 measurement1(323.0, 240.0);
 
 LevenbergMarquardtParams lmParams;
 // Make more verbose like so (in tests):
-// params.verbosityLM = LevenbergMarquardtParams::SUMMARY;
+// lmParams.verbosityLM = LevenbergMarquardtParams::SUMMARY;
 
 /* ************************************************************************* */
 TEST( SmartProjectionPoseFactor, Constructor) {