diff --git a/gtsam/geometry/tests/testFundamentalMatrix.cpp b/gtsam/geometry/tests/testFundamentalMatrix.cpp index 80e6b1d3a..a8884e791 100644 --- a/gtsam/geometry/tests/testFundamentalMatrix.cpp +++ b/gtsam/geometry/tests/testFundamentalMatrix.cpp @@ -197,35 +197,6 @@ TripleF generateTripleF( return {F[0], F[1], F[2]}; // Return a TripleF instance } -//************************************************************************* - -struct TripletFactor { - using F = FundamentalMatrix; - using SF = SimpleFundamentalMatrix; - Point2 p0, p1, p2; - - /// vector of errors returns 6D vector - Vector evaluateError(const SF& F01, const SF& F12, const SF& F20, // - Matrix* H01, Matrix* H12, Matrix* H20) const { - Vector error(6); - std::function fn = [&](const SF& F01, const SF& F12, - const SF& F20) { - Vector6 error; - error << F::transfer(F01.matrix(), p1, F20.matrix().transpose(), p2) - p0, - F::transfer(F01.matrix().transpose(), p0, F12.matrix(), p2) - p1, - F::transfer(F20.matrix(), p0, F12.matrix().transpose(), p1) - p2; - return error; - }; - if (H01) - *H01 = numericalDerivative31(fn, F01, F12, F20); - if (H12) - *H12 = numericalDerivative32(fn, F01, F12, F20); - if (H20) - *H20 = numericalDerivative33(fn, F01, F12, F20); - return fn(F01, F12, F20); - } -}; - //************************************************************************* TEST(TripleF, Transfer) { // Generate cameras on a circle, as well as fundamental matrices diff --git a/gtsam/sfm/TransferFactor.h b/gtsam/sfm/TransferFactor.h new file mode 100644 index 000000000..43142e762 --- /dev/null +++ b/gtsam/sfm/TransferFactor.h @@ -0,0 +1,48 @@ +/* ---------------------------------------------------------------------------- + * GTSAM Copyright 2010-2024, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/* + * @file TransferFactor.h + * @brief TransferFactor class + * @author Frank Dellaert + * @date October 24, 2024 + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + +template +struct TransferFactor { + 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; + error << // + F::transfer(F01.matrix(), p1, F20.matrix().transpose(), p2) - p0, + F::transfer(F01.matrix().transpose(), p0, F12.matrix(), p2) - p1, + F::transfer(F20.matrix(), p0, F12.matrix().transpose(), p1) - p2; + return error; + }; + if (H01) *H01 = numericalDerivative31(fn, F01, F12, F20); + if (H12) *H12 = numericalDerivative32(fn, F01, F12, F20); + if (H20) *H20 = numericalDerivative33(fn, F01, F12, F20); + return fn(F01, F12, F20); + } +}; + +} // namespace gtsam diff --git a/gtsam/sfm/tests/testTransferFactor.cpp b/gtsam/sfm/tests/testTransferFactor.cpp new file mode 100644 index 000000000..3c3e89693 --- /dev/null +++ b/gtsam/sfm/tests/testTransferFactor.cpp @@ -0,0 +1,95 @@ +/* + * @file testTransferFactor.cpp + * @brief Test TransferFactor class + * @author Your Name + * @date October 23, 2024 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace gtsam; + +//************************************************************************* +/// Generate three cameras on a circle, looking in +std::array generateCameraPoses() { + std::array cameraPoses; + const double radius = 1.0; + for (int i = 0; i < 3; ++i) { + double angle = i * 2.0 * M_PI / 3.0; + double c = cos(angle), s = sin(angle); + Rot3 aRb({-s, c, 0}, {0, 0, -1}, {-c, -s, 0}); + cameraPoses[i] = {aRb, Point3(radius * c, radius * s, 0)}; + } + return cameraPoses; +} + +// Function to generate a TripleF from camera poses +TripleF generateTripleF( + const std::array& cameraPoses) { + std::array F; + for (size_t i = 0; i < 3; ++i) { + size_t j = (i + 1) % 3; + const Pose3 iPj = cameraPoses[i].between(cameraPoses[j]); + EssentialMatrix E(iPj.rotation(), Unit3(iPj.translation())); + F[i] = {E, 1000.0, 1000.0, Point2(640 / 2, 480 / 2), + Point2(640 / 2, 480 / 2)}; + } + return {F[0], F[1], F[2]}; // Return a TripleF instance +} + +double focalLength = 1000; +Point2 principalPoint(640 / 2, 480 / 2); + +// Test for TransferFactor +TEST(TransferFactor, Jacobians) { + // Generate cameras on a circle + std::array cameraPoses = generateCameraPoses(); + auto triplet = generateTripleF(cameraPoses); + + // Now project a point into the three cameras + const Point3 P(0.1, 0.2, 0.3); + const Cal3_S2 K(focalLength, focalLength, 0.0, // + principalPoint.x(), principalPoint.y()); + + std::array p; + for (size_t i = 0; i < 3; ++i) { + // Project the point into each camera + PinholeCameraCal3_S2 camera(cameraPoses[i], K); + p[i] = camera.project(P); + } + + // Create a TransferFactor + TransferFactor factor{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; + std::cout << H01 << std::endl << std::endl; + std::cout << H12 << std::endl << std::endl; + std::cout << H20 << 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); +} + +//************************************************************************* +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//*************************************************************************