address comments

gtsam/slam/EssentialMatrixFactor.h

@@ -420,10 +420,13 @@ class 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. Don'tt use this factor with same
+ * assumed different for the two images, but if you use the same key for Ka and
- * calibration unknown, as Jacobians will be incorrect...
+ * 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: even stronger caveats about having priors on calibration apply.
+ * Note: see the comment about priors from EssentialMatrixFactor4: even stronger
+ * caveats about having priors on calibration apply here.
  */
 template <class CALIBRATION>
 class EssentialMatrixFactor5
@@ -442,17 +445,18 @@ class EssentialMatrixFactor5
   using Base::evaluateError;
 
   /**
-   *  Constructor
+   * Constructor
-   *  @param keyE Essential Matrix aEb variable key
+   * @param keyE Essential Matrix aEb variable key
-   *  @param keyKa Calibration variable key for camera A
+   * @param keyKa Calibration variable key for camera A
-   *  @param keyKb Calibration variable key for camera B
+   * @param keyKb Calibration variable key for camera B
-   *  @param pA point in first camera, in pixel coordinates
+   * @param pA point in first camera, in pixel coordinates
-   *  @param pB point in second camera, in pixel coordinates
+   * @param pB point in second camera, in pixel coordinates
-   *  @param model noise model is about dot product in ideal, homogeneous
+   * @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)
+                         const Point2& pB,
+                         const SharedNoiseModel& model = nullptr)
       : Base(model, keyE, keyKa, keyKb), pA_(pA), pB_(pB) {}
 
   /// @return a deep copy of this factor
@@ -492,17 +496,17 @@ class EssentialMatrixFactor5
     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
+    // 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
+      // 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
+      // Compute the jacobian of error w.r.t Kb.
       *HKb = vA.transpose() * E.matrix().leftCols<2>() * cB_H_Kb;
     }
 

gtsam/slam/tests/testEssentialMatrixFactor.cpp

@@ -117,8 +117,8 @@ TEST(EssentialMatrixFactor, ExpressionFactor) {
     // Check evaluation
     Vector expected(1);
     expected << 0;
-    vector<Matrix> Hactual(1);
+    vector<Matrix> actualH(1);
-    Vector actual = factor.unwhitenedError(values, Hactual);
+    Vector actual = factor.unwhitenedError(values, actualH);
     EXPECT(assert_equal(expected, actual, 1e-7));
   }
 }
@@ -151,8 +151,8 @@ TEST(EssentialMatrixFactor, ExpressionFactorRotationOnly) {
     // Check evaluation
     Vector expected(1);
     expected << 0;
-    vector<Matrix> Hactual(1);
+    vector<Matrix> actualH(1);
-    Vector actual = factor.unwhitenedError(values, Hactual);
+    Vector actual = factor.unwhitenedError(values, actualH);
     EXPECT(assert_equal(expected, actual, 1e-7));
   }
 }
@@ -213,9 +213,9 @@ TEST(EssentialMatrixFactor2, factor) {
     const Point2 pi = PinholeBase::Project(P2);
     Point2 expected(pi - pB(i));
 
-    Matrix Hactual1, Hactual2;
+    Matrix actualH1, actualH2;
     double d(baseline / P1.z());
-    Vector actual = factor.evaluateError(trueE, d, Hactual1, Hactual2);
+    Vector actual = factor.evaluateError(trueE, d, actualH1, actualH2);
     EXPECT(assert_equal(expected, actual, 1e-7));
 
     Values val;
@@ -278,9 +278,9 @@ TEST(EssentialMatrixFactor3, factor) {
     const Point2 pi = camera2.project(P1);
     Point2 expected(pi - pB(i));
 
-    Matrix Hactual1, Hactual2;
+    Matrix actualH1, actualH2;
     double d(baseline / P1.z());
-    Vector actual = factor.evaluateError(bodyE, d, Hactual1, Hactual2);
+    Vector actual = factor.evaluateError(bodyE, d, actualH1, actualH2);
     EXPECT(assert_equal(expected, actual, 1e-7));
 
     Values val;
@@ -552,6 +552,27 @@ TEST(EssentialMatrixFactor5, factor) {
   }
 }
 
+//*************************************************************************
+// Test that if we use the same keys for Ka and Kb the sum of the two K
+// Jacobians equals that of the single K Jacobian for EssentialMatrix4
+TEST(EssentialMatrixFactor5, SameKeys) {
+  Key keyE(1), keyK(2);
+  for (size_t i = 0; i < 5; i++) {
+    EssentialMatrixFactor4<Cal3_S2> factor4(keyE, keyK, pA(i), pB(i), model1);
+    EssentialMatrixFactor5<Cal3_S2> factor5(keyE, keyK, keyK, pA(i), pB(i),
+                                            model1);
+
+    Values truth;
+    truth.insert(keyE, trueE);
+    truth.insert(keyK, trueK);
+
+    // Check Jacobians
+    Matrix H0, H1, H2;
+    factor4.evaluateError(trueE, trueK, nullptr, &H0);
+    factor5.evaluateError(trueE, trueK, trueK, nullptr, &H1, &H2);
+    EXPECT(assert_equal(H0, H1 + H2, 1e-9));
+  }
+}
 }  // namespace example1
 
 //*************************************************************************