smart factors with extrinsics calibration

gtsam/geometry/CameraSet.h

@@ -139,6 +139,57 @@ class CameraSet : public std::vector<CAMERA, Eigen::aligned_allocator<CAMERA> >
     return ErrorVector(project2(point, Fs, E), measured);
   }
 
+  /**
+     * 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
+    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
+
+      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
@@ -148,45 +199,7 @@ class CameraSet : public std::vector<CAMERA, Eigen::aligned_allocator<CAMERA> >
   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) {
-
-    // a single point is observed in m cameras
-    size_t m = Fs.size();
-
-    // Create a SymmetricBlockMatrix
-    size_t M1 = D * m + 1;
-    std::vector<DenseIndex> dims(m + 1); // this also includes the b term
-    std::fill(dims.begin(), dims.end() - 1, D);
-    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 MatrixZD& 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 MatrixZD& 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
-
-    augmentedHessian.diagonalBlock(m)(0, 0) += b.squaredNorm();
-    return augmentedHessian;
+    return SchurComplement<N,D>(Fs, E, P, b);
   }
 
   /// Computes Point Covariance P, with lambda parameter

gtsam_unstable/slam/SmartStereoProjectionFactor.h

@@ -450,8 +450,8 @@ class SmartStereoProjectionFactor: public SmartFactorBase<StereoCamera> {
    * This corrects the Jacobians and error vector for the case in which the right pixel in the monocular camera is missing (nan)
    */
   void correctForMissingMeasurements(const Cameras& cameras, Vector& ue,
-      boost::optional<typename Cameras::FBlocks&> Fs = boost::none,
-      boost::optional<Matrix&> E = boost::none) const override
+                                     boost::optional<typename Cameras::FBlocks&> Fs = boost::none,
+                                     boost::optional<Matrix&> E = boost::none) const override
   {
     // when using stereo cameras, some of the measurements might be missing:
     for(size_t i=0; i < cameras.size(); i++){

gtsam_unstable/slam/SmartStereoProjectionFactorPP.cpp

@@ -0,0 +1,125 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file   SmartStereoProjectionFactorPP.h
+ * @brief  Smart stereo factor on poses and extrinsic calibration
+ * @author Luca Carlone
+ * @author Frank Dellaert
+ */
+
+#include <gtsam_unstable/slam/SmartStereoProjectionFactorPP.h>
+
+namespace gtsam {
+
+SmartStereoProjectionFactorPP::SmartStereoProjectionFactorPP(
+    const SharedNoiseModel& sharedNoiseModel,
+    const SmartStereoProjectionParams& params)
+    : Base(sharedNoiseModel, params) {} // note: no extrinsic specified!
+
+void SmartStereoProjectionFactorPP::add(
+    const StereoPoint2& measured,
+    const Key& w_P_body_key, const Key& body_P_cam_key,
+    const boost::shared_ptr<Cal3_S2Stereo>& K) {
+  // we index by cameras..
+  Base::add(measured, w_P_body_key);
+  // but we also store the extrinsic calibration keys in the same order
+  world_P_body_keys_.push_back(w_P_body_key);
+  body_P_cam_keys_.push_back(body_P_cam_key);
+
+  // pose keys are assumed to be unique (1 observation per time stamp), but calibration can be shared
+  if(std::find(keys_.begin(), keys_.end(), body_P_cam_key) == keys_.end())
+      keys_.push_back(body_P_cam_key); // add only unique keys
+
+  K_all_.push_back(K);
+}
+
+void SmartStereoProjectionFactorPP::add(
+    const std::vector<StereoPoint2>& measurements,
+    const KeyVector& world_P_body_keys, const KeyVector& body_P_cam_keys,
+    const std::vector<boost::shared_ptr<Cal3_S2Stereo>>& Ks) {
+  assert(world_P_body_keys.size() == measurements.size());
+  assert(world_P_body_keys.size() == body_P_cam_keys.size());
+  assert(world_P_body_keys.size() == Ks.size());
+  for (size_t i = 0; i < measurements.size(); i++) {
+      Base::add(measurements[i], world_P_body_keys[i]);
+      // pose keys are assumed to be unique (1 observation per time stamp), but calibration can be shared
+      if(std::find(keys_.begin(), keys_.end(), body_P_cam_keys[i]) == keys_.end())
+          keys_.push_back(body_P_cam_keys[i]); // add only unique keys
+
+      world_P_body_keys_.push_back(world_P_body_keys[i]);
+      body_P_cam_keys_.push_back(body_P_cam_keys[i]);
+
+      K_all_.push_back(Ks[i]);
+    }
+}
+
+void SmartStereoProjectionFactorPP::add(
+    const std::vector<StereoPoint2>& measurements,
+    const KeyVector& world_P_body_keys, const KeyVector& body_P_cam_keys,
+    const boost::shared_ptr<Cal3_S2Stereo>& K) {
+  assert(world_P_body_keys.size() == measurements.size());
+  assert(world_P_body_keys.size() == body_P_cam_keys.size());
+  for (size_t i = 0; i < measurements.size(); i++) {
+    Base::add(measurements[i], world_P_body_keys[i]);
+    // pose keys are assumed to be unique (1 observation per time stamp), but calibration can be shared
+    if(std::find(keys_.begin(), keys_.end(), body_P_cam_keys[i]) == keys_.end())
+      keys_.push_back(body_P_cam_keys[i]); // add only unique keys
+
+    world_P_body_keys_.push_back(world_P_body_keys[i]);
+    body_P_cam_keys_.push_back(body_P_cam_keys[i]);
+
+    K_all_.push_back(K);
+  }
+}
+
+void SmartStereoProjectionFactorPP::print(
+    const std::string& s, const KeyFormatter& keyFormatter) const {
+  std::cout << s << "SmartStereoProjectionFactorPP: \n ";
+  for (size_t i = 0; i < K_all_.size(); i++) {
+    K_all_[i]->print("calibration = ");
+    std::cout << " extrinsic pose key: " << keyFormatter(body_P_cam_keys_[i]) << std::endl;
+  }
+  Base::print("", keyFormatter);
+}
+
+bool SmartStereoProjectionFactorPP::equals(const NonlinearFactor& p,
+                                             double tol) const {
+  const SmartStereoProjectionFactorPP* e =
+      dynamic_cast<const SmartStereoProjectionFactorPP*>(&p);
+
+  return e && Base::equals(p, tol) &&
+      body_P_cam_keys_ == e->getExtrinsicPoseKeys();
+}
+
+double SmartStereoProjectionFactorPP::error(const Values& values) const {
+  if (this->active(values)) {
+    return this->totalReprojectionError(cameras(values));
+  } else {  // else of active flag
+    return 0.0;
+  }
+}
+
+SmartStereoProjectionFactorPP::Base::Cameras
+SmartStereoProjectionFactorPP::cameras(const Values& values) const {
+  assert(world_P_body_keys_.size() == K_all_.size());
+  assert(world_P_body_keys_.size() == body_P_cam_keys_.size());
+  Base::Cameras cameras;
+  for (size_t i = 0; i < world_P_body_keys_.size(); i++) {
+    Pose3 w_P_body = values.at<Pose3>(world_P_body_keys_[i]);
+    Pose3 body_P_cam = values.at<Pose3>(body_P_cam_keys_[i]);
+    Pose3 w_P_cam = w_P_body.compose(body_P_cam);
+    cameras.push_back(StereoCamera(w_P_cam, K_all_[i]));
+  }
+  return cameras;
+}
+
+}  // \ namespace gtsam