Use abc terminology
parent
19c75cb95c
commit
be68e07ddb
|
@ -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;
|
||||
}
|
||||
};
|
||||
|
||||
|
|
|
@ -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));
|
||||
|
||||
|
|
Loading…
Reference in New Issue