12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Got rid of inheritance

release/4.3a0
Frank Dellaert 2024-10-24 15:08:29 -07:00
parent ade1207334
commit bf47ef3432
2 changed files with 11 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
gtsam/sfm/TransferFactor.h
View File

@ -67,15 +67,18 @@ class TransferFactor : public NoiseModelFactorN<F, F> {
pk(pk) {} pk(pk) {}
// Create Matrix3 objects based on EdgeKey configurations // Create Matrix3 objects based on EdgeKey configurations
std::pair<Matrix3, Matrix3> getMatrices(const F& F1, const F& F2) const { 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 // Fill Fki and Fkj based on EdgeKey configurations
if (key1_.i() == key2_.i()) { 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(
@ -88,8 +91,8 @@ 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(F1, F2); auto [Fki, Fkj] = getMatrices(key1_, F1, key2_, F2);
return F::transfer(Fki, pi, Fkj, pj); return Transfer(Fki, pi, Fkj, pj);
}; };
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);

2
gtsam/sfm/tests/testTransferFactor.cpp
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(triplet.Fca, triplet.Fbc); auto [Fki, Fkj] = factor2.getMatrices(key20, triplet.Fca, key12, 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));