From 3036ed80677c0114c757792cd0639c6cdde41d25 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 23 Oct 2024 21:07:22 -0700 Subject: [PATCH] Verify triplet epipolar transfer --- gtsam/geometry/FundamentalMatrix.h | 5 +- .../geometry/tests/testFundamentalMatrix.cpp | 79 +++++++++++++++++-- 2 files changed, 76 insertions(+), 8 deletions(-) diff --git a/gtsam/geometry/FundamentalMatrix.h b/gtsam/geometry/FundamentalMatrix.h index 1080cd87a8..acd7130206 100644 --- a/gtsam/geometry/FundamentalMatrix.h +++ b/gtsam/geometry/FundamentalMatrix.h @@ -213,9 +213,10 @@ class SimpleFundamentalMatrix : FundamentalMatrix { /// Print the SimpleFundamentalMatrix void print(const std::string& s = "") const { - std::cout << s << "E:\n" + std::cout << s << " E:\n" << E_.matrix() << "\nfa: " << fa_ << "\nfb: " << fb_ - << "\nca: " << ca_ << "\ncb: " << cb_ << std::endl; + << "\nca: " << ca_.transpose() << "\ncb: " << cb_.transpose() + << std::endl; } /// Check equality within a tolerance diff --git a/gtsam/geometry/tests/testFundamentalMatrix.cpp b/gtsam/geometry/tests/testFundamentalMatrix.cpp index bab57b1482..40a0b47677 100644 --- a/gtsam/geometry/tests/testFundamentalMatrix.cpp +++ b/gtsam/geometry/tests/testFundamentalMatrix.cpp @@ -10,6 +10,7 @@ #include #include #include +#include #include using namespace std::placeholders; @@ -95,9 +96,9 @@ TEST(SimpleStereo, EpipolarLine) { //************************************************************************* // Create a stereo pair, but in pixels not normalized coordinates. // We're still using zero principal points here. -double fa = 1000; -double fb = 1000; -SimpleFundamentalMatrix pixelStereo(defaultE, fa, fb, zero, zero); +double focalLength = 1000; +SimpleFundamentalMatrix pixelStereo(defaultE, focalLength, focalLength, zero, + zero); //************************************************************************* TEST(PixelStereo, Conversion) { @@ -125,7 +126,8 @@ TEST(PixelStereo, PointInBToHorizontalLineInA) { // Create a stereo pair with the right camera rotated 90 degrees Rot3 aRb = Rot3::Rz(M_PI_2); // Rotate 90 degrees around the Z-axis EssentialMatrix rotatedE(aRb, Unit3(1, 0, 0)); -SimpleFundamentalMatrix rotatedPixelStereo(rotatedE, fa, fb, zero, zero); +SimpleFundamentalMatrix rotatedPixelStereo(rotatedE, focalLength, focalLength, + zero, zero); //************************************************************************* TEST(RotatedPixelStereo, Conversion) { @@ -151,7 +153,10 @@ TEST(RotatedPixelStereo, PointInBToHorizontalLineInA) { //************************************************************************* // Now check that principal points also survive conversion -SimpleFundamentalMatrix stereoWithPrincipalPoints(rotatedE, fa, fb, zero, zero); +Point2 principalPoint(640 / 2, 480 / 2); +SimpleFundamentalMatrix stereoWithPrincipalPoints(rotatedE, focalLength, + focalLength, principalPoint, + principalPoint); //************************************************************************* TEST(stereoWithPrincipalPoints, Conversion) { @@ -165,9 +170,71 @@ TEST(stereoWithPrincipalPoints, Conversion) { EXPECT(assert_equal(expected, actual, 1e-4)); } +//************************************************************************* +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; +} + +std::tuple +generateFs(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, focalLength, focalLength, principalPoint, principalPoint}; + } + return {F[0], F[1], F[2]}; +} + +//************************************************************************* +TEST(Triplet, Transfer) { + // Generate cameras on a circle, as well as fundamental matrices + auto cameraPoses = generateCameraPoses(); + auto [F01, F12, F20] = generateFs(cameraPoses); + + // Check that they are all equal + EXPECT(F01.equals(F12, 1e-9)); + EXPECT(F12.equals(F20, 1e-9)); + EXPECT(F20.equals(F01, 1e-9)); + + // 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 lines in camera 0 from projections 1 and 2 + Vector3 line1 = F01.matrix() * Vector3(p[1].x(), p[1].y(), 1); + Vector3 line2 = F20.matrix().transpose() * Vector3(p[2].x(), p[2].y(), 1); + + // Cross the lines to find the intersection point + Vector3 intersectionPoint = line1.cross(line2); + + // Normalize the intersection point + intersectionPoint /= intersectionPoint(2); + + // Compare the intersection point with the original projection in camera 0 + EXPECT(assert_equal(p[0], intersectionPoint.head<2>(), 1e-9)); +} //************************************************************************* int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ +//*************************************************************************