diff --git a/gtsam/sfm/TransferFactor.h b/gtsam/sfm/TransferFactor.h index 5c46f9b9b..8acde7b88 100644 --- a/gtsam/sfm/TransferFactor.h +++ b/gtsam/sfm/TransferFactor.h @@ -26,59 +26,56 @@ namespace gtsam { * 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 + * between the transferred points `pa` and `pb`, and the actual point `pc` in * the target view. Jacobians are done using numerical differentiation. */ template class TransferFactor : public NoiseModelFactorN { EdgeKey key1_, key2_; ///< the two EdgeKeys - Point2 pi, pj, pk; ///< The points in the three views + Point2 pa, pb, pc; ///< The points in the three views public: /** * @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. + * F1 and F2, to transfer points pa and pb to the third view, and minimize the + * difference with pc. * * 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) + * - (a, c) and (b, c) + * - (a, c) and (c, b) + * - (c, a) and (b, c) + * - (c, a) and (c, b) * - * @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 key1 First EdgeKey specifying F1: (a, c) or (c, a). + * @param key2 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& pi, const Point2& pj, - const Point2& pk, const SharedNoiseModel& model = 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), - pi(pi), - pj(pj), - pk(pk) {} + pa(pa), + pb(pb), + pc(pc) {} // Create Matrix3 objects based on EdgeKey configurations - static std::pair getMatrices(const EdgeKey& key1, - const F& F1, - const EdgeKey& key2, - const F& F2) { - // Fill Fki and Fkj based on EdgeKey configurations - if (key1.i() == key2.i()) { + 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()) { + } else if (key1_.i() == key2_.j()) { return {F1.matrix(), F2.matrix().transpose()}; - } else if (key1.j() == key2.i()) { + } else if (key1_.j() == key2_.i()) { return {F1.matrix().transpose(), F2.matrix()}; - } else if (key1.j() == key2.j()) { + } else if (key1_.j() == key2_.j()) { return {F1.matrix().transpose(), F2.matrix().transpose()}; } else { throw std::runtime_error( @@ -91,13 +88,13 @@ class TransferFactor : public NoiseModelFactorN { OptionalMatrixType H1 = nullptr, OptionalMatrixType H2 = nullptr) const override { std::function transfer = [&](const F& F1, const F& F2) { - auto [Fki, Fkj] = getMatrices(key1_, F1, key2_, F2); - return Transfer(Fki, pi, Fkj, pj); + auto [Fca, Fcb] = getMatrices(F1, F2); + return Transfer(Fca, pa, Fcb, pb); }; if (H1) *H1 = numericalDerivative21(transfer, F1, F2); if (H2) *H2 = numericalDerivative22(transfer, F1, F2); - return transfer(F1, F2) - pk; + return transfer(F1, F2) - pc; } }; -} // namespace gtsam +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/sfm/tests/testTransferFactor.cpp b/gtsam/sfm/tests/testTransferFactor.cpp index d26bad960..7bcd71b1f 100644 --- a/gtsam/sfm/tests/testTransferFactor.cpp +++ b/gtsam/sfm/tests/testTransferFactor.cpp @@ -71,7 +71,7 @@ TEST(TransferFactor, Jacobians) { factor2{key20, key12, p[0], p[1], p[2]}; // Check that getMatrices is correct - auto [Fki, Fkj] = factor2.getMatrices(key20, triplet.Fca, key12, triplet.Fbc); + auto [Fki, Fkj] = factor2.getMatrices(triplet.Fca, triplet.Fbc); EXPECT(assert_equal(triplet.Fca.matrix(), Fki)); EXPECT(assert_equal(triplet.Fbc.matrix().transpose(), Fkj));