From 56610ce5f7f8bd1987ab3cc2871547faf0c14e98 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 28 Oct 2024 09:32:04 -0700 Subject: [PATCH] Python unit tests --- gtsam/geometry/geometry.i | 6 +- python/gtsam/tests/test_FundamentalMatrix.py | 228 +++++++++++++++++++ 2 files changed, 233 insertions(+), 1 deletion(-) create mode 100644 python/gtsam/tests/test_FundamentalMatrix.py diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 671965064..3328a05bb 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -578,6 +578,8 @@ class Unit3 { // Standard Constructors Unit3(); Unit3(const gtsam::Point3& pose); + Unit3(double x, double y, double z); + Unit3(const gtsam::Point2& p, double f); // Testable void print(string s = "") const; @@ -953,6 +955,9 @@ class SimpleFundamentalMatrix { gtsam::SimpleFundamentalMatrix retract(const gtsam::Vector& delta) const; }; +gtsam::Point2 EpipolarTransfer(const gtsam::Matrix3& Fca, const gtsam::Point2& pa, + const gtsam::Matrix3& Fcb, const gtsam::Point2& pb); + #include class CalibratedCamera { // Standard Constructors and Named Constructors @@ -1066,7 +1071,6 @@ typedef gtsam::PinholeCamera PinholeCameraCal3_S2; typedef gtsam::PinholeCamera PinholeCameraCal3DS2; typedef gtsam::PinholeCamera PinholeCameraCal3Unified; typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; -typedef gtsam::PinholeCamera PinholeCameraCal3f; typedef gtsam::PinholeCamera PinholeCameraCal3Fisheye; #include diff --git a/python/gtsam/tests/test_FundamentalMatrix.py b/python/gtsam/tests/test_FundamentalMatrix.py new file mode 100644 index 000000000..4b30658b5 --- /dev/null +++ b/python/gtsam/tests/test_FundamentalMatrix.py @@ -0,0 +1,228 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +FundamentalMatrix unit tests. +Author: Frank Dellaert +""" + +# pylint: disable=no-name-in-module +import unittest + +import numpy as np +from gtsam.examples import SFMdata +from numpy.testing import assert_almost_equal + +import gtsam +from gtsam import (Cal3_S2, EssentialMatrix, FundamentalMatrix, + PinholeCameraCal3_S2, Point2, Point3, Pose3, Rot3, + SimpleFundamentalMatrix, Unit3) + + +class TestFundamentalMatrix(unittest.TestCase): + + def setUp(self): + # Create two rotations and corresponding fundamental matrix F + self.trueU = Rot3.Yaw(np.pi / 2) + self.trueV = Rot3.Yaw(np.pi / 4) + self.trueS = 0.5 + self.trueF = FundamentalMatrix(self.trueU, self.trueS, self.trueV) + + def test_localCoordinates(self): + expected = np.zeros(7) # Assuming 7 dimensions for U, V, and s + actual = self.trueF.localCoordinates(self.trueF) + assert_almost_equal(expected, actual, decimal=8) + + def test_retract(self): + actual = self.trueF.retract(np.zeros(7)) + self.assertTrue(self.trueF.equals(actual, 1e-9)) + + def test_RoundTrip(self): + d = np.array([0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7]) + hx = self.trueF.retract(d) + actual = self.trueF.localCoordinates(hx) + assert_almost_equal(d, actual, decimal=8) + + +class TestSimpleStereo(unittest.TestCase): + + def setUp(self): + # Create the simplest SimpleFundamentalMatrix, a stereo pair + self.defaultE = EssentialMatrix(Rot3(), Unit3(1, 0, 0)) + self.zero = Point2(0.0, 0.0) + self.stereoF = SimpleFundamentalMatrix(self.defaultE, 1.0, 1.0, self.zero, self.zero) + + def test_Conversion(self): + convertedF = FundamentalMatrix(self.stereoF.matrix()) + assert_almost_equal(self.stereoF.matrix(), convertedF.matrix(), decimal=8) + + def test_localCoordinates(self): + expected = np.zeros(7) + actual = self.stereoF.localCoordinates(self.stereoF) + assert_almost_equal(expected, actual, decimal=8) + + def test_retract(self): + actual = self.stereoF.retract(np.zeros(9)) + self.assertTrue(self.stereoF.equals(actual, 1e-9)) + + def test_RoundTrip(self): + d = np.array([0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7]) + hx = self.stereoF.retract(d) + actual = self.stereoF.localCoordinates(hx) + assert_almost_equal(d, actual, decimal=8) + + def test_EpipolarLine(self): + # Create a point in b + p_b = np.array([0, 2, 1]) + # Convert the point to a horizontal line in a + l_a = self.stereoF.matrix() @ p_b + # Check if the line is horizontal at height 2 + expected = np.array([0, -1, 2]) + assert_almost_equal(l_a, expected, decimal=8) + + +class TestPixelStereo(unittest.TestCase): + + def setUp(self): + # Create a stereo pair in pixels, zero principal points + self.focalLength = 1000.0 + self.defaultE = EssentialMatrix(Rot3(), Unit3(1, 0, 0)) + self.zero = Point2(0.0, 0.0) + self.pixelStereo = SimpleFundamentalMatrix( + self.defaultE, self.focalLength, self.focalLength, self.zero, self.zero + ) + + def test_Conversion(self): + expected = self.pixelStereo.matrix() + convertedF = FundamentalMatrix(self.pixelStereo.matrix()) + # Check equality of F-matrices up to a scale + actual = convertedF.matrix() + scale = expected[1, 2] / actual[1, 2] + actual *= scale + assert_almost_equal(expected, actual, decimal=5) + + def test_PointInBToHorizontalLineInA(self): + # Create a point in b + p_b = np.array([0, 300, 1]) + # Convert the point to a horizontal line in a + l_a = self.pixelStereo.matrix() @ p_b + # Check if the line is horizontal at height 0.3 + expected = np.array([0, -0.001, 0.3]) + assert_almost_equal(l_a, expected, decimal=8) + + +class TestRotatedPixelStereo(unittest.TestCase): + + def setUp(self): + # Create a stereo pair with the right camera rotated 90 degrees + self.focalLength = 1000.0 + self.zero = Point2(0.0, 0.0) + self.aRb = Rot3.Rz(np.pi / 2) # Rotate 90 degrees around the Z-axis + self.rotatedE = EssentialMatrix(self.aRb, Unit3(1, 0, 0)) + self.rotatedPixelStereo = SimpleFundamentalMatrix( + self.rotatedE, self.focalLength, self.focalLength, self.zero, self.zero + ) + + def test_Conversion(self): + expected = self.rotatedPixelStereo.matrix() + convertedF = FundamentalMatrix(self.rotatedPixelStereo.matrix()) + # Check equality of F-matrices up to a scale + actual = convertedF.matrix() + scale = expected[1, 2] / actual[1, 2] + actual *= scale + assert_almost_equal(expected, actual, decimal=4) + + def test_PointInBToHorizontalLineInA(self): + # Create a point in b + p_b = np.array([300, 0, 1]) + # Convert the point to a horizontal line in a + l_a = self.rotatedPixelStereo.matrix() @ p_b + # Check if the line is horizontal at height 0.3 + expected = np.array([0, -0.001, 0.3]) + assert_almost_equal(l_a, expected, decimal=8) + + +class TestStereoWithPrincipalPoints(unittest.TestCase): + + def setUp(self): + # Now check that principal points also survive conversion + self.focalLength = 1000.0 + self.principalPoint = Point2(640 / 2, 480 / 2) + self.aRb = Rot3.Rz(np.pi / 2) + self.rotatedE = EssentialMatrix(self.aRb, Unit3(1, 0, 0)) + self.stereoWithPrincipalPoints = SimpleFundamentalMatrix( + self.rotatedE, self.focalLength, self.focalLength, self.principalPoint, self.principalPoint + ) + + def test_Conversion(self): + expected = self.stereoWithPrincipalPoints.matrix() + convertedF = FundamentalMatrix(self.stereoWithPrincipalPoints.matrix()) + # Check equality of F-matrices up to a scale + actual = convertedF.matrix() + scale = expected[1, 2] / actual[1, 2] + actual *= scale + assert_almost_equal(expected, actual, decimal=4) + + +class TestTripleF(unittest.TestCase): + + def setUp(self): + # Generate three cameras on a circle, looking in + self.cameraPoses = SFMdata.posesOnCircle(3, 1.0) + self.focalLength = 1000.0 + self.principalPoint = Point2(640 / 2, 480 / 2) + self.triplet = self.generateTripleF(self.cameraPoses) + + def generateTripleF(self, cameraPoses): + F = [] + for i in range(3): + j = (i + 1) % 3 + iPj = cameraPoses[i].between(cameraPoses[j]) + E = EssentialMatrix(iPj.rotation(), Unit3(iPj.translation())) + F_ij = SimpleFundamentalMatrix( + E, self.focalLength, self.focalLength, self.principalPoint, self.principalPoint + ) + F.append(F_ij) + return {"Fab": F[0], "Fbc": F[1], "Fca": F[2]} + + def transferToA(self, pb, pc): + return gtsam.EpipolarTransfer(self.triplet["Fab"].matrix(), pb, self.triplet["Fca"].matrix().transpose(), pc) + + def transferToB(self, pa, pc): + return gtsam.EpipolarTransfer(self.triplet["Fab"].matrix().transpose(), pa, self.triplet["Fbc"].matrix(), pc) + + def transferToC(self, pa, pb): + return gtsam.EpipolarTransfer(self.triplet["Fca"].matrix(), pa, self.triplet["Fbc"].matrix().transpose(), pb) + + def test_Transfer(self): + triplet = self.triplet + # Check that they are all equal + self.assertTrue(triplet["Fab"].equals(triplet["Fbc"], 1e-9)) + self.assertTrue(triplet["Fbc"].equals(triplet["Fca"], 1e-9)) + self.assertTrue(triplet["Fca"].equals(triplet["Fab"], 1e-9)) + + # Now project a point into the three cameras + P = Point3(0.1, 0.2, 0.3) + K = Cal3_S2(self.focalLength, self.focalLength, 0.0, self.principalPoint[0], self.principalPoint[1]) + + p = [] + for i in range(3): + # Project the point into each camera + camera = PinholeCameraCal3_S2(self.cameraPoses[i], K) + p_i = camera.project(P) + p.append(p_i) + + # Check that transfer works + transferredA = self.transferToA(p[1], p[2]) + transferredB = self.transferToB(p[0], p[2]) + transferredC = self.transferToC(p[0], p[1]) + assert_almost_equal([p[0][0], p[0][1]], [transferredA[0], transferredA[1]], decimal=9) + assert_almost_equal([p[1][0], p[1][1]], [transferredB[0], transferredB[1]], decimal=9) + assert_almost_equal([p[2][0], p[2][1]], [transferredC[0], transferredC[1]], decimal=9) + + +if __name__ == "__main__": + unittest.main()