Use abc terminology

release/4.3a0
Frank Dellaert 2024-10-24 17:54:04 -07:00
parent 19c75cb95c
commit be68e07ddb
2 changed files with 29 additions and 32 deletions

View File

@ -26,59 +26,56 @@ namespace gtsam {
* Binary factor in the context of Structure from Motion (SfM). * Binary factor in the context of Structure from Motion (SfM).
* It is used to transfer points between different views based on the * It is used to transfer points between different views based on the
* fundamental matrices between these views. The factor computes the error * 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. * the target view. Jacobians are done using numerical differentiation.
*/ */
template <typename F> template <typename F>
class TransferFactor : public NoiseModelFactorN<F, F> { class TransferFactor : public NoiseModelFactorN<F, F> {
EdgeKey key1_, key2_; ///< the two EdgeKeys 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: public:
/** /**
* @brief Constructor for the TransferFactor class. * @brief Constructor for the TransferFactor class.
* *
* Uses EdgeKeys to determine how to use the two fundamental matrix unknowns * 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 * F1 and F2, to transfer points pa and pb to the third view, and minimize the
* difference with pk. * difference with pc.
* *
* The edge keys must represent valid edges for the transfer operation, * The edge keys must represent valid edges for the transfer operation,
* specifically one of the following configurations: * specifically one of the following configurations:
* - (i, k) and (j, k) * - (a, c) and (b, c)
* - (i, k) and (k, j) * - (a, c) and (c, b)
* - (k, i) and (j, k) * - (c, a) and (b, c)
* - (k, i) and (k, j) * - (c, a) and (c, b)
* *
* @param key1 First EdgeKey specifying F1: (i, k) or (k, i). * @param key1 First EdgeKey specifying F1: (a, c) or (c, a).
* @param key2 Second EdgeKey specifying F2: (j, k) or (k, j). * @param key2 Second EdgeKey specifying F2: (b, c) or (c, b).
* @param pi The point in the first view (i). * @param pa The point in the first view (a).
* @param pj The point in the second view (j). * @param pb The point in the second view (b).
* @param pk The point in the third (and transfer target) view (k). * @param pc The point in the third (and transfer target) view (c).
* @param model An optional SharedNoiseModel that defines the noise model * @param model An optional SharedNoiseModel that defines the noise model
* for this factor. Defaults to nullptr. * for this factor. Defaults to nullptr.
*/ */
TransferFactor(EdgeKey key1, EdgeKey key2, const Point2& pi, const Point2& pj, TransferFactor(EdgeKey key1, EdgeKey key2, const Point2& pa, const Point2& pb,
const Point2& pk, const SharedNoiseModel& model = nullptr) const Point2& pc, const SharedNoiseModel& model = nullptr)
: NoiseModelFactorN<F, F>(model, key1, key2), : NoiseModelFactorN<F, F>(model, key1, key2),
key1_(key1), key1_(key1),
key2_(key2), key2_(key2),
pi(pi), pa(pa),
pj(pj), pb(pb),
pk(pk) {} pc(pc) {}
// Create Matrix3 objects based on EdgeKey configurations // Create Matrix3 objects based on EdgeKey configurations
static std::pair<Matrix3, Matrix3> getMatrices(const EdgeKey& key1, std::pair<Matrix3, Matrix3> getMatrices(const F& F1, const F& F2) const {
const F& F1, // Fill Fca and Fcb based on EdgeKey configurations
const EdgeKey& key2, if (key1_.i() == key2_.i()) {
const F& F2) {
// Fill Fki and Fkj based on EdgeKey configurations
if (key1.i() == key2.i()) {
return {F1.matrix(), F2.matrix()}; return {F1.matrix(), F2.matrix()};
} else if (key1.i() == key2.j()) { } else if (key1_.i() == key2_.j()) {
return {F1.matrix(), F2.matrix().transpose()}; return {F1.matrix(), F2.matrix().transpose()};
} else if (key1.j() == key2.i()) { } else if (key1_.j() == key2_.i()) {
return {F1.matrix().transpose(), F2.matrix()}; 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()}; return {F1.matrix().transpose(), F2.matrix().transpose()};
} else { } else {
throw std::runtime_error( throw std::runtime_error(
@ -91,12 +88,12 @@ class TransferFactor : public NoiseModelFactorN<F, F> {
OptionalMatrixType H1 = nullptr, OptionalMatrixType H1 = nullptr,
OptionalMatrixType H2 = nullptr) const override { OptionalMatrixType H2 = nullptr) const override {
std::function<Point2(F, F)> transfer = [&](const F& F1, const F& F2) { std::function<Point2(F, F)> transfer = [&](const F& F1, const F& F2) {
auto [Fki, Fkj] = getMatrices(key1_, F1, key2_, F2); auto [Fca, Fcb] = getMatrices(F1, F2);
return Transfer(Fki, pi, Fkj, pj); return Transfer(Fca, pa, Fcb, pb);
}; };
if (H1) *H1 = numericalDerivative21<Point2, F, F>(transfer, F1, F2); if (H1) *H1 = numericalDerivative21<Point2, F, F>(transfer, F1, F2);
if (H2) *H2 = numericalDerivative22<Point2, F, F>(transfer, F1, F2); if (H2) *H2 = numericalDerivative22<Point2, F, F>(transfer, F1, F2);
return transfer(F1, F2) - pk; return transfer(F1, F2) - pc;
} }
}; };

View File

@ -71,7 +71,7 @@ TEST(TransferFactor, Jacobians) {
factor2{key20, key12, p[0], p[1], p[2]}; factor2{key20, key12, p[0], p[1], p[2]};
// Check that getMatrices is correct // 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<Matrix3>(triplet.Fca.matrix(), Fki)); EXPECT(assert_equal<Matrix3>(triplet.Fca.matrix(), Fki));
EXPECT(assert_equal<Matrix3>(triplet.Fbc.matrix().transpose(), Fkj)); EXPECT(assert_equal<Matrix3>(triplet.Fbc.matrix().transpose(), Fkj));