From baa275bf51ed63dd61f3da2c90167491a2f1806e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 26 Nov 2024 09:53:28 -0800 Subject: [PATCH] address comments --- gtsam/slam/EssentialMatrixFactor.h | 32 +++++++++------- .../slam/tests/testEssentialMatrixFactor.cpp | 37 +++++++++++++++---- 2 files changed, 47 insertions(+), 22 deletions(-) diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index 7a0b46906..5fdf138cc 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/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 - * calibration unknown, as Jacobians will be incorrect... + * 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: 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 EssentialMatrixFactor5 @@ -442,17 +445,18 @@ class EssentialMatrixFactor5 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 + * 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) + 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; } diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 609f38187..11558e38e 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -117,8 +117,8 @@ TEST(EssentialMatrixFactor, ExpressionFactor) { // Check evaluation Vector expected(1); expected << 0; - vector Hactual(1); - Vector actual = factor.unwhitenedError(values, Hactual); + vector actualH(1); + 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 Hactual(1); - Vector actual = factor.unwhitenedError(values, Hactual); + vector actualH(1); + 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 factor4(keyE, keyK, pA(i), pB(i), model1); + EssentialMatrixFactor5 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 //*************************************************************************