Merge branch 'develop' of github.com:borglab/gtsam into feature/essen…

…tial-mat-with-approx-k

gtsam/geometry/Cal3Bundler.cpp

@@ -96,23 +96,27 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal,
                               OptionalJacobian<2, 2> Dp) const {
   // Copied from Cal3DS2
   // but specialized with k1, k2 non-zero only and fx=fy and s=0
-  double x = (pi.x() - u0_) / fx_, y = (pi.y() - v0_) / fx_;
-  const Point2 invKPi(x, y);
-
-  // initialize by ignoring the distortion at all, might be problematic for
-  // pixels around boundary
-  Point2 pn(x, y);
+  double px = (pi.x() - u0_) / fx_, py = (pi.y() - v0_) / fx_;
+  const Point2 invKPi(px, py);
+  Point2 pn;
 
   // iterate until the uncalibrate is close to the actual pixel coordinate
   const int maxIterations = 10;
-  int iteration;
-  for (iteration = 0; iteration < maxIterations; ++iteration) {
-    if (distance2(uncalibrate(pn), pi) <= tol_) break;
-    const double px = pn.x(), py = pn.y(), xx = px * px, yy = py * py;
-    const double rr = xx + yy;
+  int iteration = 0;
+  do {
+    // initialize pn with distortion included
+    const double rr = (px * px) + (py * py);
     const double g = (1 + k1_ * rr + k2_ * rr * rr);
     pn = invKPi / g;
-  }
+
+    if (distance2(uncalibrate(pn), pi) <= tol_) break;
+
+    // Set px and py using intrinsic coordinates since that is where radial
+    // distortion correction is done.
+    px = pn.x(), py = pn.y();
+    iteration++;
+
+  } while (iteration < maxIterations);
 
   if (iteration >= maxIterations)
     throw std::runtime_error(
@@ -148,4 +152,4 @@ Matrix25 Cal3Bundler::D2d_intrinsic_calibration(const Point2& p) const {
   return H;
 }
 
-}  // \ namespace gtsam
+}  // namespace gtsam

gtsam/geometry/CameraSet.h

@@ -69,9 +69,12 @@ class CameraSet : public std::vector<CAMERA, Eigen::aligned_allocator<CAMERA> >
 
 public:
 
+  /// Destructor
+  virtual ~CameraSet() = default;
+
   /// Definitions for blocks of F
