From 45fc039d075868c9b038b5a3d5aaebf4aba40cba Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 30 Oct 2024 14:54:57 -0700 Subject: [PATCH] EssentialMatrixFactor5 --- gtsam/slam/EssentialMatrixFactor.h | 147 +++++++++++++++--- .../slam/tests/testEssentialMatrixFactor.cpp | 33 +++- 2 files changed, 150 insertions(+), 30 deletions(-) diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index b19bb09ab..c8f94681c 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -38,7 +38,6 @@ class EssentialMatrixFactor : public NoiseModelFactorN { typedef EssentialMatrixFactor This; public: - // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; @@ -93,8 +92,8 @@ class EssentialMatrixFactor : public NoiseModelFactorN { } /// 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 @@ -343,13 +341,12 @@ class EssentialMatrixFactor4 typedef Eigen::Matrix 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 @@ -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,104 @@ 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. Don'tt use this factor with same + * calibration unknown, as Jacobians will be incorrect... + * + * Note: even stronger caveats about having priors on calibration apply. + */ +template +class EssentialMatrixFactor5 + : public NoiseModelFactorN { + private: + Point2 pA_, pB_; ///< points in pixel coordinates + + typedef NoiseModelFactorN Base; + typedef EssentialMatrixFactor5 This; + + static constexpr int DimK = FixedDimension::value; + typedef Eigen::Matrix 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) + : 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::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 diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 0f9c2ef9f..609f38187 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -14,8 +14,8 @@ #include #include #include -#include #include +#include #include using namespace std::placeholders; @@ -101,7 +101,8 @@ TEST(EssentialMatrixFactor, ExpressionFactor) { Key key(1); for (size_t i = 0; i < 5; i++) { std::function)> f = - std::bind(&EssentialMatrix::error, std::placeholders::_1, vA(i), vB(i), std::placeholders::_2); + std::bind(&EssentialMatrix::error, std::placeholders::_1, vA(i), vB(i), + std::placeholders::_2); Expression E_(key); // leaf expression Expression expr(f, E_); // unary expression @@ -127,10 +128,11 @@ TEST(EssentialMatrixFactor, ExpressionFactorRotationOnly) { Key key(1); for (size_t i = 0; i < 5; i++) { std::function)> f = - std::bind(&EssentialMatrix::error, std::placeholders::_1, vA(i), vB(i), std::placeholders::_2); + std::bind(&EssentialMatrix::error, std::placeholders::_1, vA(i), vB(i), + std::placeholders::_2); std::function, - OptionalJacobian<5, 2>)> + OptionalJacobian<5, 3>, + OptionalJacobian<5, 2>)> g; Expression R_(key); Expression d_(trueDirection); @@ -529,6 +531,27 @@ TEST(EssentialMatrixFactor4, minimizationWithStrongCal3BundlerPrior) { 1e-6); } +//************************************************************************* +TEST(EssentialMatrixFactor5, factor) { + Key keyE(1), keyKa(2), keyKb(3); + for (size_t i = 0; i < 5; i++) { + EssentialMatrixFactor5 factor(keyE, keyKa, keyKb, pA(i), pB(i), + model1); + + // Check evaluation + Vector1 expected; + expected << 0; + Vector actual = factor.evaluateError(trueE, trueK, trueK); + EXPECT(assert_equal(expected, actual, 1e-7)); + + Values truth; + truth.insert(keyE, trueE); + truth.insert(keyKa, trueK); + truth.insert(keyKb, trueK); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, truth, 1e-6, 1e-7); + } +} + } // namespace example1 //*************************************************************************