Merge pull request #1896 from borglab/feature/moreComparisons

More comparisons

gtsam/geometry/FundamentalMatrix.cpp

@@ -74,6 +74,20 @@ Matrix3 FundamentalMatrix::matrix() const {
          V_.transpose().matrix();
 }
 
+Vector3 FundamentalMatrix::epipolarLine(const Point2& p,
+                                        OptionalJacobian<3, 7> H) {
+  Vector3 point(p.x(), p.y(), 1);  // Create a point in homogeneous coordinates
+  Vector3 line = matrix() * point;  // Compute the epipolar line
+
+  if (H) {
+    // Compute the Jacobian if requested
+    throw std::runtime_error(
+        "FundamentalMatrix::epipolarLine: Jacobian not implemented yet.");
+  }
+
+  return line;  // Return the epipolar line
+}
+
 void FundamentalMatrix::print(const std::string& s) const {
   std::cout << s << matrix() << std::endl;
 }
@@ -116,6 +130,20 @@ Matrix3 SimpleFundamentalMatrix::matrix() const {
   return Ka().transpose().inverse() * E_.matrix() * Kb().inverse();
 }
 
+Vector3 SimpleFundamentalMatrix::epipolarLine(const Point2& p,
+                                              OptionalJacobian<3, 7> H) {
+  Vector3 point(p.x(), p.y(), 1);  // Create a point in homogeneous coordinates
+  Vector3 line = matrix() * point;  // Compute the epipolar line
+
+  if (H) {
+    // Compute the Jacobian if requested
+    throw std::runtime_error(
+        "SimpleFundamentalMatrix::epipolarLine: Jacobian not implemented yet.");
+  }
+
+  return line;  // Return the epipolar line
+}
+
 void SimpleFundamentalMatrix::print(const std::string& s) const {
   std::cout << s << " E:\n"
             << E_.matrix() << "\nfa: " << fa_ << "\nfb: " << fb_

gtsam/geometry/FundamentalMatrix.h

@@ -7,6 +7,7 @@
 
 #pragma once
 
+#include <gtsam/base/OptionalJacobian.h>
 #include <gtsam/geometry/EssentialMatrix.h>
 #include <gtsam/geometry/Rot3.h>
 #include <gtsam/geometry/Unit3.h>
@@ -86,6 +87,9 @@ class GTSAM_EXPORT FundamentalMatrix {
   /// Return the fundamental matrix representation
   Matrix3 matrix() const;
 
+  /// Computes the epipolar line in a (left) for a given point in b (right)
+  Vector3 epipolarLine(const Point2& p, OptionalJacobian<3, 7> H = {});
+
   /// @name Testable
   /// @{
   /// Print the FundamentalMatrix
@@ -161,6 +165,9 @@ class GTSAM_EXPORT SimpleFundamentalMatrix {
   /// F = Ka^(-T) * E * Kb^(-1)
   Matrix3 matrix() const;
 
+  /// Computes the epipolar line in a (left) for a given point in b (right)
+  Vector3 epipolarLine(const Point2& p, OptionalJacobian<3, 7> H = {});
+
   /// @name Testable
   /// @{
   /// Print the SimpleFundamentalMatrix

gtsam/sfm/TransferFactor.h

@@ -236,6 +236,8 @@ class EssentialTransferFactorK
   /**
    * @brief Constructor that accepts a vector of point triplets.
    *
+   * @note Calibrations are assumed all different, keys are derived from edges.
+   *
    * @param edge1 First EdgeKey specifying E1: (a, c) or (c, a)
    * @param edge2 Second EdgeKey specifying E2: (b, c) or (c, b)
    * @param triplets A vector of triplets containing (pa, pb, pc)
@@ -251,6 +253,24 @@ class EssentialTransferFactorK
         TransferEdges<EM>(edge1, edge2),
         triplets_(triplets) {}
 
+  /**
+   * @brief Constructor that accepts a vector of point triplets.
+   *
+   * @note Calibrations are assumed all same, using given key `keyK`.
+   *
+   * @param edge1 First EdgeKey specifying E1: (a, c) or (c, a)
+   * @param edge2 Second EdgeKey specifying E2: (b, c) or (c, b)
+   * @param keyK Calibration key for all views.
+   * @param triplets A vector of triplets containing (pa, pb, pc)
+   * @param model An optional SharedNoiseModel
+   */
+  EssentialTransferFactorK(EdgeKey edge1, EdgeKey edge2, Key keyK,
+                           const std::vector<Triplet>& triplets,
+                           const SharedNoiseModel& model = nullptr)
+      : Base(model, edge1, edge2, keyK, keyK, keyK),
+        TransferEdges<EM>(edge1, edge2),
+        triplets_(triplets) {}
+
   /// Transfer points pa and pb to view c and evaluate error.
   Vector2 TransferError(const Matrix3& Eca, const K& Ka, const Point2& pa,
                         const Matrix3& Ecb, const K& Kb, const Point2& pb,

gtsam/sfm/sfm.i

@@ -98,8 +98,11 @@ virtual class EssentialTransferFactor : gtsam::NoiseModelFactor {
 template <K = {gtsam::Cal3_S2, gtsam::Cal3f, gtsam::Cal3Bundler}>
 virtual class EssentialTransferFactorK : gtsam::NoiseModelFactor {
   EssentialTransferFactorK(gtsam::EdgeKey edge1, gtsam::EdgeKey edge2,
-                          const std::vector<std::tuple<gtsam::Point2, gtsam::Point2, gtsam::Point2>>& triplets,
-                          const gtsam::noiseModel::Base* model = nullptr);
+                           const std::vector<std::tuple<gtsam::Point2, gtsam::Point2, gtsam::Point2>>& triplets,
+                           const gtsam::noiseModel::Base* model = nullptr);
+  EssentialTransferFactorK(gtsam::EdgeKey edge1, gtsam::EdgeKey edge2, size_t keyK,
+                           const std::vector<std::tuple<gtsam::Point2, gtsam::Point2, gtsam::Point2>>& triplets,
+                           const gtsam::noiseModel::Base* model = nullptr);
 };
 
 #include <gtsam/sfm/ShonanFactor.h>

gtsam/sfm/tests/testTransferFactor.cpp

@@ -154,7 +154,7 @@ TEST(TransferFactor, MultipleTuples) {
 }
 
 //*************************************************************************
-// Test for EssentialTransferFactor
+// Test for EssentialTransferFactorK
 TEST(EssentialTransferFactor, Jacobians) {
   using namespace fixture;
 

gtsam/slam/EssentialMatrixFactor.h

@@ -38,7 +38,6 @@ class EssentialMatrixFactor : public NoiseModelFactorN<EssentialMatrix> {
   typedef EssentialMatrixFactor This;
 
  public:
-
   // Provide access to the Matrix& version of evaluateError:
   using Base::evaluateError;
 
@@ -93,8 +92,8 @@ class EssentialMatrixFactor : public NoiseModelFactorN<EssentialMatrix> {
   }
 
   /// vector of errors returns 1D vector
-  Vector evaluateError(
-      const EssentialMatrix& E, OptionalMatrixType H) const override {
+  Vector evaluateError(const EssentialMatrix& E,
+                       OptionalMatrixType H) const override {
     Vector error(1);
     error << E.error(vA_, vB_, H);
     return error;
@@ -118,7 +117,6 @@ class EssentialMatrixFactor2
   typedef EssentialMatrixFactor2 This;
 
  public:
-
   // Provide access to the Matrix& version of evaluateError:
   using Base::evaluateError;
 
@@ -178,9 +176,9 @@ class EssentialMatrixFactor2
    * @param E essential matrix
    * @param d inverse depth d
    */
-  Vector evaluateError(
-      const EssentialMatrix& E, const double& d,
-      OptionalMatrixType DE, OptionalMatrixType Dd) const override {
+  Vector evaluateError(const EssentialMatrix& E, const double& d,
+                       OptionalMatrixType DE,
+                       OptionalMatrixType Dd) const override {
     // We have point x,y in image 1
     // Given a depth Z, the corresponding 3D point P1 = Z*(x,y,1) = (x,y,1)/d
     // We then convert to second camera by P2 = 1R2'*(P1-1T2)
@@ -241,7 +239,6 @@ class EssentialMatrixFactor3 : public EssentialMatrixFactor2 {
   Rot3 cRb_;  ///< Rotation from body to camera frame
 
  public:
-
   // Provide access to the Matrix& version of evaluateError:
   using Base::evaluateError;
 
@@ -292,9 +289,9 @@ class EssentialMatrixFactor3 : public EssentialMatrixFactor2 {
    * @param E essential matrix
    * @param d inverse depth d
    */
-  Vector evaluateError(
-      const EssentialMatrix& E, const double& d,
-      OptionalMatrixType DE, OptionalMatrixType Dd) const override {
+  Vector evaluateError(const EssentialMatrix& E, const double& d,
+                       OptionalMatrixType DE,
+                       OptionalMatrixType Dd) const override {
     if (!DE) {
       // Convert E from body to camera frame
       EssentialMatrix cameraE = cRb_ * E;
@@ -304,8 +301,9 @@ class EssentialMatrixFactor3 : public EssentialMatrixFactor2 {
       // Version with derivatives
       Matrix D_e_cameraE, D_cameraE_E;  // 2*5, 5*5
       EssentialMatrix cameraE = E.rotate(cRb_, D_cameraE_E);
-      // Using the pointer version of evaluateError since the Base class (EssentialMatrixFactor2)
-      // does not have the matrix reference version of evaluateError
+      // Using the pointer version of evaluateError since the Base class
+      // (EssentialMatrixFactor2) does not have the matrix reference version of
+      // evaluateError
       Vector e = Base::evaluateError(cameraE, d, &D_e_cameraE, Dd);
       *DE = D_e_cameraE * D_cameraE_E;  // (2*5) * (5*5)
       return e;
@@ -327,7 +325,7 @@ class EssentialMatrixFactor3 : public EssentialMatrixFactor2 {
  * Even with a prior, we can only optimize 2 DoF in the calibration. So the
  * prior should have a noise model with very low sigma in the remaining
  * dimensions. This has been tested to work on Cal3_S2. With Cal3Bundler, it
- * works only with a strong prior (low sigma noisemodel) on all degrees of
+ * works only with a strong prior (low sigma noise model) on all degrees of
  * freedom.
  */
 template <class CALIBRATION>
@@ -343,21 +341,20 @@ class EssentialMatrixFactor4
   typedef Eigen::Matrix<double, 2, DimK> JacobianCalibration;
 
  public:
-
-   // Provide access to the Matrix& version of evaluateError:
+  // Provide access to the Matrix& version of evaluateError:
   using Base::evaluateError;
 
   /**
    *  Constructor
-   *  @param keyE Essential Matrix (from camera B to A) variable key
+   *  @param keyE Essential Matrix aEb variable key
    *  @param keyK Calibration variable key (common for both cameras)
    *  @param pA point in first camera, in pixel coordinates
    *  @param pB point in second camera, in pixel coordinates
    *  @param model noise model is about dot product in ideal, homogeneous
    * coordinates
    */
   EssentialMatrixFactor4(Key keyE, Key keyK, const Point2& pA, const Point2& pB,
-                         const SharedNoiseModel& model)
+                         const SharedNoiseModel& model = nullptr)
       : Base(model, keyE, keyK), pA_(pA), pB_(pB) {}
 
   /// @return a deep copy of this factor
@@ -385,32 +382,32 @@ class EssentialMatrixFactor4
    * @param H2 optional jacobian of error w.r.t K
    * @return * Vector 1D vector of algebraic error
    */
-  Vector evaluateError(
-      const EssentialMatrix& E, const CALIBRATION& K,
-      OptionalMatrixType H1, OptionalMatrixType H2) const override {
+  Vector evaluateError(const EssentialMatrix& E, const CALIBRATION& K,
+                       OptionalMatrixType HE,
+                       OptionalMatrixType HK) const override {
     // converting from pixel coordinates to normalized coordinates cA and cB
     JacobianCalibration cA_H_K;  // dcA/dK
     JacobianCalibration cB_H_K;  // dcB/dK
-    Point2 cA = K.calibrate(pA_, H2 ? &cA_H_K : 0, OptionalNone);
-    Point2 cB = K.calibrate(pB_, H2 ? &cB_H_K : 0, OptionalNone);
+    Point2 cA = K.calibrate(pA_, HK ? &cA_H_K : 0, OptionalNone);
+    Point2 cB = K.calibrate(pB_, HK ? &cB_H_K : 0, OptionalNone);
 
     // convert to homogeneous coordinates
     Vector3 vA = EssentialMatrix::Homogeneous(cA);
     Vector3 vB = EssentialMatrix::Homogeneous(cB);
 
-    if (H2) {
+    if (HK) {
       // compute the jacobian of error w.r.t K
 
       // error function f = vA.T * E * vB
       // H2 = df/dK = vB.T * E.T * dvA/dK + vA.T * E * dvB/dK
       // where dvA/dK = dvA/dcA * dcA/dK, dVB/dK = dvB/dcB * dcB/dK
       // and dvA/dcA = dvB/dcB = [[1, 0], [0, 1], [0, 0]]
-      *H2 = vB.transpose() * E.matrix().transpose().leftCols<2>() * cA_H_K +
+      *HK = vB.transpose() * E.matrix().transpose().leftCols<2>() * cA_H_K +
             vA.transpose() * E.matrix().leftCols<2>() * cB_H_K;
     }
 
     Vector error(1);
-    error << E.error(vA, vB, H1);
+    error << E.error(vA, vB, HE);
 
     return error;
   }
@@ -420,4 +417,108 @@ class EssentialMatrixFactor4
 };
 // EssentialMatrixFactor4
 
+/**
+ * Binary factor that optimizes for E and two calibrations Ka and Kb using the
+ * algebraic epipolar error (Ka^-1 pA)'E (Kb^-1 pB). The calibrations are
+ * assumed different for the two images, but if you use the same key for Ka and
+ * Kb, the sum of the two K Jacobians equals that of the K Jacobian for
+ * EssentialMatrix4. If you know there is a single global calibration, use
+ * that factor instead.
+ *
+ * Note: see the comment about priors from EssentialMatrixFactor4: even stronger
+ * caveats about having priors on calibration apply here.
+ */
+template <class CALIBRATION>
+class EssentialMatrixFactor5
+    : public NoiseModelFactorN<EssentialMatrix, CALIBRATION, CALIBRATION> {
+ private:
+  Point2 pA_, pB_;  ///< points in pixel coordinates
+
+  typedef NoiseModelFactorN<EssentialMatrix, CALIBRATION, CALIBRATION> Base;
+  typedef EssentialMatrixFactor5 This;
+
+  static constexpr int DimK = FixedDimension<CALIBRATION>::value;
+  typedef Eigen::Matrix<double, 2, DimK> JacobianCalibration;
+
+ public:
+  // Provide access to the Matrix& version of evaluateError:
+  using Base::evaluateError;
+
+  /**
+   * Constructor
+   * @param keyE Essential Matrix aEb variable key
+   * @param keyKa Calibration variable key for camera A
+   * @param keyKb Calibration variable key for camera B
+   * @param pA point in first camera, in pixel coordinates
+   * @param pB point in second camera, in pixel coordinates
+   * @param model noise model is about dot product in ideal, homogeneous
+   * coordinates
+   */
+  EssentialMatrixFactor5(Key keyE, Key keyKa, Key keyKb, const Point2& pA,
+                         const Point2& pB,
+                         const SharedNoiseModel& model = nullptr)
+      : Base(model, keyE, keyKa, keyKb), pA_(pA), pB_(pB) {}
+
+  /// @return a deep copy of this factor
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
+    return std::static_pointer_cast<gtsam::NonlinearFactor>(
+        gtsam::NonlinearFactor::shared_ptr(new This(*this)));
+  }
+
+  /// print
+  void print(
+      const std::string& s = "",
+      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
+    Base::print(s);
+    std::cout << "  EssentialMatrixFactor5 with measurements\n  ("
+              << pA_.transpose() << ")' and (" << pB_.transpose() << ")'"
+              << std::endl;
+  }
+
+  /**
+   * @brief Calculate the algebraic epipolar error pA' (Ka^-1)' E Kb pB.
+   *
+   * @param E essential matrix for key keyE
+   * @param Ka calibration for camera A for key keyKa
+   * @param Kb calibration for camera B for key keyKb
+   * @param H1 optional jacobian of error w.r.t E
+   * @param H2 optional jacobian of error w.r.t Ka
+   * @param H3 optional jacobian of error w.r.t Kb
+   * @return * Vector 1D vector of algebraic error
+   */
+  Vector evaluateError(const EssentialMatrix& E, const CALIBRATION& Ka,
+                       const CALIBRATION& Kb, OptionalMatrixType HE,
+                       OptionalMatrixType HKa,
+                       OptionalMatrixType HKb) const override {
+    // converting from pixel coordinates to normalized coordinates cA and cB
+    JacobianCalibration cA_H_Ka;  // dcA/dKa
+    JacobianCalibration cB_H_Kb;  // dcB/dKb
+    Point2 cA = Ka.calibrate(pA_, HKa ? &cA_H_Ka : 0, OptionalNone);
+    Point2 cB = Kb.calibrate(pB_, HKb ? &cB_H_Kb : 0, OptionalNone);
+
+    // Convert to homogeneous coordinates.
+    Vector3 vA = EssentialMatrix::Homogeneous(cA);
+    Vector3 vB = EssentialMatrix::Homogeneous(cB);
+
+    if (HKa) {
+      // Compute the jacobian of error w.r.t Ka.
+      *HKa = vB.transpose() * E.matrix().transpose().leftCols<2>() * cA_H_Ka;
+    }
+
+    if (HKb) {
+      // Compute the jacobian of error w.r.t Kb.
+      *HKb = vA.transpose() * E.matrix().leftCols<2>() * cB_H_Kb;
+    }
+
+    Vector error(1);
+    error << E.error(vA, vB, HE);
+
+    return error;
+  }
+
+ public:
+  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
+};
+// EssentialMatrixFactor5
+
 }  // namespace gtsam