-  typedef Eigen::Matrix<double, ZDim, D> MatrixZD;
-  typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > FBlocks;
+  using MatrixZD = Eigen::Matrix<double, ZDim, D>;
+  using FBlocks = std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD>>;
 
   /**
    * print
@@ -139,6 +142,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 +202,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/geometry/Point2.h

@@ -82,5 +82,18 @@ GTSAM_EXPORT std::list<Point2> circleCircleIntersection(Point2 c1, Point2 c2, bo
 GTSAM_EXPORT std::list<Point2> circleCircleIntersection(Point2 c1, double r1,
     Point2 c2, double r2, double tol = 1e-9);
 
+template <typename A1, typename A2>
+struct Range;
+
+template <>
+struct Range<Point2, Point2> {
+  typedef double result_type;
+  double operator()(const Point2& p, const Point2& q,
+                    OptionalJacobian<1, 2> H1 = boost::none,
+                    OptionalJacobian<1, 2> H2 = boost::none) {
+    return distance2(p, q, H1, H2);
+  }
+};
+
 } // \ namespace gtsam
 

gtsam/geometry/SO3.cpp

@@ -278,7 +278,8 @@ Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) {
     } else {
       // when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0)
       // use Taylor expansion: theta \approx 1/2-(t-3)/12 + O((t-3)^2)
-      magnitude = 0.5 - tr_3 * tr_3 / 12.0;
+      // see https://github.com/borglab/gtsam/issues/746 for details
+      magnitude = 0.5 - tr_3 / 12.0;
     }
     omega = magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12);
   }

gtsam/geometry/tests/testCal3Bundler.cpp

@@ -62,6 +62,60 @@ Point2 calibrate_(const Cal3Bundler& k, const Point2& pt) {
   return k.calibrate(pt);
 }
 
+/* ************************************************************************* */
+TEST(Cal3Bundler, DuncalibrateDefault) {
+  Cal3Bundler trueK(1, 0, 0);
+  Matrix Dcal, Dp;
+  Point2 actual = trueK.uncalibrate(p, Dcal, Dp);
+  Point2 expected = p;
+  CHECK(assert_equal(expected, actual, 1e-7));
+  Matrix numerical1 = numericalDerivative21(uncalibrate_, trueK, p);
+  Matrix numerical2 = numericalDerivative22(uncalibrate_, trueK, p);
+  CHECK(assert_equal(numerical1, Dcal, 1e-7));
+  CHECK(assert_equal(numerical2, Dp, 1e-7));
+}
+
+/* ************************************************************************* */
+TEST(Cal3Bundler, DcalibrateDefault) {
+  Cal3Bundler trueK(1, 0, 0);
+  Matrix Dcal, Dp;
+  Point2 pn(0.5, 0.5);
+  Point2 pi = trueK.uncalibrate(pn);
+  Point2 actual = trueK.calibrate(pi, Dcal, Dp);
+  CHECK(assert_equal(pn, actual, 1e-7));
+  Matrix numerical1 = numericalDerivative21(calibrate_, trueK, pi);
+  Matrix numerical2 = numericalDerivative22(calibrate_, trueK, pi);
+  CHECK(assert_equal(numerical1, Dcal, 1e-5));
+  CHECK(assert_equal(numerical2, Dp, 1e-5));
+}
+
+/* ************************************************************************* */
+TEST(Cal3Bundler, DuncalibratePrincipalPoint) {
+  Cal3Bundler K(5, 0, 0, 2, 2);
+  Matrix Dcal, Dp;
+  Point2 actual = K.uncalibrate(p, Dcal, Dp);
+  Point2 expected(12, 17);
+  CHECK(assert_equal(expected, actual, 1e-7));
+  Matrix numerical1 = numericalDerivative21(uncalibrate_, K, p);
+  Matrix numerical2 = numericalDerivative22(uncalibrate_, K, p);
+  CHECK(assert_equal(numerical1, Dcal, 1e-7));
+  CHECK(assert_equal(numerical2, Dp, 1e-7));
+}
+
+/* ************************************************************************* */
+TEST(Cal3Bundler, DcalibratePrincipalPoint) {
+  Cal3Bundler K(2, 0, 0, 2, 2);
+  Matrix Dcal, Dp;
+  Point2 pn(0.5, 0.5);
+  Point2 pi = K.uncalibrate(pn);
+  Point2 actual = K.calibrate(pi, Dcal, Dp);
+  CHECK(assert_equal(pn, actual, 1e-7));
+  Matrix numerical1 = numericalDerivative21(calibrate_, K, pi);
+  Matrix numerical2 = numericalDerivative22(calibrate_, K, pi);
+  CHECK(assert_equal(numerical1, Dcal, 1e-5));
+  CHECK(assert_equal(numerical2, Dp, 1e-5));
+}
+
 /* ************************************************************************* */
 TEST(Cal3Bundler, Duncalibrate) {
   Matrix Dcal, Dp;

gtsam/gtsam.i

@@ -2166,6 +2166,34 @@ virtual class NoiseModelFactor: gtsam::NonlinearFactor {
   Vector whitenedError(const gtsam::Values& x) const;
 };
 
+#include <gtsam/nonlinear/CustomFactor.h>
+virtual class CustomFactor: gtsam::NoiseModelFactor {
+  /*
+   * Note CustomFactor will not be wrapped for MATLAB, as there is no supporting machinery there.
+   * This is achieved by adding `gtsam::CustomFactor` to the ignore list in `matlab/CMakeLists.txt`.
+   */
+  CustomFactor();
+  /*
+   * Example:
+   * ```
+   * def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]):
+   *    <calculated error>
+   *    if not H is None:
+   *        <calculate the Jacobian>
+   *        H[0] = J1 # 2-d numpy array for a Jacobian block
+   *        H[1] = J2
+   *        ...
+   *    return error # 1-d numpy array
+   *
+   * cf = CustomFactor(noise_model, keys, error_func)
+   * ```
+   */
+  CustomFactor(const gtsam::SharedNoiseModel& noiseModel, const gtsam::KeyVector& keys,
+               const gtsam::CustomErrorFunction& errorFunction);
+
+  void print(string s = "", gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter);
+};
+
 #include <gtsam/nonlinear/Values.h>
 class Values {
   Values();

gtsam/nonlinear/CustomFactor.cpp

@@ -0,0 +1,76 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    CustomFactor.cpp
+ * @brief   Class to enable arbitrary factors with runtime swappable error function.
+ * @author  Fan Jiang
+ */
+
+#include <gtsam/nonlinear/CustomFactor.h>
+
+namespace gtsam {
+
+/*
+ * Calculates the unwhitened error by invoking the callback functor (i.e. from Python).
+ */
+Vector CustomFactor::unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H) const {
+  if(this->active(x)) {
+
+    if(H) {
+      /*
+       * In this case, we pass the raw pointer to the `std::vector<Matrix>` object directly to pybind.
+       * As the type `std::vector<Matrix>` has been marked as opaque in `preamble.h`, any changes in
+       * Python will be immediately reflected on the C++ side.
+       *
+       * Example:
+       * ```
+       * def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]):
+       *    <calculated error>
+       *    if not H is None:
+       *        <calculate the Jacobian>
+       *        H[0] = J1
+       *        H[1] = J2
+       *        ...
+       *    return error
+       * ```
+       */
+      return this->error_function_(*this, x, H.get_ptr());
+    } else {
+      /*
+       * In this case, we pass the a `nullptr` to pybind, and it will translate to `None` in Python.
+       * Users can check for `None` in their callback to determine if the Jacobian is requested.
+       */
+      return this->error_function_(*this, x, nullptr);
+    }
+  } else {
+    return Vector::Zero(this->dim());
+  }
+}
+
+void CustomFactor::print(const std::string &s, const KeyFormatter &keyFormatter) const {
+  std::cout << s << "CustomFactor on ";
+  auto keys_ = this->keys();
+  bool f = false;
+  for (const Key &k: keys_) {
+    if (f)
+      std::cout << ", ";
+    std::cout << keyFormatter(k);
+    f = true;
+  }
+  std::cout << "\n";
+  if (this->noiseModel_)
+    this->noiseModel_->print("  noise model: ");
+  else
+    std::cout << "no noise model" << std::endl;
+}
+
+}