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

TripletError vs TransferFactor

release/4.3a0
Frank Dellaert 2024-10-24 12:21:37 -07:00
parent d44cca770d
commit 34182bddda
2 changed files with 32 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
gtsam/sfm/TransferFactor.h
View File

@ -22,13 +22,12 @@
namespace gtsam { namespace gtsam {
template <typename F> template <typename F>
struct TransferFactor { struct TripletError {
Point2 p0, p1, p2; Point2 p0, p1, p2;
/// vector of errors returns 6D vector /// vector of errors returns 6D vector
Vector evaluateError(const F& F01, const F& F12, const F& F20, // Vector evaluateError(const F& F01, const F& F12, const F& F20, //
Matrix* H01, Matrix* H12, Matrix* H20) const { Matrix* H01, Matrix* H12, Matrix* H20) const {
Vector error(6);
std::function<Vector6(F, F, F)> fn = [&](const F& F01, const F& F12, std::function<Vector6(F, F, F)> fn = [&](const F& F01, const F& F12,
const F& F20) { const F& F20) {
Vector6 error; Vector6 error;
@ -45,4 +44,23 @@ struct TransferFactor {
} }
}; };
template <typename F>
struct TransferFactor {
Point2 p0, p1, p2;
/// vector of errors returns 2D vector
Vector evaluateError(const F& F12, const F& F20, //
Matrix* H12, Matrix* H20) const {
std::function<Vector2(F, F)> fn = [&](const F& F12, const F& F20) {
Vector2 error;
error << //
F::transfer(F20.matrix(), p0, F12.matrix().transpose(), p1) - p2;
return error;
};
if (H12) *H12 = numericalDerivative21<Vector2, F, F>(fn, F12, F20);
if (H20) *H20 = numericalDerivative22<Vector2, F, F>(fn, F12, F20);
return fn(F12, F20);
}
};
} // namespace gtsam } // namespace gtsam

17
gtsam/sfm/tests/testTransferFactor.cpp
View File

@ -70,18 +70,25 @@ TEST(TransferFactor, Jacobians) {
} }
// Create a TransferFactor // Create a TransferFactor
TransferFactor<SimpleFundamentalMatrix> factor{p[0], p[1], p[2]}; TripletError<SimpleFundamentalMatrix> error{p[0], p[1], p[2]};
Matrix H01, H12, H20; Matrix H01, H12, H20;
Vector error = factor.evaluateError(triplet.F01, triplet.F12, triplet.F20, Vector e = error.evaluateError(triplet.F01, triplet.F12, triplet.F20, &H01,
&H01, &H12, &H20); &H12, &H20);
std::cout << "Error: " << error << std::endl; std::cout << "Error: " << e << std::endl;
std::cout << H01 << std::endl << std::endl; std::cout << H01 << std::endl << std::endl;
std::cout << H12 << std::endl << std::endl; std::cout << H12 << std::endl << std::endl;
std::cout << H20 << std::endl; std::cout << H20 << std::endl;
// Create a TransferFactor
TransferFactor<SimpleFundamentalMatrix> factor{p[0], p[1], p[2]};
Matrix H0, H1;
Vector e2 = factor.evaluateError(triplet.F12, triplet.F20, &H0, &H1);
std::cout << "Error: " << e2 << std::endl;
std::cout << H0 << std::endl << std::endl;
std::cout << H1 << std::endl << std::endl;
// Check Jacobians // Check Jacobians
Values values; Values values;
values.insert(0, triplet.F01);
values.insert(1, triplet.F12); values.insert(1, triplet.F12);
values.insert(2, triplet.F20); values.insert(2, triplet.F20);
// EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-7); // EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-7);