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).
* 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 <typename F>
class TransferFactor : public NoiseModelFactorN<F, F> {
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<F, F>(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<Matrix3, Matrix3> 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<Matrix3, Matrix3> 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,12 +88,12 @@ class TransferFactor : public NoiseModelFactorN<F, F> {
OptionalMatrixType H1 = nullptr,
OptionalMatrixType H2 = nullptr) const override {
std::function<Point2(F, F)> 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<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]};
// 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.Fbc.matrix().transpose(), Fkj));