Merge branch 'feature/sampson-epipolar-error' into feature/essential-…

…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/EssentialMatrix.cpp

@@ -101,16 +101,61 @@ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb,
 }
 
 /* ************************************************************************* */
-double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, //
-    OptionalJacobian<1, 5> H) const {
+double EssentialMatrix::error(const Vector3& vA, const Vector3& vB,  //
+                              OptionalJacobian<1, 5> H) const {
+  // compute the epipolar lines
+  Point3 lB = E_ * vB;
+  Point3 lA = E_.transpose() * vA;
+
+  // compute the algebraic error as a simple dot product.
+  double algebraic_error = dot(vA, lB);
+
+  // compute the line-norms for the two lines
+  Matrix23 I;
+  I.setIdentity();
+  Matrix21 nA = I * lA;
+  Matrix21 nB = I * lB;
+  double nA_sq_norm = nA.squaredNorm();
+  double nB_sq_norm = nB.squaredNorm();
+
+  // compute the normalizing denominator and finally the sampson error
+  double denominator = sqrt(nA_sq_norm + nB_sq_norm);
+  double sampson_error = algebraic_error / denominator;
+
   if (H) {
     // See math.lyx
-    Matrix13 HR = vA.transpose() * E_ * skewSymmetric(-vB);
-    Matrix12 HD = vA.transpose() * skewSymmetric(-rotation().matrix() * vB)
-        * direction().basis();
-    *H << HR, HD;
+    // computing the derivatives of the numerator w.r.t. E
+    Matrix13 numerator_H_R = vA.transpose() * E_ * skewSymmetric(-vB);
+    Matrix12 numerator_H_D = vA.transpose() *
+                             skewSymmetric(-rotation().matrix() * vB) *
+                             direction().basis();
+
+    // computing the derivatives of the denominator w.r.t. E
+    Matrix12 denominator_H_nA = nA.transpose() / denominator;
+    Matrix12 denominator_H_nB = nB.transpose() / denominator;
+    Matrix13 denominator_H_lA = denominator_H_nA * I;
+    Matrix13 denominator_H_lB = denominator_H_nB * I;
+    Matrix33 lB_H_R = E_ * skewSymmetric(-vB);
+    Matrix32 lB_H_D =
+        skewSymmetric(-rotation().matrix() * vB) * direction().basis();
+    Matrix33 lA_H_R = skewSymmetric(E_.matrix().transpose() * vA);
+    Matrix32 lA_H_D =
+        rotation().inverse().matrix() * skewSymmetric(vA) * direction().basis();
+
+    Matrix13 denominator_H_R =
+        denominator_H_lA * lA_H_R + denominator_H_lB * lB_H_R;
+    Matrix12 denominator_H_D =
+        denominator_H_lA * lA_H_D + denominator_H_lB * lB_H_D;
+
+    Matrix15 denominator_H;
+    denominator_H << denominator_H_R, denominator_H_D;
+    Matrix15 numerator_H;
+    numerator_H << numerator_H_R, numerator_H_D;
+
+    *H = numerator_H / denominator -
+         algebraic_error * denominator_H / (denominator * denominator);
   }
-  return dot(vA, E_ * vB);
+  return sampson_error;
 }
 
 /* ************************************************************************* */

gtsam/geometry/EssentialMatrix.h

@@ -159,7 +159,7 @@ class EssentialMatrix {
     return E.rotate(cRb);
   }
 
-  /// epipolar error, algebraic
+  /// epipolar error, sampson
   GTSAM_EXPORT double error(const Vector3& vA, const Vector3& vB,
       OptionalJacobian<1, 5> H = boost::none) const;
 

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;