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

True factor

release/4.3a0
Frank Dellaert 2024-10-24 13:23:00 -07:00
parent 68c9da8c4e
commit e6bfcada40
2 changed files with 13 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
gtsam/sfm/TransferFactor.h
View File

@ -45,12 +45,19 @@ struct TripletError {
};
template <typename F>
struct TransferFactor {
class TransferFactor : public NoiseModelFactorN<F, F> {
Point2 p0, p1, p2;
public:
// Constructor
TransferFactor(Key key1, Key key2, const Point2& p0, const Point2& p1,
const Point2& p2, const SharedNoiseModel& model = nullptr)
: NoiseModelFactorN<F, F>(model, key1, key2), p0(p0), p1(p1), p2(p2) {}
/// vector of errors returns 2D vector
Vector evaluateError(const F& F12, const F& F20, //
Matrix* H12, Matrix* H20) const {
OptionalMatrixType H12 = nullptr,
OptionalMatrixType H20 = nullptr) const override {
std::function<Vector2(F, F)> fn = [&](const F& F12, const F& F20) {
Vector2 error;
error << //

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

@ -80,7 +80,7 @@ TEST(TransferFactor, Jacobians) {
std::cout << H20 << std::endl;
// Create a TransferFactor
TransferFactor<SimpleFundamentalMatrix> factor{p[0], p[1], p[2]};
TransferFactor<SimpleFundamentalMatrix> factor{0, 1, 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;
@ -89,9 +89,9 @@ TEST(TransferFactor, Jacobians) {
// Check Jacobians
Values values;
values.insert(1, triplet.F12);
values.insert(2, triplet.F20);
// EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-7);
values.insert(0, triplet.F12);
values.insert(1, triplet.F20);
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-7);
}
//*************************************************************************