Use abc terminology
parent
19c75cb95c
commit
be68e07ddb
|
@ -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,13 +88,13 @@ 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;
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
} // namespace gtsam
|
} // namespace gtsam
|
|
@ -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));
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue