diff --git a/examples/EssentialViewGraphExample.cpp b/examples/EssentialViewGraphExample.cpp index e21a36e16..205708c62 100644 --- a/examples/EssentialViewGraphExample.cpp +++ b/examples/EssentialViewGraphExample.cpp @@ -27,7 +27,7 @@ #include #include #include -#include // Contains EssentialTransferFactor +#include // Contains EssentialTransferFactorK #include @@ -63,7 +63,7 @@ int main(int argc, char* argv[]) { // Create the factor graph NonlinearFactorGraph graph; - using Factor = EssentialTransferFactor; + using Factor = EssentialTransferFactorK; for (size_t a = 0; a < 4; ++a) { size_t b = (a + 1) % 4; // Next camera diff --git a/gtsam/sfm/TransferFactor.h b/gtsam/sfm/TransferFactor.h index 2964c01ad..c8b249ddc 100644 --- a/gtsam/sfm/TransferFactor.h +++ b/gtsam/sfm/TransferFactor.h @@ -59,7 +59,7 @@ class TransferEdges { return edge1.j(); else throw std::runtime_error( - "EssentialTransferFactor: No common key in edge keys."); + "EssentialTransferFactorK: No common key in edge keys."); } /// Create Matrix3 objects based on EdgeKey configurations. @@ -85,8 +85,11 @@ class TransferEdges { */ template class TransferFactor : public NoiseModelFactorN, public TransferEdges { + public: using Base = NoiseModelFactorN; using Triplet = std::tuple; + + protected: std::vector triplets_; ///< Point triplets. public: @@ -130,6 +133,81 @@ class TransferFactor : public NoiseModelFactorN, public TransferEdges { /** * @class EssentialTransferFactor + * @brief Transfers points between views using essential matrices with a shared + * calibration. + * + * This factor is templated on the calibration class K and extends + * the TransferFactor for EssentialMatrices. It involves two essential matrices + * and a shared calibration object (K). The evaluateError function calibrates + * the image points, calls the base class's transfer method, and computes the + * error using bulk numerical differentiation. + */ +template +class EssentialTransferFactor : public TransferFactor { + using EM = EssentialMatrix; + using Triplet = std::tuple; + std::shared_ptr calibration_; ///< Shared pointer to calibration object + + public: + using Base = TransferFactor; + using This = EssentialTransferFactor; + using shared_ptr = std::shared_ptr; + + /** + * @brief Constructor that accepts a vector of point triplets and a shared + * calibration. + * + * @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) + * @param calibration Shared pointer to calibration object + * @param model An optional SharedNoiseModel + */ + EssentialTransferFactor(EdgeKey edge1, EdgeKey edge2, + const std::vector& triplets, + const std::shared_ptr& calibration, + const SharedNoiseModel& model = nullptr) + : Base(edge1, edge2, triplets, model), calibration_(calibration) {} + + /// Transfer points pa and pb to view c and evaluate error. + Vector2 TransferError(const Matrix3& Eca, const Point2& pa, + const Matrix3& Ecb, const Point2& pb, + const Point2& pc) const { + const Point2 pA = calibration_->calibrate(pa); + const Point2 pB = calibration_->calibrate(pb); + const Point2 pC = EpipolarTransfer(Eca, pA, Ecb, pB); + return calibration_->uncalibrate(pC) - pc; + } + + /// Evaluate error function + Vector evaluateError(const EM& E1, const EM& E2, + OptionalMatrixType H1 = nullptr, + OptionalMatrixType H2 = nullptr) const override { + std::function errorFunction = + [&](const EM& e1, const EM& e2) { + Vector errors(2 * this->triplets_.size()); + size_t idx = 0; + auto [Eca, Ecb] = this->getMatrices(e1, e2); + for (const auto& [pa, pb, pc] : this->triplets_) { + errors.segment<2>(idx) = TransferError(Eca, pa, Ecb, pb, pc); + idx += 2; + } + return errors; + }; + + // Compute error + Vector errors = errorFunction(E1, E2); + + // Compute Jacobians if requested + if (H1) *H1 = numericalDerivative21(errorFunction, E1, E2); + if (H2) *H2 = numericalDerivative22(errorFunction, E1, E2); + + return errors; + } +}; + +/** + * @class EssentialTransferFactorK * @brief Transfers points between views using essential matrices, optimizes for * calibrations of the views, as well. Note that the EssentialMatrixFactor4 does * something similar but without transfer. @@ -143,7 +221,7 @@ class TransferFactor : public NoiseModelFactorN, public TransferEdges { * and computes the error using bulk numerical differentiation. */ template -class EssentialTransferFactor +class EssentialTransferFactorK : public NoiseModelFactorN, TransferEdges { using EM = EssentialMatrix; @@ -152,7 +230,7 @@ class EssentialTransferFactor public: using Base = NoiseModelFactorN; - using This = EssentialTransferFactor; + using This = EssentialTransferFactorK; using shared_ptr = std::shared_ptr; /** @@ -163,9 +241,9 @@ class EssentialTransferFactor * @param triplets A vector of triplets containing (pa, pb, pc) * @param model An optional SharedNoiseModel */ - EssentialTransferFactor(EdgeKey edge1, EdgeKey edge2, - const std::vector& triplets, - const SharedNoiseModel& model = nullptr) + EssentialTransferFactorK(EdgeKey edge1, EdgeKey edge2, + const std::vector& triplets, + const SharedNoiseModel& model = nullptr) : Base(model, edge1, edge2, Symbol('k', ViewA(edge1, edge2)), // calibration key for view a Symbol('k', ViewB(edge1, edge2)), // calibration key for view b @@ -173,6 +251,16 @@ class EssentialTransferFactor 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, + const K& Kc, const Point2& pc) const { + const Point2 pA = Ka.calibrate(pa); + const Point2 pB = Kb.calibrate(pb); + const Point2 pC = EpipolarTransfer(Eca, pA, Ecb, pB); + return Kc.uncalibrate(pC) - pc; + } + /// Evaluate error function Vector evaluateError(const EM& E1, const EM& E2, const K& Ka, const K& Kb, const K& Kc, OptionalMatrixType H1 = nullptr, @@ -187,10 +275,8 @@ class EssentialTransferFactor size_t idx = 0; auto [Eca, Ecb] = this->getMatrices(e1, e2); for (const auto& [pa, pb, pc] : triplets_) { - const Point2 pA = kA.calibrate(pa); - const Point2 pB = kB.calibrate(pb); - const Point2 pC = EpipolarTransfer(Eca, pA, Ecb, pB); - errors.segment<2>(idx) = kC.uncalibrate(pC) - pc; + errors.segment<2>(idx) = + TransferError(Eca, kA, pa, Ecb, kB, pb, kC, pc); idx += 2; } return errors; diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index 80479eb78..a15b73ea1 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -77,11 +77,11 @@ gtsam::Values initialCamerasAndPointsEstimate(const gtsam::SfmData& db); #include #include -template +template virtual class TransferFactor : gtsam::NoiseModelFactor { TransferFactor(gtsam::EdgeKey edge1, gtsam::EdgeKey edge2, - const std::vector>& triplets - ); + const std::vector>& triplets, + const gtsam::noiseModel::Base* model = nullptr); }; #include @@ -90,8 +90,16 @@ virtual class TransferFactor : gtsam::NoiseModelFactor { template virtual class EssentialTransferFactor : gtsam::NoiseModelFactor { EssentialTransferFactor(gtsam::EdgeKey edge1, gtsam::EdgeKey edge2, - const std::vector>& triplets - ); + const std::vector>& triplets, + const K* calibration, + const gtsam::noiseModel::Base* model = nullptr); +}; + +template +virtual class EssentialTransferFactorK : gtsam::NoiseModelFactor { + EssentialTransferFactorK(gtsam::EdgeKey edge1, gtsam::EdgeKey edge2, + 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 b1655e0a9..ced3d2ce7 100644 --- a/gtsam/sfm/tests/testTransferFactor.cpp +++ b/gtsam/sfm/tests/testTransferFactor.cpp @@ -6,10 +6,13 @@ */ #include +#include #include #include #include +#include + using namespace gtsam; using symbol_shorthand::K; @@ -173,7 +176,51 @@ TEST(EssentialTransferFactor, Jacobians) { EdgeKey key02(0, 2); // Create an EssentialTransferFactor - EssentialTransferFactor factor(key01, key02, {{p[1], p[2], p[0]}}); + auto sharedCal = std::make_shared(cal); + EssentialTransferFactor factor(key01, key02, {{p[1], p[2], p[0]}}, + sharedCal); + + // Create Values and insert variables + Values values; + values.insert(key01, E01); // Essential matrix between views 0 and 1 + values.insert(key02, E02); // Essential matrix between views 0 and 2 + + // Check error + Matrix H1, H2, H3, H4, H5; + Vector error = factor.evaluateError(E01, E02, // + &H1, &H2); + EXPECT(H1.rows() == 2 && H1.cols() == 5) + EXPECT(H2.rows() == 2 && H2.cols() == 5) + EXPECT(assert_equal(Vector::Zero(2), error, 1e-9)) + + // Check Jacobians + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-7); +} + +//************************************************************************* +// Test for EssentialTransferFactorK +TEST(EssentialTransferFactorK, Jacobians) { + using namespace fixture; + + // Create EssentialMatrices between cameras + EssentialMatrix E01 = + EssentialMatrix::FromPose3(cameraPoses[0].between(cameraPoses[1])); + EssentialMatrix E02 = + EssentialMatrix::FromPose3(cameraPoses[0].between(cameraPoses[2])); + + // Project a point into the three cameras + const Point3 P(0.1, 0.2, 0.3); + std::array p; + for (size_t i = 0; i < 3; ++i) { + p[i] = cameras[i].project(P); + } + + // Create EdgeKeys + EdgeKey key01(0, 1); + EdgeKey key02(0, 2); + + // Create an EssentialTransferFactorK + EssentialTransferFactorK factor(key01, key02, {{p[1], p[2], p[0]}}); // Create Values and insert variables Values values; @@ -185,10 +232,9 @@ TEST(EssentialTransferFactor, Jacobians) { // Check error Matrix H1, H2, H3, H4, H5; - Vector error = - factor.evaluateError(E01, E02, // - fixture::cal, fixture::cal, fixture::cal, // - &H1, &H2, &H3, &H4, &H5); + Vector error = factor.evaluateError(E01, E02, // + cal, cal, cal, // + &H1, &H2, &H3, &H4, &H5); EXPECT(H1.rows() == 2 && H1.cols() == 5) EXPECT(H2.rows() == 2 && H2.cols() == 5) EXPECT(H3.rows() == 2 && H3.cols() == 5) diff --git a/python/gtsam/examples/ViewGraphComparison.py b/python/gtsam/examples/ViewGraphComparison.py index d8340e393..1eb43cec8 100644 --- a/python/gtsam/examples/ViewGraphComparison.py +++ b/python/gtsam/examples/ViewGraphComparison.py @@ -105,22 +105,18 @@ def build_factor_graph(method, num_cameras, measurements, cal): elif method == "SimpleF": FactorClass = gtsam.TransferFactorSimpleFundamentalMatrix elif method == "Essential+Ks": - FactorClass = gtsam.EssentialTransferFactorCal3f + 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 == "Calibrated": - FactorClass = gtsam.TransferFactorEssentialMatrix + FactorClass = gtsam.EssentialTransferFactorCal3f # No priors on calibration needed else: raise ValueError(f"Unknown method {method}") - if method == "Calibrated": - # Calibrate measurements using ground truth calibration - z = [[cal.calibrate(m) for m in cam_measurements] for cam_measurements in measurements] - else: - z = measurements + z = measurements # shorthand for a in range(num_cameras): b = (a + 1) % num_cameras # Next camera @@ -138,9 +134,14 @@ def build_factor_graph(method, num_cameras, measurements, cal): tuples3.append((z[c][j], z[b][j], z[a][j])) # Add transfer factors between views a, b, and c. - 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)) + 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)) + 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 @@ -279,6 +280,7 @@ def plot_results(results): fig.tight_layout() plt.show() + # Main function def main(): # Parse command line arguments @@ -345,8 +347,6 @@ def main(): # Compute final error final_error = graph.error(result) - if method == "Calibrated": - final_error *= cal.f() * cal.f() # Store results results[method]["distances"].extend(distances)