diff --git a/gtsam/sfm/TransferFactor.h b/gtsam/sfm/TransferFactor.h index fd65db8b9..0f9f95c13 100644 --- a/gtsam/sfm/TransferFactor.h +++ b/gtsam/sfm/TransferFactor.h @@ -17,56 +17,83 @@ #include #include +#include #include namespace gtsam { -template -struct TripletError { - Point2 p0, p1, p2; - - /// vector of errors returns 6D vector - Vector evaluateError(const F& F01, const F& F12, const F& F20, // - Matrix* H01, Matrix* H12, Matrix* H20) const { - std::function fn = [&](const F& F01, const F& F12, - const F& F20) { - Vector6 error; - error << // - F::transfer(F01.matrix(), p1, F20.matrix().transpose(), p2) - p0, - F::transfer(F01.matrix().transpose(), p0, F12.matrix(), p2) - p1, - F::transfer(F20.matrix(), p0, F12.matrix().transpose(), p1) - p2; - return error; - }; - if (H01) *H01 = numericalDerivative31(fn, F01, F12, F20); - if (H12) *H12 = numericalDerivative32(fn, F01, F12, F20); - if (H20) *H20 = numericalDerivative33(fn, F01, F12, F20); - return fn(F01, F12, F20); - } -}; - +/** + * Binary factor in the context of Structure from Motion (SfM). + * It is used to transfer points between different views based on the + * fundamental matrices between these views. The factor computes the error + * between the transferred points `pi` and `pj`, and the actual point `pk` in + * the target view. Jacobians are done using numerical differentiation. + */ template class TransferFactor : public NoiseModelFactorN { - Point2 p0, p1, p2; + EdgeKey key1_, key2_; ///< the two EdgeKeys + Point2 pi, pj, pk; ///< The points in the three views public: - // Constructor - TransferFactor(Key key1, Key key2, const Point2& p0, const Point2& p1, - const Point2& p2, const SharedNoiseModel& model = nullptr) - : NoiseModelFactorN(model, key1, key2), p0(p0), p1(p1), p2(p2) {} + /** + * @brief Constructor for the TransferFactor class. + * + * Uses EdgeKeys to determine how to use the two fundamental matrix unknowns + * F1 and F2, to transfer points pi and pj to the third view, and minimize the + * difference with pk. + * + * The edge keys must represent valid edges for the transfer operation, + * specifically one of the following configurations: + * - (i, k) and (j, k) + * - (i, k) and (k, j) + * - (k, i) and (j, k) + * - (k, i) and (k, j) + * + * @param key1 First EdgeKey specifying F1: (i, k) or (k, i). + * @param key2 Second EdgeKey specifying F2: (j, k) or (k, j). + * @param pi The point in the first view (i). + * @param pj The point in the second view (j). + * @param pk The point in the third (and transfer target) view (k). + * @param model An optional SharedNoiseModel that defines the noise model + * for this factor. Defaults to nullptr. + */ + TransferFactor(EdgeKey key1, EdgeKey key2, const Point2& pi, const Point2& pj, + const Point2& pk, const SharedNoiseModel& model = nullptr) + : NoiseModelFactorN(model, key1, key2), + key1_(key1), + key2_(key2), + pi(pi), + pj(pj), + pk(pk) {} + + // Create Matrix3 objects based on EdgeKey configurations + std::pair getMatrices(const F& F1, const F& F2) const { + // Fill Fki and Fkj based on EdgeKey configurations + if (key1_.i() == key2_.i()) { + return {F1.matrix(), F2.matrix()}; + } else if (key1_.i() == key2_.j()) { + return {F1.matrix(), F2.matrix().transpose()}; + } else if (key1_.j() == key2_.i()) { + return {F1.matrix().transpose(), F2.matrix()}; + } else if (key1_.j() == key2_.j()) { + return {F1.matrix().transpose(), F2.matrix().transpose()}; + } else { + throw std::runtime_error( + "TransferFactor: invalid EdgeKey configuration."); + } + } /// vector of errors returns 2D vector - Vector evaluateError(const F& F12, const F& F20, // - OptionalMatrixType H12 = nullptr, - OptionalMatrixType H20 = nullptr) const override { - std::function fn = [&](const F& F12, const F& F20) { - Vector2 error; - error << // - F::transfer(F20.matrix(), p0, F12.matrix().transpose(), p1) - p2; - return error; + Vector evaluateError(const F& F1, const F& F2, + OptionalMatrixType H1 = nullptr, + OptionalMatrixType H2 = nullptr) const override { + std::function transfer = [&](const F& F1, const F& F2) { + auto [Fki, Fkj] = getMatrices(F1, F2); + return F::transfer(Fki, pi, Fkj, pj); }; - if (H12) *H12 = numericalDerivative21(fn, F12, F20); - if (H20) *H20 = numericalDerivative22(fn, F12, F20); - return fn(F12, F20); + if (H1) *H1 = numericalDerivative21(transfer, F1, F2); + if (H2) *H2 = numericalDerivative22(transfer, F1, F2); + return transfer(F1, F2) - pk; } }; diff --git a/gtsam/sfm/tests/testTransferFactor.cpp b/gtsam/sfm/tests/testTransferFactor.cpp index afceb8fdb..7bcd71b1f 100644 --- a/gtsam/sfm/tests/testTransferFactor.cpp +++ b/gtsam/sfm/tests/testTransferFactor.cpp @@ -6,20 +6,15 @@ */ #include -#include -#include -#include -#include -#include -#include #include -#include -#include #include #include using namespace gtsam; +double focalLength = 1000; +Point2 principalPoint(640 / 2, 480 / 2); + //************************************************************************* /// Generate three cameras on a circle, looking in std::array generateCameraPoses() { @@ -34,6 +29,7 @@ std::array generateCameraPoses() { return cameraPoses; } +//************************************************************************* // Function to generate a TripleF from camera poses TripleF generateTripleF( const std::array& cameraPoses) { @@ -48,9 +44,7 @@ TripleF generateTripleF( return {F[0], F[1], F[2]}; // Return a TripleF instance } -double focalLength = 1000; -Point2 principalPoint(640 / 2, 480 / 2); - +//************************************************************************* // Test for TransferFactor TEST(TransferFactor, Jacobians) { // Generate cameras on a circle @@ -70,28 +64,29 @@ TEST(TransferFactor, Jacobians) { } // Create a TransferFactor - TripletError error{p[0], p[1], p[2]}; - Matrix H01, H12, H20; - Vector e = error.evaluateError(triplet.F01, triplet.F12, triplet.F20, &H01, - &H12, &H20); - std::cout << "Error: " << e << std::endl; - std::cout << H01 << std::endl << std::endl; - std::cout << H12 << std::endl << std::endl; - std::cout << H20 << std::endl; + EdgeKey key01(0, 1), key12(1, 2), key20(2, 0); + TransferFactor // + factor0{key01, key20, p[1], p[2], p[0]}, + factor1{key12, key01, p[2], p[0], p[1]}, + factor2{key20, key12, p[0], p[1], p[2]}; - // Create a TransferFactor - TransferFactor factor{0, 1, p[0], p[1], p[2]}; - Matrix H0, H1; - Vector e2 = factor.evaluateError(triplet.F12, triplet.F20, &H0, &H1); - std::cout << "Error: " << e2 << std::endl; - std::cout << H0 << std::endl << std::endl; - std::cout << H1 << std::endl << std::endl; + // Check that getMatrices is correct + auto [Fki, Fkj] = factor2.getMatrices(triplet.Fca, triplet.Fbc); + EXPECT(assert_equal(triplet.Fca.matrix(), Fki)); + EXPECT(assert_equal(triplet.Fbc.matrix().transpose(), Fkj)); - // Check Jacobians + // Create Values with edge keys Values values; - values.insert(0, triplet.F12); - values.insert(1, triplet.F20); - EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-7); + values.insert(key01, triplet.Fab); + values.insert(key12, triplet.Fbc); + values.insert(key20, triplet.Fca); + + // Check error and Jacobians + for (auto&& f : {factor0, factor1, factor2}) { + Vector error = f.unwhitenedError(values); + EXPECT(assert_equal(Z_2x1, error)); + EXPECT_CORRECT_FACTOR_JACOBIANS(f, values, 1e-5, 1e-7); + } } //*************************************************************************