diff --git a/gtsam/sfm/TransferFactor.h b/gtsam/sfm/TransferFactor.h index 43142e762..cec7a445e 100644 --- a/gtsam/sfm/TransferFactor.h +++ b/gtsam/sfm/TransferFactor.h @@ -22,13 +22,12 @@ namespace gtsam { template -struct TransferFactor { +struct TripletError { Point2 p0, p1, p2; /// vector of errors returns 6D vector Vector evaluateError(const F& F01, const F& F12, const F& F20, // Matrix* H01, Matrix* H12, Matrix* H20) const { - Vector error(6); std::function fn = [&](const F& F01, const F& F12, const F& F20) { Vector6 error; @@ -45,4 +44,23 @@ struct TransferFactor { } }; +template +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 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(fn, F12, F20); + if (H20) *H20 = numericalDerivative22(fn, F12, F20); + return fn(F12, F20); + } +}; + } // namespace gtsam diff --git a/gtsam/sfm/tests/testTransferFactor.cpp b/gtsam/sfm/tests/testTransferFactor.cpp index 3c3e89693..44b03ad5d 100644 --- a/gtsam/sfm/tests/testTransferFactor.cpp +++ b/gtsam/sfm/tests/testTransferFactor.cpp @@ -70,18 +70,25 @@ TEST(TransferFactor, Jacobians) { } // Create a TransferFactor - TransferFactor factor{p[0], p[1], p[2]}; + TripletError error{p[0], p[1], p[2]}; Matrix H01, H12, H20; - Vector error = factor.evaluateError(triplet.F01, triplet.F12, triplet.F20, - &H01, &H12, &H20); - std::cout << "Error: " << error << std::endl; + Vector e = error.evaluateError(triplet.F01, triplet.F12, triplet.F20, &H01, + &H12, &H20); + std::cout << "Error: " << e << std::endl; std::cout << H01 << std::endl << std::endl; std::cout << H12 << std::endl << std::endl; std::cout << H20 << std::endl; + // Create a TransferFactor + TransferFactor 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 Values values; - values.insert(0, triplet.F01); values.insert(1, triplet.F12); values.insert(2, triplet.F20); // EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-7);