diff --git a/gtsam/sfm/TransferFactor.h b/gtsam/sfm/TransferFactor.h index 13de4eb29..9704810dd 100644 --- a/gtsam/sfm/TransferFactor.h +++ b/gtsam/sfm/TransferFactor.h @@ -16,37 +16,63 @@ #pragma once #include +#include #include #include +#include #include +#include + namespace gtsam { /** * Base class that holds the EdgeKeys and provides the getMatrices method. */ template -struct TransferEdges { - EdgeKey edge1, edge2; ///< The two EdgeKeys. +class TransferEdges { + protected: + EdgeKey edge1_, edge2_; ///< The two EdgeKeys. + uint32_t c_; ///< The transfer target - TransferEdges(EdgeKey edge1, EdgeKey edge2) : edge1(edge1), edge2(edge2) {} + public: + TransferEdges(EdgeKey edge1, EdgeKey edge2) + : edge1_(edge1), edge2_(edge2), c_(viewC(edge1, edge2)) {} - // Create Matrix3 objects based on EdgeKey configurations. - std::pair getMatrices(const F& F1, const F& F2) const { - // Fill Fca and Fcb based on EdgeKey configurations. - if (edge1.i() == edge2.i()) { - return {F1.matrix(), F2.matrix()}; - } else if (edge1.i() == edge2.j()) { - return {F1.matrix(), F2.matrix().transpose()}; - } else if (edge1.j() == edge2.i()) { - return {F1.matrix().transpose(), F2.matrix()}; - } else if (edge1.j() == edge2.j()) { - return {F1.matrix().transpose(), F2.matrix().transpose()}; - } else { + /// Returns the view A index based on the EdgeKeys + static size_t viewA(const EdgeKey& edge1, const EdgeKey& edge2) { + size_t c = viewC(edge1, edge2); + return (edge1.i() == c) ? edge1.j() : edge1.i(); + } + + /// Returns the view B index based on the EdgeKeys + static size_t viewB(const EdgeKey& edge1, const EdgeKey& edge2) { + size_t c = viewC(edge1, edge2); + return (edge2.i() == c) ? edge2.j() : edge2.i(); + } + + /// Returns the view C index based on the EdgeKeys + static size_t viewC(const EdgeKey& edge1, const EdgeKey& edge2) { + if (edge1.i() == edge2.i() || edge1.i() == edge2.j()) + return edge1.i(); + else if (edge1.j() == edge2.i() || edge1.j() == edge2.j()) + return edge1.j(); + else throw std::runtime_error( - "TransferEdges: invalid EdgeKey configuration between edge1 (" + - std::string(edge1) + ") and edge2 (" + std::string(edge2) + ")."); - } + "EssentialTransferFactor: No common key in edge keys."); + } + + /// Create Matrix3 objects based on EdgeKey configurations. + std::pair getMatrices(const F& F1, const F& F2) const { + // Determine whether to transpose F1 + const Matrix3 Fca = + edge1_.i() == c_ ? F1.matrix() : F1.matrix().transpose(); + + // Determine whether to transpose F2 + const Matrix3 Fcb = + edge2_.i() == c_ ? F2.matrix() : F2.matrix().transpose(); + + return {Fca, Fcb}; } }; @@ -64,26 +90,6 @@ class TransferFactor : public NoiseModelFactorN, public TransferEdges { std::vector triplets_; ///< Point triplets. public: - /** - * @brief Constructor for a single triplet of points. - * - * @note Batching all points for the same transfer will be much faster. - * - * @param edge1 First EdgeKey specifying F1: (a, c) or (c, a). - * @param edge2 Second EdgeKey specifying F2: (b, c) or (c, b). - * @param pa The point in the first view (a). - * @param pb The point in the second view (b). - * @param pc The point in the third (and transfer target) view (c). - * @param model An optional SharedNoiseModel that defines the noise model - * for this factor. Defaults to nullptr. - */ - TransferFactor(EdgeKey edge1, EdgeKey edge2, const Point2& pa, - const Point2& pb, const Point2& pc, - const SharedNoiseModel& model = nullptr) - : Base(model, edge1, edge2), - TransferEdges(edge1, edge2), - triplets_({std::make_tuple(pa, pb, pc)}) {} - /** * @brief Constructor that accepts a vector of point triplets. * @@ -104,25 +110,107 @@ class TransferFactor : public NoiseModelFactorN, public TransferEdges { Vector evaluateError(const F& F1, const F& F2, OptionalMatrixType H1 = nullptr, OptionalMatrixType H2 = nullptr) const override { - std::function errorFunction = [&](const F& F1, - const F& F2) { + std::function errorFunction = [&](const F& f1, + const F& f2) { Vector errors(2 * triplets_.size()); size_t idx = 0; - auto [Fca, Fcb] = this->getMatrices(F1, F2); - for (const auto& tuple : triplets_) { - const auto& [pa, pb, pc] = tuple; - Point2 transferredPoint = EpipolarTransfer(Fca, pa, Fcb, pb); - Vector2 error = transferredPoint - pc; - errors.segment<2>(idx) = error; + auto [Fca, Fcb] = this->getMatrices(f1, f2); + for (const auto& [pa, pb, pc] : triplets_) { + errors.segment<2>(idx) = EpipolarTransfer(Fca, pa, Fcb, pb) - pc; idx += 2; } return errors; }; - if (H1) *H1 = numericalDerivative21(errorFunction, F1, F2); - if (H2) *H2 = numericalDerivative22(errorFunction, F1, F2); - return errorFunction(F1, F2); + if (H1) *H1 = numericalDerivative21(errorFunction, F1, F2); + if (H2) *H2 = numericalDerivative22(errorFunction, F1, F2); + return errorFunction(F1, F2); } }; +/** + * @class EssentialTransferFactor + * @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. + * + * @note Derives calibration keys from edges, and uses symbol 'k'. + * + * This factor is templated on the calibration class K and extends + * the TransferFactor for EssentialMatrices. It involves two essential matrices + * and three calibration objects (Ka, Kb, Kc). 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 NoiseModelFactorN, + TransferEdges { + using EM = EssentialMatrix; + using Triplet = std::tuple; + std::vector triplets_; ///< Point triplets + + public: + using Base = NoiseModelFactorN; + using This = EssentialTransferFactor; + using shared_ptr = std::shared_ptr; + + /** + * @brief Constructor that accepts a vector of point triplets. + * + * @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 model An optional SharedNoiseModel + */ + EssentialTransferFactor(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 + Symbol('k', viewC(edge1, edge2))), // calibration key for target c + TransferEdges(edge1, edge2), + triplets_(triplets) {} + + /// Evaluate error function + Vector evaluateError(const EM& E1, const EM& E2, const K& Ka, const K& Kb, + const K& Kc, OptionalMatrixType H1 = nullptr, + OptionalMatrixType H2 = nullptr, + OptionalMatrixType H3 = nullptr, + OptionalMatrixType H4 = nullptr, + OptionalMatrixType H5 = nullptr) const override { + std::function + errorFunction = [&](const EM& e1, const EM& e2, const K& kA, + const K& kB, const K& kC) { + Vector errors(2 * triplets_.size()); + 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; + idx += 2; + } + return errors; + }; + + // Compute error + Vector errors = errorFunction(E1, E2, Ka, Kb, Kc); + + // Compute Jacobians if requested + if (H1) *H1 = numericalDerivative51(errorFunction, E1, E2, Ka, Kb, Kc); + if (H2) *H2 = numericalDerivative52(errorFunction, E1, E2, Ka, Kb, Kc); + if (H3) *H3 = numericalDerivative53(errorFunction, E1, E2, Ka, Kb, Kc); + if (H4) *H4 = numericalDerivative54(errorFunction, E1, E2, Ka, Kb, Kc); + if (H5) *H5 = numericalDerivative55(errorFunction, E1, E2, Ka, Kb, Kc); + + return errors; + } + + /// Return the dimension of the factor + size_t dim() const override { return 2 * triplets_.size(); } +}; + } // namespace gtsam \ No newline at end of file diff --git a/gtsam/sfm/tests/testTransferFactor.cpp b/gtsam/sfm/tests/testTransferFactor.cpp index 2dca12c2a..b8eed3b3a 100644 --- a/gtsam/sfm/tests/testTransferFactor.cpp +++ b/gtsam/sfm/tests/testTransferFactor.cpp @@ -1,8 +1,8 @@ /* * @file testTransferFactor.cpp - * @brief Test TransferFactor class - * @author Your Name - * @date October 23, 2024 + * @brief Test TransferFactor classes + * @author Frank Dellaert + * @date October 2024 */ #include @@ -11,9 +11,10 @@ #include using namespace gtsam; +using symbol_shorthand::K; //************************************************************************* -/// Generate three cameras on a circle, looking in +/// Generate three cameras on a circle, looking inwards std::array generateCameraPoses() { std::array cameraPoses; const double radius = 1.0; @@ -82,9 +83,9 @@ TEST(TransferFactor, Jacobians) { // Create a TransferFactor 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]}; + 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 Values with edge keys Values values; @@ -153,9 +154,68 @@ TEST(TransferFactor, MultipleTuples) { EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-7); } +//************************************************************************* +// Test for EssentialTransferFactor +TEST(EssentialTransferFactor, Jacobians) { + // Generate cameras on a circle + std::array cameraPoses = generateCameraPoses(); + + // Create calibration + const Cal3_S2 commonK(focalLength, focalLength, 0.0, principalPoint.x(), + principalPoint.y()); + + // Create cameras + std::array, 3> cameras; + for (size_t i = 0; i < 3; ++i) { + cameras[i] = PinholeCamera(cameraPoses[i], commonK); + } + + // 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 EssentialTransferFactor + EssentialTransferFactor factor(key01, key02, {{p[1], p[2], p[0]}}); + + // 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 + values.insert(K(1), commonK); // Calibration for view A (view 1) + values.insert(K(2), commonK); // Calibration for view B (view 2) + values.insert(K(0), commonK); // Calibration for view C (view 0) + + // Check error + Matrix H1, H2, H3, H4, H5; + Vector error = factor.evaluateError(E01, E02, commonK, commonK, commonK, // + &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) + EXPECT(H4.rows() == 2 && H4.cols() == 5) + EXPECT(H5.rows() == 2 && H5.cols() == 5) + EXPECT(assert_equal(Vector::Zero(2), error, 1e-9)) + + // Check Jacobians + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-7); +} + //************************************************************************* int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -//************************************************************************* +//************************************************************************* \ No newline at end of file