diff --git a/gtsam/sfm/TransferFactor.h b/gtsam/sfm/TransferFactor.h index d2085f57e..13de4eb29 100644 --- a/gtsam/sfm/TransferFactor.h +++ b/gtsam/sfm/TransferFactor.h @@ -22,6 +22,34 @@ namespace gtsam { +/** + * Base class that holds the EdgeKeys and provides the getMatrices method. + */ +template +struct TransferEdges { + EdgeKey edge1, edge2; ///< The two EdgeKeys. + + TransferEdges(EdgeKey edge1, EdgeKey edge2) : edge1(edge1), edge2(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 { + throw std::runtime_error( + "TransferEdges: invalid EdgeKey configuration between edge1 (" + + std::string(edge1) + ") and edge2 (" + std::string(edge2) + ")."); + } + } +}; + /** * Binary factor in the context of Structure from Motion (SfM). * It is used to transfer transfer corresponding points from two views to a @@ -30,76 +58,57 @@ namespace gtsam { * the target view. Jacobians are done using numerical differentiation. */ template -class TransferFactor : public NoiseModelFactorN { - EdgeKey key1_, key2_; ///< the two EdgeKeys - std::vector> - triplets_; ///< Point triplets +class TransferFactor : public NoiseModelFactorN, public TransferEdges { + using Base = NoiseModelFactorN; + using Triplet = std::tuple; + std::vector triplets_; ///< Point triplets. public: /** - * @brief Constructor for a single triplet of points + * @brief Constructor for a single triplet of points. * - * @note: batching all points for the same transfer will be much faster. + * @note Batching all points for the same transfer will be much faster. * - * @param key1 First EdgeKey specifying F1: (a, c) or (c, a). - * @param key2 Second EdgeKey specifying F2: (b, c) or (c, b). + * @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 key1, EdgeKey key2, const Point2& pa, const Point2& pb, - const Point2& pc, const SharedNoiseModel& model = nullptr) - : NoiseModelFactorN(model, key1, key2), - key1_(key1), - key2_(key2), + 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. * - * @param key1 First EdgeKey specifying F1: (a, c) or (c, a). - * @param key2 Second EdgeKey specifying F2: (b, c) or (c, b). + * @param edge1 First EdgeKey specifying F1: (a, c) or (c, a). + * @param edge2 Second EdgeKey specifying F2: (b, c) or (c, b). * @param triplets A vector of triplets containing (pa, pb, pc). * @param model An optional SharedNoiseModel that defines the noise model * for this factor. Defaults to nullptr. */ - TransferFactor( - EdgeKey key1, EdgeKey key2, - const std::vector>& triplets, - const SharedNoiseModel& model = nullptr) - : NoiseModelFactorN(model, key1, key2), - key1_(key1), - key2_(key2), + TransferFactor(EdgeKey edge1, EdgeKey edge2, + const std::vector& triplets, + const SharedNoiseModel& model = nullptr) + : Base(model, edge1, edge2), + TransferEdges(edge1, edge2), triplets_(triplets) {} - // 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 (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 2*N vector + /// Vector of errors returns 2*N vector. 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) { + std::function errorFunction = [&](const F& F1, + const F& F2) { Vector errors(2 * triplets_.size()); size_t idx = 0; - auto [Fca, Fcb] = getMatrices(F1, F2); + 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); @@ -109,9 +118,10 @@ class TransferFactor : public NoiseModelFactorN { } return errors; }; - if (H1) *H1 = numericalDerivative21(transfer, F1, F2); - if (H2) *H2 = numericalDerivative22(transfer, F1, F2); - return transfer(F1, F2); + if (H1) *H1 = numericalDerivative21(errorFunction, F1, F2); + if (H2) *H2 = numericalDerivative22(errorFunction, F1, F2); + return errorFunction(F1, F2); + } };