From eca2bb5d8a916043608866792c13e7cb86cae10a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 30 Oct 2024 10:27:51 -0700 Subject: [PATCH 1/8] epipolarLine --- gtsam/geometry/FundamentalMatrix.cpp | 28 ++++++++++++++++++++++++++++ gtsam/geometry/FundamentalMatrix.h | 7 +++++++ 2 files changed, 35 insertions(+) diff --git a/gtsam/geometry/FundamentalMatrix.cpp b/gtsam/geometry/FundamentalMatrix.cpp index 5e8c26e627..837ba72632 100644 --- a/gtsam/geometry/FundamentalMatrix.cpp +++ b/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_ diff --git a/gtsam/geometry/FundamentalMatrix.h b/gtsam/geometry/FundamentalMatrix.h index c114c2b5be..df2c2881af 100644 --- a/gtsam/geometry/FundamentalMatrix.h +++ b/gtsam/geometry/FundamentalMatrix.h @@ -7,6 +7,7 @@ #pragma once +#include #include #include #include @@ -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 From 45fc039d075868c9b038b5a3d5aaebf4aba40cba Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 30 Oct 2024 14:54:57 -0700 Subject: [PATCH 2/8] 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 b19bb09ab6..c8f94681cf 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 0f9c2ef9f8..609f381871 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 //************************************************************************* From f8d00f82f16e16110b5d0884b56550f24cd35c74 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 30 Oct 2024 16:06:56 -0700 Subject: [PATCH 3/8] Allow for global calibration --- gtsam/sfm/TransferFactor.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam/sfm/TransferFactor.h b/gtsam/sfm/TransferFactor.h index c8b249ddcb..b73a71aa4f 100644 --- a/gtsam/sfm/TransferFactor.h +++ b/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) From ce196d962fbd62283cda8295cbc93f7cb6722657 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 30 Oct 2024 16:07:22 -0700 Subject: [PATCH 4/8] Wrap EMFs --- gtsam/slam/EssentialMatrixFactor.h | 4 +-- gtsam/slam/slam.i | 50 +++++++++++++++++++++++++++--- 2 files changed, 47 insertions(+), 7 deletions(-) diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index c8f94681cf..7a0b469064 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -354,7 +354,7 @@ class EssentialMatrixFactor4 * 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 @@ -452,7 +452,7 @@ class EssentialMatrixFactor5 * coordinates */ EssentialMatrixFactor5(Key keyE, Key keyKa, Key keyKb, const Point2& pA, - const Point2& pB, const SharedNoiseModel& model) + const Point2& pB, const SharedNoiseModel& model = nullptr) : Base(model, keyE, keyKa, keyKb), pA_(pA), pB_(pB) {} /// @return a deep copy of this factor diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index 97dcfcae7b..a226f06ec6 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -107,7 +107,7 @@ typedef gtsam::GeneralSFMFactor, gtsam::Point3> GeneralSFMFactorPoseCal3Unified; -template virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { GeneralSFMFactor2(const gtsam::Point2& measured, @@ -221,15 +221,55 @@ typedef gtsam::PoseRotationPrior PoseRotationPrior3D; #include virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor { - EssentialMatrixFactor(size_t key, const gtsam::Point2& pA, - const gtsam::Point2& pB, - const gtsam::noiseModel::Base* noiseModel); + EssentialMatrixFactor(size_t key, + const gtsam::Point2& pA, const gtsam::Point2& pB, + const gtsam::noiseModel::Base* model); void print(string s = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::EssentialMatrixFactor& other, double tol) const; gtsam::Vector evaluateError(const gtsam::EssentialMatrix& E) const; }; +virtual class EssentialMatrixFactor2 : gtsam::NoiseModelFactor { + EssentialMatrixFactor2(size_t key1, size_t key2, + const gtsam::Point2& pA, const gtsam::Point2& pB, + const gtsam::noiseModel::Base* model); + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + gtsam::Vector evaluateError(const gtsam::EssentialMatrix& E, double d) const; +}; + +virtual class EssentialMatrixFactor3 : gtsam::EssentialMatrixFactor2 { + EssentialMatrixFactor3(size_t key1, size_t key2, + const gtsam::Point2& pA, const gtsam::Point2& pB, + const gtsam::Rot3& cRb, const gtsam::noiseModel::Base* model); + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + gtsam::Vector evaluateError(const gtsam::EssentialMatrix& E, double d) const; +}; + +template +virtual class EssentialMatrixFactor4 : gtsam::NoiseModelFactor { + EssentialMatrixFactor4(size_t keyE, size_t keyK, + const gtsam::Point2& pA, const gtsam::Point2& pB, + const gtsam::noiseModel::Base* model = nullptr); + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + gtsam::Vector evaluateError(const gtsam::EssentialMatrix& E, const CALIBRATION& K) const; +}; + +template +virtual class EssentialMatrixFactor5 : gtsam::NoiseModelFactor { + EssentialMatrixFactor5(size_t keyE, size_t keyKa, size_t keyKb, + const gtsam::Point2& pA, const gtsam::Point2& pB, + const gtsam::noiseModel::Base* model = nullptr); + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + gtsam::Vector evaluateError(const gtsam::EssentialMatrix& E, + const CALIBRATION& Ka, const CALIBRATION& Kb) const; +}; + #include virtual class EssentialMatrixConstraint : gtsam::NoiseModelFactor { EssentialMatrixConstraint(size_t key1, size_t key2, const gtsam::EssentialMatrix &measuredE, From 360dc4138c9d51f2683d8bc8af51eb692b76dc7e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 30 Oct 2024 16:07:40 -0700 Subject: [PATCH 5/8] Compare 3 more cases --- python/gtsam/examples/ViewGraphComparison.py | 112 ++++++++++++------- 1 file changed, 69 insertions(+), 43 deletions(-) diff --git a/python/gtsam/examples/ViewGraphComparison.py b/python/gtsam/examples/ViewGraphComparison.py index 1eb43cec88..37558a46c7 100644 --- a/python/gtsam/examples/ViewGraphComparison.py +++ b/python/gtsam/examples/ViewGraphComparison.py @@ -32,7 +32,7 @@ K = gtsam.symbol_shorthand.K # Methods to compare -methods = ["SimpleF", "Fundamental", "Essential+Ks", "Calibrated"] +methods = ["SimpleF", "Fundamental", "Essential+Ks", "Essential+K", "Calibrated", "Binary+Ks", "Binary+K"] # Formatter function for printing keys @@ -76,8 +76,8 @@ def simulate_data(points, poses, cal, rng, noise_std): return measurements -# Function to compute ground truth matrices def compute_ground_truth(method, poses, cal): + """Function to compute ground truth edge variables.""" E1 = EssentialMatrix.FromPose3(poses[0].between(poses[1])) E2 = EssentialMatrix.FromPose3(poses[0].between(poses[2])) F1 = FundamentalMatrix(cal.K(), E1, cal.K()) @@ -90,58 +90,80 @@ def compute_ground_truth(method, poses, cal): SF1 = SimpleFundamentalMatrix(E1, f, f, c, c) SF2 = SimpleFundamentalMatrix(E2, f, f, c, c) return SF1, SF2 - elif method == "Essential+Ks" or method == "Calibrated": - return E1, E2 else: - raise ValueError(f"Unknown method {method}") + return E1, E2 def build_factor_graph(method, num_cameras, measurements, cal): """build the factor graph""" graph = NonlinearFactorGraph() + # Determine the FactorClass based on the method if method == "Fundamental": FactorClass = gtsam.TransferFactorFundamentalMatrix elif method == "SimpleF": FactorClass = gtsam.TransferFactorSimpleFundamentalMatrix - elif method == "Essential+Ks": + elif method in ["Essential+Ks", "Essential+K"]: FactorClass = gtsam.EssentialTransferFactorKCal3f - # add priors on all calibrations: - for i in range(num_cameras): - model = gtsam.noiseModel.Isotropic.Sigma(1, 10.0) - graph.addPriorCal3f(K(i), cal, model) + elif method == "Binary+K": + FactorClass = gtsam.EssentialMatrixFactor4Cal3f + elif method == "Binary+Ks": + FactorClass = gtsam.EssentialMatrixFactor5Cal3f elif method == "Calibrated": FactorClass = gtsam.EssentialTransferFactorCal3f - # No priors on calibration needed else: raise ValueError(f"Unknown method {method}") + # Add priors on calibrations if necessary + if method in ["Essential+Ks", "Binary+Ks"]: + for i in range(num_cameras): + model = gtsam.noiseModel.Isotropic.Sigma(1, 10.0) + graph.addPriorCal3f(K(i), cal, model) + elif method in ["Essential+K", "Binary+K"]: + model = gtsam.noiseModel.Isotropic.Sigma(1, 10.0) + graph.addPriorCal3f(K(0), cal, model) + z = measurements # shorthand for a in range(num_cameras): b = (a + 1) % num_cameras # Next camera c = (a + 2) % num_cameras # Camera after next - - # Vectors to collect tuples for each factor - tuples1 = [] - tuples2 = [] - tuples3 = [] - - # Collect data for the three factors - for j in range(len(measurements[0])): - tuples1.append((z[a][j], z[b][j], z[c][j])) - tuples2.append((z[a][j], z[c][j], z[b][j])) - tuples3.append((z[c][j], z[b][j], z[a][j])) - - # Add transfer factors between views a, b, and c. - if method in ["Calibrated"]: - graph.add(FactorClass(EdgeKey(a, c), EdgeKey(b, c), tuples1, cal)) - graph.add(FactorClass(EdgeKey(a, b), EdgeKey(b, c), tuples2, cal)) - graph.add(FactorClass(EdgeKey(a, c), EdgeKey(a, b), tuples3, cal)) + if method in ["Binary+Ks", "Binary+K"]: + # Add binary Essential Matrix factors + ab, ac = EdgeKey(a, b).key(), EdgeKey(a, c).key() + for j in range(len(measurements[0])): + if method == "Binary+Ks": + graph.add(FactorClass(ab, K(a), K(b), z[a][j], z[b][j])) + graph.add(FactorClass(ac, K(a), K(c), z[a][j], z[c][j])) + else: # Binary+K + graph.add(FactorClass(ab, K(0), z[a][j], z[b][j])) + graph.add(FactorClass(ac, K(0), z[a][j], z[c][j])) else: - graph.add(FactorClass(EdgeKey(a, c), EdgeKey(b, c), tuples1)) - graph.add(FactorClass(EdgeKey(a, b), EdgeKey(b, c), tuples2)) - graph.add(FactorClass(EdgeKey(a, c), EdgeKey(a, b), tuples3)) + # Add transfer factors between views a, b, and c + + # Vectors to collect tuples for each factor + tuples1 = [] + tuples2 = [] + tuples3 = [] + + for j in range(len(measurements[0])): + tuples1.append((z[a][j], z[b][j], z[c][j])) + tuples2.append((z[a][j], z[c][j], z[b][j])) + tuples3.append((z[c][j], z[b][j], z[a][j])) + + # Add transfer factors between views a, b, and c. + if method in ["Calibrated"]: + graph.add(FactorClass(EdgeKey(a, c), EdgeKey(b, c), tuples1, cal)) + graph.add(FactorClass(EdgeKey(a, b), EdgeKey(b, c), tuples2, cal)) + graph.add(FactorClass(EdgeKey(a, c), EdgeKey(a, b), tuples3, cal)) + elif method == "Essential+K": + graph.add(FactorClass(EdgeKey(a, c), EdgeKey(b, c), K(0), tuples1)) + graph.add(FactorClass(EdgeKey(a, b), EdgeKey(b, c), K(0), tuples2)) + graph.add(FactorClass(EdgeKey(a, c), EdgeKey(a, b), K(0), tuples3)) + else: + graph.add(FactorClass(EdgeKey(a, c), EdgeKey(b, c), tuples1)) + graph.add(FactorClass(EdgeKey(a, b), EdgeKey(b, c), tuples2)) + graph.add(FactorClass(EdgeKey(a, c), EdgeKey(a, b), tuples3)) return graph @@ -159,22 +181,23 @@ def get_initial_estimate(method, num_cameras, ground_truth, cal): initialEstimate.insert(EdgeKey(a, b).key(), F1) initialEstimate.insert(EdgeKey(a, c).key(), F2) total_dimension += F1.dim() + F2.dim() - elif method in ["Essential+Ks", "Calibrated"]: + elif method in ["Essential+Ks", "Essential+K", "Binary+Ks", "Binary+K", "Calibrated"]: E1, E2 = ground_truth for a in range(num_cameras): - b = (a + 1) % num_cameras # Next camera - c = (a + 2) % num_cameras # Camera after next + b = (a + 1) % num_cameras + c = (a + 2) % num_cameras initialEstimate.insert(EdgeKey(a, b).key(), E1) initialEstimate.insert(EdgeKey(a, c).key(), E2) total_dimension += E1.dim() + E2.dim() - else: - raise ValueError(f"Unknown method {method}") - if method == "Essential+Ks": - # Insert initial calibrations + # Insert initial calibrations + if method in ["Essential+Ks", "Binary+Ks"]: for i in range(num_cameras): initialEstimate.insert(K(i), cal) total_dimension += cal.dim() + elif method in ["Essential+K", "Binary+K"]: + initialEstimate.insert(K(0), cal) + total_dimension += cal.dim() print(f"Total dimension of the problem: {total_dimension}") return initialEstimate @@ -205,7 +228,7 @@ def compute_distances(method, result, ground_truth, num_cameras, cal): key_ab = EdgeKey(a, b).key() key_ac = EdgeKey(a, c).key() - if method in ["Essential+Ks", "Calibrated"]: + if method in ["Essential+Ks", "Essential+K", "Binary+Ks", "Binary+K", "Calibrated"]: E_est_ab = result.atEssentialMatrix(key_ab) E_est_ac = result.atEssentialMatrix(key_ac) @@ -218,15 +241,18 @@ def compute_distances(method, result, ground_truth, num_cameras, cal): SF_est_ac = result.atSimpleFundamentalMatrix(key_ac).matrix() F_est_ab = FundamentalMatrix(SF_est_ab) F_est_ac = FundamentalMatrix(SF_est_ac) - elif method == "Essential+Ks": - # Retrieve calibrations from result: + elif method in ["Essential+Ks", "Binary+Ks"]: + # Retrieve calibrations from result for each camera cal_a = result.atCal3f(K(a)) cal_b = result.atCal3f(K(b)) cal_c = result.atCal3f(K(c)) - - # Convert estimated EssentialMatrices to FundamentalMatrices F_est_ab = FundamentalMatrix(cal_a.K(), E_est_ab, cal_b.K()) F_est_ac = FundamentalMatrix(cal_a.K(), E_est_ac, cal_c.K()) + elif method in ["Essential+K", "Binary+K"]: + # Use single shared calibration + cal_shared = result.atCal3f(K(0)) + F_est_ab = FundamentalMatrix(cal_shared.K(), E_est_ab, cal_shared.K()) + F_est_ac = FundamentalMatrix(cal_shared.K(), E_est_ac, cal_shared.K()) elif method == "Calibrated": # Use ground truth calibration F_est_ab = FundamentalMatrix(cal.K(), E_est_ab, cal.K()) From 183774556851cb098e58c8ab38b180b863d298ba Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 5 Nov 2024 15:06:25 -0500 Subject: [PATCH 6/8] Trying to optimize well --- python/gtsam/examples/ViewGraphComparison.py | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/python/gtsam/examples/ViewGraphComparison.py b/python/gtsam/examples/ViewGraphComparison.py index 37558a46c7..7a9f77d098 100644 --- a/python/gtsam/examples/ViewGraphComparison.py +++ b/python/gtsam/examples/ViewGraphComparison.py @@ -117,10 +117,10 @@ def build_factor_graph(method, num_cameras, measurements, cal): # Add priors on calibrations if necessary if method in ["Essential+Ks", "Binary+Ks"]: for i in range(num_cameras): - model = gtsam.noiseModel.Isotropic.Sigma(1, 10.0) + model = gtsam.noiseModel.Isotropic.Sigma(1, 1000.0) graph.addPriorCal3f(K(i), cal, model) elif method in ["Essential+K", "Binary+K"]: - model = gtsam.noiseModel.Isotropic.Sigma(1, 10.0) + model = gtsam.noiseModel.Isotropic.Sigma(1, 1000.0) graph.addPriorCal3f(K(0), cal, model) z = measurements # shorthand @@ -186,6 +186,8 @@ def get_initial_estimate(method, num_cameras, ground_truth, cal): for a in range(num_cameras): b = (a + 1) % num_cameras c = (a + 2) % num_cameras + # initialEstimate.insert(EdgeKey(a, b).key(), E1.retract(0.1 * np.ones((5, 1)))) + # initialEstimate.insert(EdgeKey(a, c).key(), E2.retract(0.1 * np.ones((5, 1)))) initialEstimate.insert(EdgeKey(a, b).key(), E1) initialEstimate.insert(EdgeKey(a, c).key(), E2) total_dimension += E1.dim() + E2.dim() @@ -206,8 +208,9 @@ def get_initial_estimate(method, num_cameras, ground_truth, cal): def optimize(graph, initialEstimate, method): """optimize the graph""" params = LevenbergMarquardtParams() - params.setlambdaInitial(1e10) # Initialize lambda to a high value - params.setlambdaUpperBound(1e10) + if method not in ["Calibrated", "Binary+K", "Binary+Ks"]: + params.setlambdaInitial(1e10) # Initialize lambda to a high value + params.setlambdaUpperBound(1e10) # params.setAbsoluteErrorTol(0.1) params.setVerbosityLM("SUMMARY") optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate, params) @@ -373,6 +376,8 @@ def main(): # Compute final error final_error = graph.error(result) + if method in ["Binary+K", "Binary+Ks"]: + final_error *= cal.f() * cal.f() # Store results results[method]["distances"].extend(distances) From 5f667b44255b0e6181b620833ba8097f596cd0b4 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 5 Nov 2024 17:24:13 -0500 Subject: [PATCH 7/8] Rename factor --- gtsam/sfm/TransferFactor.h | 18 ++++++++++++++++++ gtsam/sfm/sfm.i | 7 +++++-- gtsam/sfm/tests/testTransferFactor.cpp | 2 +- 3 files changed, 24 insertions(+), 3 deletions(-) diff --git a/gtsam/sfm/TransferFactor.h b/gtsam/sfm/TransferFactor.h index b73a71aa4f..0338762be2 100644 --- a/gtsam/sfm/TransferFactor.h +++ b/gtsam/sfm/TransferFactor.h @@ -253,6 +253,24 @@ class EssentialTransferFactorK TransferEdges(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& triplets, + const SharedNoiseModel& model = nullptr) + : Base(model, edge1, edge2, keyK, keyK, keyK), + TransferEdges(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, diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index a15b73ea19..24f24f1bb4 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -98,8 +98,11 @@ virtual class EssentialTransferFactor : gtsam::NoiseModelFactor { template virtual class EssentialTransferFactorK : gtsam::NoiseModelFactor { EssentialTransferFactorK(gtsam::EdgeKey edge1, gtsam::EdgeKey edge2, - const std::vector>& triplets, - const gtsam::noiseModel::Base* model = nullptr); + const std::vector>& triplets, + const gtsam::noiseModel::Base* model = nullptr); + EssentialTransferFactorK(gtsam::EdgeKey edge1, gtsam::EdgeKey edge2, size_t keyK, + const std::vector>& triplets, + const gtsam::noiseModel::Base* model = nullptr); }; #include diff --git a/gtsam/sfm/tests/testTransferFactor.cpp b/gtsam/sfm/tests/testTransferFactor.cpp index ced3d2ce7c..b34bdf8327 100644 --- a/gtsam/sfm/tests/testTransferFactor.cpp +++ b/gtsam/sfm/tests/testTransferFactor.cpp @@ -154,7 +154,7 @@ TEST(TransferFactor, MultipleTuples) { } //************************************************************************* -// Test for EssentialTransferFactor +// Test for EssentialTransferFactorK TEST(EssentialTransferFactor, Jacobians) { using namespace fixture; From baa275bf51ed63dd61f3da2c90167491a2f1806e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 26 Nov 2024 09:53:28 -0800 Subject: [PATCH 8/8] 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 7a0b469064..5fdf138ccd 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 609f381871..11558e38ef 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 //*************************************************************************