diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index 909576aa0..5eb11f8ed 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -7,10 +7,10 @@ #pragma once +#include +#include #include #include -#include -#include #include #include @@ -31,7 +31,11 @@ class EssentialMatrix { public: /// Static function to convert Point2 to homogeneous coordinates - static Vector3 Homogeneous(const Point2& p) { + static Vector3 Homogeneous(const Point2& p, + OptionalJacobian<3, 2> H = boost::none) { + if (H) { + H->setIdentity(); + } return Vector3(p.x(), p.y(), 1); } diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index 86a498cdc..fd3fb64f0 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -241,6 +241,18 @@ TEST (EssentialMatrix, epipoles) { EXPECT(assert_equal(e2, E.epipole_b())); } +//************************************************************************* +TEST(EssentialMatrix, Homogeneous) { + Point2 input(5.0, 1.3); + Vector3 expected(5.0, 1.3, 1.0); + Matrix32 expectedH; + expectedH << 1.0, 0.0, 0.0, 1.0, 0.0, 0.0; + Matrix32 actualH; + Vector3 actual = EssentialMatrix::Homogeneous(input, actualH); + EXPECT(assert_equal(actual, expected)); + EXPECT(assert_equal(actualH, expectedH)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index a99ac9512..786b9596b 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -7,9 +7,10 @@ #pragma once -#include #include #include +#include + #include namespace gtsam { @@ -17,25 +18,24 @@ namespace gtsam { /** * Factor that evaluates epipolar error p'Ep for given essential matrix */ -class EssentialMatrixFactor: public NoiseModelFactor1 { - - Vector3 vA_, vB_; ///< Homogeneous versions, in ideal coordinates +class EssentialMatrixFactor : public NoiseModelFactor1 { + Vector3 vA_, vB_; ///< Homogeneous versions, in ideal coordinates typedef NoiseModelFactor1 Base; typedef EssentialMatrixFactor This; -public: - + public: /** * Constructor * @param key Essential Matrix variable key * @param pA point in first camera, in calibrated coordinates * @param pB point in second camera, in calibrated coordinates - * @param model noise model is about dot product in ideal, homogeneous coordinates + * @param model noise model is about dot product in ideal, homogeneous + * coordinates */ EssentialMatrixFactor(Key key, const Point2& pA, const Point2& pB, - const SharedNoiseModel& model) : - Base(model, key) { + const SharedNoiseModel& model) + : Base(model, key) { vA_ = EssentialMatrix::Homogeneous(pA); vB_ = EssentialMatrix::Homogeneous(pB); } @@ -45,13 +45,15 @@ public: * @param key Essential Matrix variable key * @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 + * @param model noise model is about dot product in ideal, homogeneous + * coordinates * @param K calibration object, will be used only in constructor */ - template + template EssentialMatrixFactor(Key key, const Point2& pA, const Point2& pB, - const SharedNoiseModel& model, boost::shared_ptr K) : - Base(model, key) { + const SharedNoiseModel& model, + boost::shared_ptr K) + : Base(model, key) { assert(K); vA_ = EssentialMatrix::Homogeneous(K->calibrate(pA)); vB_ = EssentialMatrix::Homogeneous(K->calibrate(pB)); @@ -64,23 +66,25 @@ public: } /// print - void print(const std::string& s = "", + void print( + const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { Base::print(s); std::cout << " EssentialMatrixFactor with measurements\n (" - << vA_.transpose() << ")' and (" << vB_.transpose() << ")'" - << std::endl; + << vA_.transpose() << ")' and (" << vB_.transpose() << ")'" + << std::endl; } /// vector of errors returns 1D vector - Vector evaluateError(const EssentialMatrix& E, boost::optional H = - boost::none) const override { + Vector evaluateError( + const EssentialMatrix& E, + boost::optional H = boost::none) const override { Vector error(1); error << E.error(vA_, vB_, H); return error; } -public: + public: GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; @@ -88,17 +92,16 @@ public: * Binary factor that optimizes for E and inverse depth d: assumes measurement * in image 2 is perfect, and returns re-projection error in image 1 */ -class EssentialMatrixFactor2: public NoiseModelFactor2 { - - Point3 dP1_; ///< 3D point corresponding to measurement in image 1 - Point2 pn_; ///< Measurement in image 2, in ideal coordinates - double f_; ///< approximate conversion factor for error scaling +class EssentialMatrixFactor2 + : public NoiseModelFactor2 { + Point3 dP1_; ///< 3D point corresponding to measurement in image 1 + Point2 pn_; ///< Measurement in image 2, in ideal coordinates + double f_; ///< approximate conversion factor for error scaling typedef NoiseModelFactor2 Base; typedef EssentialMatrixFactor2 This; -public: - + public: /** * Constructor * @param key1 Essential Matrix variable key @@ -108,8 +111,10 @@ public: * @param model noise model should be in pixels, as well */ EssentialMatrixFactor2(Key key1, Key key2, const Point2& pA, const Point2& pB, - const SharedNoiseModel& model) : - Base(model, key1, key2), dP1_(EssentialMatrix::Homogeneous(pA)), pn_(pB) { + const SharedNoiseModel& model) + : Base(model, key1, key2), + dP1_(EssentialMatrix::Homogeneous(pA)), + pn_(pB) { f_ = 1.0; } @@ -122,11 +127,13 @@ public: * @param K calibration object, will be used only in constructor * @param model noise model should be in pixels, as well */ - template + template EssentialMatrixFactor2(Key key1, Key key2, const Point2& pA, const Point2& pB, - const SharedNoiseModel& model, boost::shared_ptr K) : - Base(model, key1, key2), dP1_( - EssentialMatrix::Homogeneous(K->calibrate(pA))), pn_(K->calibrate(pB)) { + const SharedNoiseModel& model, + boost::shared_ptr K) + : Base(model, key1, key2), + dP1_(EssentialMatrix::Homogeneous(K->calibrate(pA))), + pn_(K->calibrate(pB)) { f_ = 0.5 * (K->fx() + K->fy()); } @@ -137,12 +144,13 @@ public: } /// print - void print(const std::string& s = "", + void print( + const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { Base::print(s); std::cout << " EssentialMatrixFactor2 with measurements\n (" - << dP1_.transpose() << ")' and (" << pn_.transpose() - << ")'" << std::endl; + << dP1_.transpose() << ")' and (" << pn_.transpose() << ")'" + << std::endl; } /* @@ -150,30 +158,28 @@ public: * @param E essential matrix * @param d inverse depth d */ - Vector evaluateError(const EssentialMatrix& E, const double& d, - boost::optional DE = boost::none, boost::optional Dd = - boost::none) const override { - + Vector evaluateError( + const EssentialMatrix& E, const double& d, + boost::optional DE = boost::none, + boost::optional Dd = boost::none) 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) // The homogeneous coordinates of can be written as // 2R1*(P1-1T2) == 2R1*d*(P1-1T2) == 2R1*((x,y,1)-d*1T2) - // where we multiplied with d which yields equivalent homogeneous coordinates. - // Note that this is just the homography 2R1 for d==0 - // The point d*P1 = (x,y,1) is computed in constructor as dP1_ + // where we multiplied with d which yields equivalent homogeneous + // coordinates. Note that this is just the homography 2R1 for d==0 The point + // d*P1 = (x,y,1) is computed in constructor as dP1_ // Project to normalized image coordinates, then uncalibrate - Point2 pn(0,0); + Point2 pn(0, 0); if (!DE && !Dd) { - Point3 _1T2 = E.direction().point3(); Point3 d1T2 = d * _1T2; - Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2); // 2R1*((x,y,1)-d*1T2) + Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2); // 2R1*((x,y,1)-d*1T2) pn = PinholeBase::Project(dP2); } else { - // Calculate derivatives. TODO if slow: optimize with Mathematica // 3*2 3*3 3*3 Matrix D_1T2_dir, DdP2_rot, DP2_point; @@ -187,20 +193,19 @@ public: if (DE) { Matrix DdP2_E(3, 5); - DdP2_E << DdP2_rot, -DP2_point * d * D_1T2_dir; // (3*3), (3*3) * (3*2) - *DE = f_ * Dpn_dP2 * DdP2_E; // (2*3) * (3*5) + DdP2_E << DdP2_rot, -DP2_point * d * D_1T2_dir; // (3*3), (3*3) * (3*2) + *DE = f_ * Dpn_dP2 * DdP2_E; // (2*3) * (3*5) } - if (Dd) // efficient backwards computation: + if (Dd) // efficient backwards computation: // (2*3) * (3*3) * (3*1) *Dd = -f_ * (Dpn_dP2 * (DP2_point * _1T2)); - } Point2 reprojectionError = pn - pn_; return f_ * reprojectionError; } -public: + public: GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; // EssentialMatrixFactor2 @@ -210,15 +215,13 @@ public: * in image 2 is perfect, and returns re-projection error in image 1 * This version takes an extrinsic rotation to allow for omni-directional rigs */ -class EssentialMatrixFactor3: public EssentialMatrixFactor2 { - +class EssentialMatrixFactor3 : public EssentialMatrixFactor2 { typedef EssentialMatrixFactor2 Base; typedef EssentialMatrixFactor3 This; - Rot3 cRb_; ///< Rotation from body to camera frame - -public: + Rot3 cRb_; ///< Rotation from body to camera frame + public: /** * Constructor * @param key1 Essential Matrix variable key @@ -229,9 +232,8 @@ public: * @param model noise model should be in calibrated coordinates, as well */ EssentialMatrixFactor3(Key key1, Key key2, const Point2& pA, const Point2& pB, - const Rot3& cRb, const SharedNoiseModel& model) : - EssentialMatrixFactor2(key1, key2, pA, pB, model), cRb_(cRb) { - } + const Rot3& cRb, const SharedNoiseModel& model) + : EssentialMatrixFactor2(key1, key2, pA, pB, model), cRb_(cRb) {} /** * Constructor @@ -242,12 +244,11 @@ public: * @param K calibration object, will be used only in constructor * @param model noise model should be in pixels, as well */ - template + template EssentialMatrixFactor3(Key key1, Key key2, const Point2& pA, const Point2& pB, - const Rot3& cRb, const SharedNoiseModel& model, - boost::shared_ptr K) : - EssentialMatrixFactor2(key1, key2, pA, pB, model, K), cRb_(cRb) { - } + const Rot3& cRb, const SharedNoiseModel& model, + boost::shared_ptr K) + : EssentialMatrixFactor2(key1, key2, pA, pB, model, K), cRb_(cRb) {} /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { @@ -256,7 +257,8 @@ public: } /// print - void print(const std::string& s = "", + void print( + const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { Base::print(s); std::cout << " EssentialMatrixFactor3 with rotation " << cRb_ << std::endl; @@ -267,9 +269,10 @@ public: * @param E essential matrix * @param d inverse depth d */ - Vector evaluateError(const EssentialMatrix& E, const double& d, - boost::optional DE = boost::none, boost::optional Dd = - boost::none) const override { + Vector evaluateError( + const EssentialMatrix& E, const double& d, + boost::optional DE = boost::none, + boost::optional Dd = boost::none) const override { if (!DE) { // Convert E from body to camera frame EssentialMatrix cameraE = cRb_ * E; @@ -277,18 +280,114 @@ public: return Base::evaluateError(cameraE, d, boost::none, Dd); } else { // Version with derivatives - Matrix D_e_cameraE, D_cameraE_E; // 2*5, 5*5 + Matrix D_e_cameraE, D_cameraE_E; // 2*5, 5*5 EssentialMatrix cameraE = E.rotate(cRb_, D_cameraE_E); Vector e = Base::evaluateError(cameraE, d, D_e_cameraE, Dd); - *DE = D_e_cameraE * D_cameraE_E; // (2*5) * (5*5) + *DE = D_e_cameraE * D_cameraE_E; // (2*5) * (5*5) return e; } } -public: + public: GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; // EssentialMatrixFactor3 -}// gtsam +/** + * Factor that evaluates algebraic epipolar error (K^-1 p)'E (K^-1 p) for given + * essential matrix and calibration. The calibration is shared between two + * images. + */ +template +class EssentialMatrixFactor4 + : public NoiseModelFactor2 { + private: + Point2 pA_, pB_; ///< points in pixel coordinates + typedef NoiseModelFactor2 Base; + typedef EssentialMatrixFactor4 This; + + static const int DimK = FixedDimension::value; + typedef Eigen::Matrix JacobianCalibration; + + public: + /** + * Constructor + * @param essentialMatrixKey Essential Matrix variable key + * @param calibrationKey Calibration variable key + * @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 essentialMatrixKey, Key calibrationKey, + const Point2& pA, const Point2& pB, + const SharedNoiseModel& model) + : Base(model, essentialMatrixKey, calibrationKey), pA_(pA), pB_(pB) {} + + /// @return a deep copy of this factor + gtsam::NonlinearFactor::shared_ptr clone() const override { + return boost::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 << " EssentialMatrixFactor4 with measurements\n (" + << pA_.transpose() << ")' and (" << pB_.transpose() << ")'" + << std::endl; + } + + /// vector of errors returns 1D vector + /** + * @brief Calculate the algebraic epipolar error p' (K^-1)' E K p. + * + * @param E essential matrix for key essentialMatrixKey + * @param K calibration (common for both images) for key calibrationKey + * @param H1 optional jacobian in E + * @param H2 optional jacobian in K + * @return * Vector + */ + Vector evaluateError( + const EssentialMatrix& E, const CALIBRATION& K, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const override { + Vector error(1); + // 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); + Point2 cB = K.calibrate(pB_, H2 ? &cB_H_K : 0); + + // Homogeneous the coordinates + Matrix32 vA_H_cA, vB_H_cB; + Vector3 vA = EssentialMatrix::Homogeneous(cA, H2 ? &vA_H_cA : 0); + Vector3 vB = EssentialMatrix::Homogeneous(cB, H2 ? &vB_H_cB : 0); + + if (H2) { + // compute the jacobian of error w.r.t K + + // using dvA/dK = dvA/dcA * dcA/dK and dVB/dK = dvB/dcB * dcB/dK + // Matrix vA_H_K = vA_H_cA * cA_H_K; + // Matrix vB_H_K = vB_H_cB * cB_H_K; + + // error function f = vA.T * E * vB + // H2 = df/dK = vB.T * E.T * dvA/dK + vA.T * E * dvB/dK + *H2 = vB.transpose() * E.matrix().transpose() * vA_H_cA * cA_H_K + + vA.transpose() * E.matrix() * vB_H_cB * cB_H_K; + } + + error << E.error(vA, vB, H1); + + return error; + } + + public: + GTSAM_MAKE_ALIGNED_OPERATOR_NEW +}; +// EssentialMatrixFactor4 + +} // namespace gtsam diff --git a/gtsam/slam/EssentialMatrixWithCalibrationFactor.h b/gtsam/slam/EssentialMatrixWithCalibrationFactor.h deleted file mode 100644 index 568a94585..000000000 --- a/gtsam/slam/EssentialMatrixWithCalibrationFactor.h +++ /dev/null @@ -1,127 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file EssentialMatrixWithCalibrationFactor.h - * - * @brief A factor evaluating algebraic epipolar error with essential matrix and - * calibration as variables. - * - * @author Ayush Baid - * @author Akshay Krishnan - * @date April 23, 2021 - */ - -#pragma once - -#include -#include - -#include - -namespace gtsam { - -/** - * Factor that evaluates algebraic epipolar error (K^-1 p)'E (K^-1 p) for given - * essential matrix and calibration shared between two images. - */ -template -class EssentialMatrixWithCalibrationFactor - : public NoiseModelFactor2 { - Point2 pA_, pB_; ///< points in pixel coordinates - - typedef NoiseModelFactor2 Base; - typedef EssentialMatrixWithCalibrationFactor This; - - public: - /** - * Constructor - * @param essentialMatrixKey Essential Matrix variable key - * @param calibrationKey Calibration variable key - * @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 - */ - EssentialMatrixWithCalibrationFactor(Key essentialMatrixKey, - Key calibrationKey, const Point2& pA, - const Point2& pB, - const SharedNoiseModel& model) - : Base(model, essentialMatrixKey, calibrationKey), pA_(pA), pB_(pB) {} - - /// @return a deep copy of this factor - gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::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 << " EssentialMatrixWithCalibrationFactor with measurements\n (" - << pA_.transpose() << ")' and (" << pB_.transpose() << ")'" - << std::endl; - } - - /// vector of errors returns 1D vector - /** - * @brief Calculate the algebraic epipolar error p' (K^-1)' E K p. - * - * @param E essential matrix for key essentialMatrixKey - * @param K calibration (common for both images) for key calibrationKey - * @param H1 optional jacobian in E - * @param H2 optional jacobian in K - * @return * Vector - */ - Vector evaluateError( - const EssentialMatrix& E, const CALIBRATION& K, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const override { - Vector error(1); - // converting from pixel coordinates to normalized coordinates cA and cB - Matrix cA_H_K; // dcA/dK - Matrix cB_H_K; // dcB/dK - Point2 cA = K.calibrate(pA_, cA_H_K); - Point2 cB = K.calibrate(pB_, cB_H_K); - - // Homogeneous the coordinates - Vector3 vA = EssentialMatrix::Homogeneous(cA); - Vector3 vB = EssentialMatrix::Homogeneous(cB); - - if (H2) { - // compute the jacobian of error w.r.t K - - // dvX / dcX [3x2] = [1, 0], [0, 1], [0, 0] - Matrix v_H_c = - (Matrix(3, 2) << 1.0, 0.0, 0.0, 1.0, 0.0, 0.0).finished(); // [3x2] - - // computing dvA/dK = dvA/dcA * dcA/dK and dVB/dK = dvB/dcB * dcB/dK - Matrix vA_H_K = v_H_c * cA_H_K; - Matrix vB_H_K = v_H_c * cB_H_K; - - // error function f = vB.T * E * vA - // H2 = df/dK = vB.T * E.T * dvA/dK + vA.T * E * dvB/dK - *H2 = vB.transpose() * E.matrix().transpose() * vA_H_K + - vA.transpose() * E.matrix() * vB_H_K; - } - - error << E.error(vA, vB, H1); - - return error; - } - - public: - GTSAM_MAKE_ALIGNED_OPERATOR_NEW -}; - -} // namespace gtsam diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 95db611d7..3a53157f6 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -39,7 +39,9 @@ SfmData data; bool readOK = readBAL(filename, data); Rot3 c1Rc2 = data.cameras[1].pose().rotation(); Point3 c1Tc2 = data.cameras[1].pose().translation(); -PinholeCamera camera2(data.cameras[1].pose(), Cal3_S2()); +// TODO: maybe default value not good; assert with 0th +Cal3_S2 trueK = Cal3_S2(); +PinholeCamera camera2(data.cameras[1].pose(), trueK); Rot3 trueRotation(c1Rc2); Unit3 trueDirection(c1Tc2); EssentialMatrix trueE(trueRotation, trueDirection); @@ -351,7 +353,112 @@ TEST (EssentialMatrixFactor3, minimization) { EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); } -} // namespace example1 +//************************************************************************* +TEST(EssentialMatrixFactor4, factor) { + Key keyE(1); + Key keyK(1); + for (size_t i = 0; i < 5; i++) { + EssentialMatrixFactor4 factor(keyE, keyK, pA(i), pB(i), model1); + + // Check evaluation + Vector1 expected; + expected << 0; + Matrix HEactual; + Matrix HKactual; + Vector actual = factor.evaluateError(trueE, trueK, HEactual, HKactual); + EXPECT(assert_equal(expected, actual, 1e-7)); + + // Use numerical derivatives to calculate the expected Jacobian + Matrix HEexpected; + Matrix HKexpected; + typedef Eigen::Matrix Vector1; + boost::function f = + boost::bind(&EssentialMatrixFactor4::evaluateError, factor, _1, + _2, boost::none, boost::none); + HEexpected = numericalDerivative21( + f, trueE, trueK); + HKexpected = numericalDerivative22( + f, trueE, trueK); + + // Verify the Jacobian is correct + EXPECT(assert_equal(HEexpected, HEactual, 1e-8)); + EXPECT(assert_equal(HKexpected, HKactual, 1e-8)); + } +} + +//************************************************************************* +TEST(EssentialMatrixFactor4, evaluateErrorJacobians) { + Key keyE(1); + Key keyK(2); + // initialize essential matrix + Rot3 r = Rot3::Expmap(Vector3(M_PI / 6, M_PI / 3, M_PI / 9)); + Unit3 t(Point3(2, -1, 0.5)); + EssentialMatrix E = EssentialMatrix::FromRotationAndDirection(r, t); + Cal3_S2 K(200, 1, 1, 10, 10); + Values val; + val.insert(keyE, E); + val.insert(keyK, K); + + Point2 pA(10.0, 20.0); + Point2 pB(12.0, 15.0); + + EssentialMatrixFactor4 f(keyE, keyK, pA, pB, model1); + EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-6); +} + +//************************************************************************* +TEST(EssentialMatrixFactor4, minimization) { + // As before, we start with a factor graph and add constraints to it + NonlinearFactorGraph graph; + for (size_t i = 0; i < 5; i++) + graph.emplace_shared>(1, 2, pA(i), pB(i), + model1); + + // Check error at ground truth + Values truth; + truth.insert(1, trueE); + truth.insert(2, trueK); + EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8); + + // Check error at initial estimate + Values initial; + EssentialMatrix initialE = + trueE.retract((Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished()); + Cal3_S2 initialK = + trueK.retract((Vector(5) << 0.1, -0.1, 0.03, -0.2, 0.2).finished()); + initial.insert(1, initialE); + initial.insert(2, trueK); +#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) + EXPECT_DOUBLES_EQUAL(643.26, graph.error(initial), 1e-2); +#else + EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), + 1e-2); // TODO: update this value too +#endif + + // Optimize + LevenbergMarquardtParams parameters; + LevenbergMarquardtOptimizer optimizer(graph, initial, parameters); + Values result = optimizer.optimize(); + + // Check result + EssentialMatrix actualE = result.at(1); + Cal3_S2 actualK = result.at(2); + EXPECT(assert_equal(trueE, actualE, 1e-1)); // TODO: fix the tolerance + EXPECT(assert_equal(trueK, actualK, 1e-1)); // TODO: fix the tolerance + + // Check error at result + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); + + // Check errors individually + for (size_t i = 0; i < 5; i++) + EXPECT_DOUBLES_EQUAL( + 0, + actualE.error(EssentialMatrix::Homogeneous(actualK.calibrate(pA(i))), + EssentialMatrix::Homogeneous(actualK.calibrate(pB(i)))), + 1e-6); +} + +} // namespace example1 //************************************************************************* @@ -373,21 +480,21 @@ Point2 pB(size_t i) { return data.tracks[i].measurements[1].second; } -boost::shared_ptr // -K = boost::make_shared(500, 0, 0); -PinholeCamera camera2(data.cameras[1].pose(), *K); +Cal3Bundler trueK = Cal3Bundler(500, 0, 0); +boost::shared_ptr K = boost::make_shared(trueK); +PinholeCamera camera2(data.cameras[1].pose(), trueK); Vector vA(size_t i) { - Point2 xy = K->calibrate(pA(i)); + Point2 xy = trueK.calibrate(pA(i)); return EssentialMatrix::Homogeneous(xy); } Vector vB(size_t i) { - Point2 xy = K->calibrate(pB(i)); + Point2 xy = trueK.calibrate(pB(i)); return EssentialMatrix::Homogeneous(xy); } //************************************************************************* -TEST (EssentialMatrixFactor, extraMinimization) { +TEST(EssentialMatrixFactor, extraMinimization) { // Additional test with camera moving in positive X direction NonlinearFactorGraph graph; @@ -526,7 +633,59 @@ TEST (EssentialMatrixFactor3, extraTest) { } } -} // namespace example2 +TEST(EssentialMatrixFactor4, extraMinimization) { + // Additional test with camera moving in positive X direction + + NonlinearFactorGraph graph; + for (size_t i = 0; i < 5; i++) + graph.emplace_shared>(1, 2, pA(i), + pB(i), model1); + + // Check error at ground truth + Values truth; + truth.insert(1, trueE); + truth.insert(2, trueK); + EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8); + + // Check error at initial estimate + Values initial; + EssentialMatrix initialE = + trueE.retract((Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished()); + Cal3Bundler initialK = + trueK.retract((Vector(3) << 0.1, -0.02, 0.03).finished()); + initial.insert(1, initialE); + initial.insert(2, initialK); + +#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) + EXPECT_DOUBLES_EQUAL(633.71, graph.error(initial), 1e-2); +#else + EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); // TODO: fix this +#endif + + // Optimize + LevenbergMarquardtParams parameters; + LevenbergMarquardtOptimizer optimizer(graph, initial, parameters); + Values result = optimizer.optimize(); + + // Check result + EssentialMatrix actualE = result.at(1); + Cal3Bundler actualK = result.at(2); + EXPECT(assert_equal(trueE, actualE, 1e-1)); // TODO: tighten tolerance + EXPECT(assert_equal(trueK, actualK, 1e-1)); // TODO: tighten tolerance + + // Check error at result + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); + + // Check errors individually + for (size_t i = 0; i < 5; i++) + EXPECT_DOUBLES_EQUAL( + 0, + actualE.error(EssentialMatrix::Homogeneous(actualK.calibrate(pA(i))), + EssentialMatrix::Homogeneous(actualK.calibrate(pB(i)))), + 1e-6); +} + +} // namespace example2 /* ************************************************************************* */ int main() { diff --git a/gtsam/slam/tests/testEssentialMatrixWithCalibrationFactor.cpp b/gtsam/slam/tests/testEssentialMatrixWithCalibrationFactor.cpp deleted file mode 100644 index 4d77e1fcd..000000000 --- a/gtsam/slam/tests/testEssentialMatrixWithCalibrationFactor.cpp +++ /dev/null @@ -1,455 +0,0 @@ -/** - * @file testEssentialMatrixWithCalibrationFactor.cpp - * @brief Test EssentialMatrixWithCalibrationFactor class - * @author Ayush Baid - * @author Akshay Krishnan - * @date April 22, 2021 - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace std; -using namespace gtsam; - -// Noise model for first type of factor is evaluating algebraic error -noiseModel::Isotropic::shared_ptr model1 = - noiseModel::Isotropic::Sigma(1, 0.01); -// Noise model for second type of factor is evaluating pixel coordinates -noiseModel::Unit::shared_ptr model2 = noiseModel::Unit::Create(2); - -// The rotation between body and camera is: -gtsam::Point3 bX(1, 0, 0), bY(0, 1, 0), bZ(0, 0, 1); -gtsam::Rot3 cRb = gtsam::Rot3(bX, bZ, -bY).inverse(); - -namespace example1 { - -const string filename = findExampleDataFile("5pointExample1.txt"); -SfmData data; -bool readOK = readBAL(filename, data); -Rot3 c1Rc2 = data.cameras[1].pose().rotation(); -Point3 c1Tc2 = data.cameras[1].pose().translation(); -// TODO: maybe default value not good; assert with 0th -Cal3_S2 trueK = Cal3_S2(); -// PinholeCamera camera2(data.cameras[1].pose(), trueK); -Rot3 trueRotation(c1Rc2); -Unit3 trueDirection(c1Tc2); -EssentialMatrix trueE(trueRotation, trueDirection); - -double baseline = 0.1; // actual baseline of the camera - -Point2 pA(size_t i) { return data.tracks[i].measurements[0].second; } -Point2 pB(size_t i) { return data.tracks[i].measurements[1].second; } -Vector vA(size_t i, Cal3_S2 K) { - return EssentialMatrix::Homogeneous(K.calibrate(pA(i))); -} -Vector vB(size_t i, Cal3_S2 K) { - return EssentialMatrix::Homogeneous(K.calibrate(pB(i))); -} - -//************************************************************************* -TEST(EssentialMatrixWithCalibrationFactor, testData) { - CHECK(readOK); - - // Check E matrix - Matrix expected(3, 3); - expected << 0, 0, 0, 0, 0, -0.1, 0.1, 0, 0; - Matrix aEb_matrix = - skewSymmetric(c1Tc2.x(), c1Tc2.y(), c1Tc2.z()) * c1Rc2.matrix(); - EXPECT(assert_equal(expected, aEb_matrix, 1e-8)); - - // Check some projections - EXPECT(assert_equal(Point2(0, 0), pA(0), 1e-8)); - EXPECT(assert_equal(Point2(0, 0.1), pB(0), 1e-8)); - EXPECT(assert_equal(Point2(0, -1), pA(4), 1e-8)); - EXPECT(assert_equal(Point2(-1, 0.2), pB(4), 1e-8)); - - // Check homogeneous version - EXPECT(assert_equal(Vector3(-1, 0.2, 1), vB(4, trueK), 1e-8)); - - // check the calibration - Cal3_S2 expectedK(1, 1, 0, 0, 0); - EXPECT(assert_equal(expectedK, trueK)); - - // Check epipolar constraint - for (size_t i = 0; i < 5; i++) - EXPECT_DOUBLES_EQUAL( - 0, vA(i, trueK).transpose() * aEb_matrix * vB(i, trueK), 1e-8); - - // Check epipolar constraint - for (size_t i = 0; i < 5; i++) - EXPECT_DOUBLES_EQUAL(0, trueE.error(vA(i, trueK), vB(i, trueK)), 1e-7); -} - -//************************************************************************* -TEST(EssentialMatrixWithCalibrationFactor, factor) { - Key keyE(1); - Key keyK(1); - for (size_t i = 0; i < 5; i++) { - EssentialMatrixWithCalibrationFactor factor(keyE, keyK, pA(i), - pB(i), model1); - - // Check evaluation - Vector expected(1); - expected << 0; - Matrix HEactual; - Matrix HKactual; - Vector actual = factor.evaluateError(trueE, trueK, HEactual, HKactual); - EXPECT(assert_equal(expected, actual, 1e-7)); - - // Use numerical derivatives to calculate the expected Jacobian - Matrix HEexpected; - Matrix HKexpected; - typedef Eigen::Matrix Vector1; - // TODO: fix this - boost::function f = - boost::bind( - &EssentialMatrixWithCalibrationFactor::evaluateError, - factor, _1, _2, boost::none, boost::none); - HEexpected = numericalDerivative21( - f, trueE, trueK); - HKexpected = numericalDerivative22( - f, trueE, trueK); - - // Verify the Jacobian is correct - EXPECT(assert_equal(HEexpected, HEactual, 1e-8)); - EXPECT(assert_equal(HKexpected, HKactual, 1e-8)); - } -} - -// //************************************************************************* -// TEST(EssentialMatrixWithCalibrationFactor, ExpressionFactor) { -// Key keyE(1); -// Key keyK(2); -// for (size_t i = 0; i < 5; i++) { -// boost::function, OptionalJacobian<1, 3>)> f = -// boost::bind(&EssentialMatrix::error, _1, pA(i), pB(i), _2); -// Expression E_(keyE); // leaf expression -// Expression K_(keyK); // leaf expression -// Expression expr(f, E_, K_); // unary expression - -// // Test the derivatives using Paul's magic -// Values values; -// values.insert(keyE, trueE); -// values.insert(keyK, trueK); -// EXPECT_CORRECT_EXPRESSION_JACOBIANS(expr, values, 1e-5, 1e-9); - -// // Create the factor -// ExpressionFactor factor(model1, 0, expr); - -// // Check evaluation -// Vector expected(1); -// expected << 0; -// vector Hactual(1); -// Vector actual = factor.unwhitenedError(values, Hactual); -// EXPECT(assert_equal(expected, actual, 1e-7)); -// } -// } - -//************************************************************************* -// TEST(EssentialMatrixWithCalibrationFactor, ExpressionFactorRotationOnly) { -// Key keyE(1); -// Key keyK(1); -// for (size_t i = 0; i < 5; i++) { -// boost::function)> f -// = -// boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2); -// boost::function, -// OptionalJacobian<5, 2>)> g; -// Expression R_(key); -// Expression d_(trueDirection); -// Expression -// E_(&EssentialMatrix::FromRotationAndDirection, R_, d_); -// Expression expr(f, E_); - -// // Test the derivatives using Paul's magic -// Values values; -// values.insert(key, trueRotation); -// EXPECT_CORRECT_EXPRESSION_JACOBIANS(expr, values, 1e-5, 1e-9); - -// // Create the factor -// ExpressionFactor factor(model1, 0, expr); - -// // Check evaluation -// Vector expected(1); -// expected << 0; -// vector Hactual(1); -// Vector actual = factor.unwhitenedError(values, Hactual); -// EXPECT(assert_equal(expected, actual, 1e-7)); -// } -// } - -//************************************************************************* -TEST(EssentialMatrixWithCalibrationFactor, evaluateErrorJacobians) { - Key keyE(1); - Key keyK(2); - // initialize essential matrix - Rot3 r = Rot3::Expmap(Vector3(M_PI / 6, M_PI / 3, M_PI / 9)); - Unit3 t(Point3(2, -1, 0.5)); - EssentialMatrix E = EssentialMatrix::FromRotationAndDirection(r, t); - Cal3_S2 K(200, 1, 1, 10, 10); - Values val; - val.insert(keyE, E); - val.insert(keyK, K); - - Point2 pA(10.0, 20.0); - Point2 pB(12.0, 15.0); - - EssentialMatrixWithCalibrationFactor f(keyE, keyK, pA, pB, model1); - EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-6); -} - -//************************************************************************* -TEST(EssentialMatrixWithCalibrationFactor, minimization) { - // Here we want to optimize directly on essential matrix constraints - // Yi Ma's algorithm (Ma01ijcv) is a bit cumbersome to implement, - // but GTSAM does the equivalent anyway, provided we give the right - // factors. In this case, the factors are the constraints. - - // We start with a factor graph and add constraints to it - // Noise sigma is 1cm, assuming metric measurements - NonlinearFactorGraph graph; - for (size_t i = 0; i < 5; i++) - graph.emplace_shared>( - 1, 2, pA(i), pB(i), model1); - - // Check error at ground truth - Values truth; - truth.insert(1, trueE); - truth.insert(2, trueK); - EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8); - - // Check error at initial estimate - Values initial; - EssentialMatrix initialE = - trueE.retract((Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished()); - Cal3_S2 initialK = - trueK.retract((Vector(5) << 0.1, -0.1, 0.03, -0.2, 0.2).finished()); - initial.insert(1, initialE); - initial.insert(2, trueK); -#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) - EXPECT_DOUBLES_EQUAL(643.26, graph.error(initial), 1e-2); -#else - EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), - 1e-2); // TODO: update this value too -#endif - - // Optimize - LevenbergMarquardtParams parameters; - LevenbergMarquardtOptimizer optimizer(graph, initial, parameters); - Values result = optimizer.optimize(); - - // Check result - EssentialMatrix actualE = result.at(1); - Cal3_S2 actualK = result.at(2); - EXPECT(assert_equal(trueE, actualE, 1e-2)); - EXPECT(assert_equal(trueK, actualK, 1e-2)); - - // Check error at result - EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); - - // Check errors individually - for (size_t i = 0; i < 5; i++) - EXPECT_DOUBLES_EQUAL(0, actualE.error(vA(i, actualK), vB(i, actualK)), - 1e-6); -} - -} // namespace example1 - -//************************************************************************* - -// namespace example2 { - -// const string filename = findExampleDataFile("5pointExample2.txt"); -// SfmData data; -// bool readOK = readBAL(filename, data); -// Rot3 aRb = data.cameras[1].pose().rotation(); -// Point3 aTb = data.cameras[1].pose().translation(); -// EssentialMatrix trueE(aRb, Unit3(aTb)); - -// double baseline = 10; // actual baseline of the camera - -// Point2 pA(size_t i) { -// return data.tracks[i].measurements[0].second; -// } -// Point2 pB(size_t i) { -// return data.tracks[i].measurements[1].second; -// } - -// boost::shared_ptr // -// K = boost::make_shared(500, 0, 0); -// PinholeCamera camera2(data.cameras[1].pose(), *K); - -// Vector vA(size_t i) { -// Point2 xy = K->calibrate(pA(i)); -// return EssentialMatrix::Homogeneous(xy); -// } -// Vector vB(size_t i) { -// Point2 xy = K->calibrate(pB(i)); -// return EssentialMatrix::Homogeneous(xy); -// } - -// //************************************************************************* -// TEST (EssentialWithMatrixCalibrationFactor, extraMinimization) { -// // Additional test with camera moving in positive X direction - -// NonlinearFactorGraph graph; -// for (size_t i = 0; i < 5; i++) -// graph.emplace_shared(1, pA(i), -// pB(i), model1, K); - -// // Check error at ground truth -// Values truth; -// truth.insert(1, trueE); -// EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8); - -// // Check error at initial estimate -// Values initial; -// EssentialMatrix initialE = trueE.retract( -// (Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished()); -// initial.insert(1, initialE); - -// #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) -// EXPECT_DOUBLES_EQUAL(643.26, graph.error(initial), 1e-2); -// #else -// EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); -// #endif - -// // Optimize -// LevenbergMarquardtParams parameters; -// LevenbergMarquardtOptimizer optimizer(graph, initial, parameters); -// Values result = optimizer.optimize(); - -// // Check result -// EssentialMatrix actual = result.at(1); -// EXPECT(assert_equal(trueE, actual, 1e-1)); - -// // Check error at result -// EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); - -// // Check errors individually -// for (size_t i = 0; i < 5; i++) -// EXPECT_DOUBLES_EQUAL(0, actual.error(vA(i), vB(i)), 1e-6); - -// } - -// //************************************************************************* -// TEST (EssentialMatrixFactor2, extraTest) { -// for (size_t i = 0; i < 5; i++) { -// EssentialMatrixFactor2 factor(100, i, pA(i), pB(i), model2, K); - -// // Check evaluation -// Point3 P1 = data.tracks[i].p; -// const Point2 pi = camera2.project(P1); -// Point2 expected(pi - pB(i)); - -// Matrix Hactual1, Hactual2; -// double d(baseline / P1.z()); -// Vector actual = factor.evaluateError(trueE, d, Hactual1, Hactual2); -// EXPECT(assert_equal(expected, actual, 1e-7)); - -// // Use numerical derivatives to calculate the expected Jacobian -// Matrix Hexpected1, Hexpected2; -// boost::function f = boost::bind( -// &EssentialMatrixFactor2::evaluateError, &factor, _1, _2, boost::none, -// boost::none); -// Hexpected1 = numericalDerivative21(f, -// trueE, d); Hexpected2 = numericalDerivative22(f, trueE, d); - -// // Verify the Jacobian is correct -// EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6)); -// EXPECT(assert_equal(Hexpected2, Hactual2, 1e-8)); -// } -// } - -// //************************************************************************* -// TEST (EssentialMatrixFactor2, extraMinimization) { -// // Additional test with camera moving in positive X direction - -// // We start with a factor graph and add constraints to it -// // Noise sigma is 1, assuming pixel measurements -// NonlinearFactorGraph graph; -// for (size_t i = 0; i < data.number_tracks(); i++) -// graph.emplace_shared(100, i, pA(i), pB(i), -// model2, K); - -// // Check error at ground truth -// Values truth; -// truth.insert(100, trueE); -// for (size_t i = 0; i < data.number_tracks(); i++) { -// Point3 P1 = data.tracks[i].p; -// truth.insert(i, double(baseline / P1.z())); -// } -// EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8); - -// // Optimize -// LevenbergMarquardtParams parameters; -// // parameters.setVerbosity("ERROR"); -// LevenbergMarquardtOptimizer optimizer(graph, truth, parameters); -// Values result = optimizer.optimize(); - -// // Check result -// EssentialMatrix actual = result.at(100); -// EXPECT(assert_equal(trueE, actual, 1e-1)); -// for (size_t i = 0; i < data.number_tracks(); i++) -// EXPECT_DOUBLES_EQUAL(truth.at(i), result.at(i), 1e-1); - -// // Check error at result -// EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); -// } - -// //************************************************************************* -// TEST (EssentialMatrixFactor3, extraTest) { - -// // The "true E" in the body frame is -// EssentialMatrix bodyE = cRb.inverse() * trueE; - -// for (size_t i = 0; i < 5; i++) { -// EssentialMatrixFactor3 factor(100, i, pA(i), pB(i), cRb, model2, K); - -// // Check evaluation -// Point3 P1 = data.tracks[i].p; -// const Point2 pi = camera2.project(P1); -// Point2 expected(pi - pB(i)); - -// Matrix Hactual1, Hactual2; -// double d(baseline / P1.z()); -// Vector actual = factor.evaluateError(bodyE, d, Hactual1, Hactual2); -// EXPECT(assert_equal(expected, actual, 1e-7)); - -// // Use numerical derivatives to calculate the expected Jacobian -// Matrix Hexpected1, Hexpected2; -// boost::function f = boost::bind( -// &EssentialMatrixFactor3::evaluateError, &factor, _1, _2, boost::none, -// boost::none); -// Hexpected1 = numericalDerivative21(f, -// bodyE, d); Hexpected2 = numericalDerivative22(f, bodyE, d); - -// // Verify the Jacobian is correct -// EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6)); -// EXPECT(assert_equal(Hexpected2, Hactual2, 1e-8)); -// } -// } - -// } // namespace example2 - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */