From 26a3728d8068e50049a4d70cf5d3cc243d57c8f9 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 28 Oct 2024 08:55:49 -0700 Subject: [PATCH 01/14] Fix createPoses --- examples/SFMdata.h | 2 +- python/gtsam/examples/SFMExample.py | 2 +- python/gtsam/examples/SFMdata.py | 61 ++++++++++++++----- .../examples/TranslationAveragingExample.py | 3 +- python/gtsam/examples/VisualISAM2Example.py | 2 +- python/gtsam/examples/VisualISAMExample.py | 2 +- 6 files changed, 52 insertions(+), 20 deletions(-) diff --git a/examples/SFMdata.h b/examples/SFMdata.h index f2561b4900..2a86aa5932 100644 --- a/examples/SFMdata.h +++ b/examples/SFMdata.h @@ -56,7 +56,7 @@ std::vector createPoints() { /** * Create a set of ground-truth poses - * Default values give a circular trajectory, radius 30 at pi/4 intervals, + * Default values give a circular trajectory, radius 30 at pi/4 intervals, * always facing the circle center */ std::vector createPoses( diff --git a/python/gtsam/examples/SFMExample.py b/python/gtsam/examples/SFMExample.py index 87bb3cb871..c8d1f1271c 100644 --- a/python/gtsam/examples/SFMExample.py +++ b/python/gtsam/examples/SFMExample.py @@ -62,7 +62,7 @@ def main(): points = SFMdata.createPoints() # Create the set of ground-truth poses - poses = SFMdata.createPoses(K) + poses = SFMdata.createPoses() # Create a factor graph graph = NonlinearFactorGraph() diff --git a/python/gtsam/examples/SFMdata.py b/python/gtsam/examples/SFMdata.py index ad414a256d..1ce7430f60 100644 --- a/python/gtsam/examples/SFMdata.py +++ b/python/gtsam/examples/SFMdata.py @@ -3,14 +3,14 @@ - The landmarks form a 10 meter cube - The robot rotates around the landmarks, always facing towards the cube """ + # pylint: disable=invalid-name, E1101 from typing import List import numpy as np -import gtsam -from gtsam import Cal3_S2, Point3, Pose3 +from gtsam import Point3, Pose3, Rot3 def createPoints() -> List[Point3]: @@ -28,16 +28,49 @@ def createPoints() -> List[Point3]: return points -def createPoses(K: Cal3_S2) -> List[Pose3]: - """Generate a set of ground-truth camera poses arranged in a circle about the origin.""" - radius = 40.0 - height = 10.0 - angles = np.linspace(0, 2 * np.pi, 8, endpoint=False) - up = gtsam.Point3(0, 0, 1) - target = gtsam.Point3(0, 0, 0) - poses = [] - for theta in angles: - position = gtsam.Point3(radius * np.cos(theta), radius * np.sin(theta), height) - camera = gtsam.PinholeCameraCal3_S2.Lookat(position, target, up, K) - poses.append(camera.pose()) +_M_PI_2 = np.pi / 2 +_M_PI_4 = np.pi / 4 + + +def createPoses( + init: Pose3 = Pose3(Rot3.Ypr(_M_PI_2, 0, -_M_PI_2), Point3(30, 0, 0)), + delta: Pose3 = Pose3( + Rot3.Ypr(0, -_M_PI_4, 0), + Point3(np.sin(_M_PI_4) * 30, 0, 30 * (1 - np.sin(_M_PI_4))), + ), + steps: int = 8, +) -> List[Pose3]: + """ + Create a set of ground-truth poses + Default values give a circular trajectory, radius 30 at pi/4 intervals, + always facing the circle center + """ + poses = [init] + for _ in range(1, steps): + poses.append(poses[-1].compose(delta)) return poses + + +def posesOnCircle(num_cameras=8, R=30): + """Create regularly spaced poses with specified radius and number of cameras.""" + theta = 2 * np.pi / num_cameras + + # Initial pose at angle 0, position (R, 0, 0), facing the center with Y-axis pointing down + init_rotation = Rot3.Ypr(_M_PI_2, 0, -_M_PI_2) + init_position = np.array([R, 0, 0]) + init = Pose3(init_rotation, init_position) + + # Delta rotation: rotate by -theta around Z-axis (counterclockwise movement) + delta_rotation = Rot3.Ypr(0, -theta, 0) + + # Delta translation in world frame + delta_translation_world = np.array([R * (np.cos(theta) - 1), R * np.sin(theta), 0]) + + # Transform delta translation to local frame of the camera + delta_translation_local = init.rotation().unrotate(delta_translation_world) + + # Define delta pose + delta = Pose3(delta_rotation, delta_translation_local) + + # Generate poses + return createPoses(init, delta, num_cameras) diff --git a/python/gtsam/examples/TranslationAveragingExample.py b/python/gtsam/examples/TranslationAveragingExample.py index ea1cab88d2..e3380ce942 100644 --- a/python/gtsam/examples/TranslationAveragingExample.py +++ b/python/gtsam/examples/TranslationAveragingExample.py @@ -31,8 +31,7 @@ def get_data() -> Tuple[gtsam.Values, List[gtsam.BinaryMeasurementUnit3]]: that lie on a circle and face the center. The poses of 8 cameras are obtained from SFMdata and the unit translations directions between some camera pairs are computed from their global translations. """ - fx, fy, s, u0, v0 = 50.0, 50.0, 0.0, 50.0, 50.0 - wTc_list = SFMdata.createPoses(gtsam.Cal3_S2(fx, fy, s, u0, v0)) + wTc_list = SFMdata.createPoses() # Rotations of the cameras in the world frame. wRc_values = gtsam.Values() # Normalized translation directions from camera i to camera j diff --git a/python/gtsam/examples/VisualISAM2Example.py b/python/gtsam/examples/VisualISAM2Example.py index 60d4fb376b..03dd079f0a 100644 --- a/python/gtsam/examples/VisualISAM2Example.py +++ b/python/gtsam/examples/VisualISAM2Example.py @@ -73,7 +73,7 @@ def visual_ISAM2_example(): points = SFMdata.createPoints() # Create the set of ground-truth poses - poses = SFMdata.createPoses(K) + poses = SFMdata.createPoses() # Create an iSAM2 object. Unlike iSAM1, which performs periodic batch steps # to maintain proper linearization and efficient variable ordering, iSAM2 diff --git a/python/gtsam/examples/VisualISAMExample.py b/python/gtsam/examples/VisualISAMExample.py index 9691b3c46d..48e803919a 100644 --- a/python/gtsam/examples/VisualISAMExample.py +++ b/python/gtsam/examples/VisualISAMExample.py @@ -36,7 +36,7 @@ def main(): # Create the set of ground-truth landmarks points = SFMdata.createPoints() # Create the set of ground-truth poses - poses = SFMdata.createPoses(K) + poses = SFMdata.createPoses() # Create a NonlinearISAM object which will relinearize and reorder the variables # every "reorderInterval" updates From f206c2f1e5b6fe0041e15b939351a8f9be201d4d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 28 Oct 2024 08:56:21 -0700 Subject: [PATCH 02/14] Fix FromPose3 --- gtsam/geometry/geometry.i | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 095a350dd9..cf86723ae2 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -620,10 +620,10 @@ class EssentialMatrix { EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Unit3& aTb); // Constructors from Pose3 - gtsam::EssentialMatrix FromPose3(const gtsam::Pose3& _1P2_); + static gtsam::EssentialMatrix FromPose3(const gtsam::Pose3& _1P2_); - gtsam::EssentialMatrix FromPose3(const gtsam::Pose3& _1P2_, - Eigen::Ref H); + static gtsam::EssentialMatrix FromPose3(const gtsam::Pose3& _1P2_, + Eigen::Ref H); // Testable void print(string s = "") const; From e98acff79ac5825d6fae35b2f2209a4e52ecd64a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Oct 2024 15:57:16 -0700 Subject: [PATCH 03/14] Wrap FundamentalMatrix classes --- gtsam/geometry/geometry.i | 48 +++++++++++++++++++++++++++++++++++++ gtsam/nonlinear/nonlinear.i | 3 +++ 2 files changed, 51 insertions(+) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index cf86723ae2..8fd1e46b29 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -903,6 +903,54 @@ class Cal3Bundler { void serialize() const; }; +#include + +// FundamentalMatrix class +class FundamentalMatrix { + // Constructors + FundamentalMatrix(); + FundamentalMatrix(const gtsam::Rot3& U, double s, const gtsam::Rot3& V); + FundamentalMatrix(const gtsam::Matrix3& F); + + // Overloaded constructors for specific calibration types + FundamentalMatrix(const gtsam::Cal3_S2& Ka, const gtsam::Pose3& aPb, + const gtsam::Cal3_S2& Kb); + + // Methods + gtsam::Matrix3 matrix() const; + + // Testable + void print(const std::string& s = "") const; + bool equals(const gtsam::FundamentalMatrix& other, double tol = 1e-9) const; + + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::Vector localCoordinates(const gtsam::FundamentalMatrix& F) const; + gtsam::FundamentalMatrix retract(const gtsam::Vector& delta) const; +}; + +// SimpleFundamentalMatrix class +class SimpleFundamentalMatrix { + // Constructors + SimpleFundamentalMatrix(); + SimpleFundamentalMatrix(const gtsam::EssentialMatrix& E, double fa, double fb, + const gtsam::Point2& ca, const gtsam::Point2& cb); + + // Methods + gtsam::Matrix3 matrix() const; + + // Testable + void print(const std::string& s = "") const; + bool equals(const gtsam::SimpleFundamentalMatrix& other, double tol = 1e-9) const; + + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::Vector localCoordinates(const gtsam::SimpleFundamentalMatrix& F) const; + gtsam::SimpleFundamentalMatrix retract(const gtsam::Vector& delta) const; +}; + #include class CalibratedCamera { // Standard Constructors and Named Constructors diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index f40deab5aa..f493fe8f6e 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -11,6 +11,7 @@ namespace gtsam { #include #include #include +#include #include #include #include @@ -537,6 +538,8 @@ class ISAM2 { template , gtsam::PinholeCamera, gtsam::PinholeCamera, From 9cfca976bab21cde5f9369947eec3d2f4a104e76 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Oct 2024 16:55:23 -0700 Subject: [PATCH 04/14] New constructor --- gtsam/geometry/FundamentalMatrix.h | 20 +++++++++++++++++--- gtsam/geometry/geometry.i | 2 ++ 2 files changed, 19 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/FundamentalMatrix.h b/gtsam/geometry/FundamentalMatrix.h index 6f04f45e8e..770cd711de 100644 --- a/gtsam/geometry/FundamentalMatrix.h +++ b/gtsam/geometry/FundamentalMatrix.h @@ -54,6 +54,22 @@ class GTSAM_EXPORT FundamentalMatrix { */ FundamentalMatrix(const Matrix3& F); + /** + * @brief Construct from essential matrix and calibration matrices + * + * Initializes the FundamentalMatrix from the given essential matrix E + * and calibration matrices Ka and Kb. + * + * @tparam CAL Calibration type, expected to have a matrix() method + * @param E Essential matrix + * @param Ka Calibration matrix for the left camera + * @param Kb Calibration matrix for the right camera + */ + template + FundamentalMatrix(const CAL& Ka, const EssentialMatrix& E, const CAL& Kb) + : FundamentalMatrix(Ka.K().transpose().inverse() * E.matrix() * + Kb.K().inverse()) {} + /** * @brief Construct from calibration matrices Ka, Kb, and pose aPb * @@ -67,9 +83,7 @@ class GTSAM_EXPORT FundamentalMatrix { */ template FundamentalMatrix(const CAL& Ka, const Pose3& aPb, const CAL& Kb) - : FundamentalMatrix(Ka.K().transpose().inverse() * - EssentialMatrix::FromPose3(aPb).matrix() * - Kb.K().inverse()) {} + : FundamentalMatrix(Ka, EssentialMatrix::FromPose3(aPb), Kb) {} /// Return the fundamental matrix representation Matrix3 matrix() const; diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 8fd1e46b29..13cdcf70d3 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -913,6 +913,8 @@ class FundamentalMatrix { FundamentalMatrix(const gtsam::Matrix3& F); // Overloaded constructors for specific calibration types + FundamentalMatrix(const gtsam::Cal3_S2& Ka, const gtsam::EssentialMatrix& E, + const gtsam::Cal3_S2& Kb); FundamentalMatrix(const gtsam::Cal3_S2& Ka, const gtsam::Pose3& aPb, const gtsam::Cal3_S2& Kb); From 36539f250aee3ba4dcfc2dc5d15d4267ca10960a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Oct 2024 23:52:42 -0700 Subject: [PATCH 05/14] Small API change --- gtsam/geometry/FundamentalMatrix.h | 13 +++++-------- gtsam/geometry/geometry.i | 9 +++++---- 2 files changed, 10 insertions(+), 12 deletions(-) diff --git a/gtsam/geometry/FundamentalMatrix.h b/gtsam/geometry/FundamentalMatrix.h index 770cd711de..12cc20b3eb 100644 --- a/gtsam/geometry/FundamentalMatrix.h +++ b/gtsam/geometry/FundamentalMatrix.h @@ -60,15 +60,14 @@ class GTSAM_EXPORT FundamentalMatrix { * Initializes the FundamentalMatrix from the given essential matrix E * and calibration matrices Ka and Kb. * - * @tparam CAL Calibration type, expected to have a matrix() method * @param E Essential matrix * @param Ka Calibration matrix for the left camera * @param Kb Calibration matrix for the right camera */ - template - FundamentalMatrix(const CAL& Ka, const EssentialMatrix& E, const CAL& Kb) - : FundamentalMatrix(Ka.K().transpose().inverse() * E.matrix() * - Kb.K().inverse()) {} + FundamentalMatrix(const Matrix3& Ka, const EssentialMatrix& E, + const Matrix3& Kb) + : FundamentalMatrix(Ka.transpose().inverse() * E.matrix() * + Kb.inverse()) {} /** * @brief Construct from calibration matrices Ka, Kb, and pose aPb @@ -76,13 +75,11 @@ class GTSAM_EXPORT FundamentalMatrix { * Initializes the FundamentalMatrix from the given calibration * matrices Ka and Kb, and the pose aPb. * - * @tparam CAL Calibration type, expected to have a matrix() method * @param Ka Calibration matrix for the left camera * @param aPb Pose from the left to the right camera * @param Kb Calibration matrix for the right camera */ - template - FundamentalMatrix(const CAL& Ka, const Pose3& aPb, const CAL& Kb) + FundamentalMatrix(const Matrix3& Ka, const Pose3& aPb, const Matrix3& Kb) : FundamentalMatrix(Ka, EssentialMatrix::FromPose3(aPb), Kb) {} /// Return the fundamental matrix representation diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 13cdcf70d3..671965064f 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -913,10 +913,10 @@ class FundamentalMatrix { FundamentalMatrix(const gtsam::Matrix3& F); // Overloaded constructors for specific calibration types - FundamentalMatrix(const gtsam::Cal3_S2& Ka, const gtsam::EssentialMatrix& E, - const gtsam::Cal3_S2& Kb); - FundamentalMatrix(const gtsam::Cal3_S2& Ka, const gtsam::Pose3& aPb, - const gtsam::Cal3_S2& Kb); + FundamentalMatrix(const gtsam::Matrix3& Ka, const gtsam::EssentialMatrix& E, + const gtsam::Matrix3& Kb); + FundamentalMatrix(const gtsam::Matrix3& Ka, const gtsam::Pose3& aPb, + const gtsam::Matrix3& Kb); // Methods gtsam::Matrix3 matrix() const; @@ -1066,6 +1066,7 @@ 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 From 56610ce5f7f8bd1987ab3cc2871547faf0c14e98 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 28 Oct 2024 09:32:04 -0700 Subject: [PATCH 06/14] 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 671965064f..3328a05bbc 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 0000000000..4b30658b5e --- /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() From fba31d99f2392ba79828d63904c801b8de5cb1cd Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 28 Oct 2024 09:51:56 -0700 Subject: [PATCH 07/14] TestManyCamerasCircle --- python/gtsam/tests/test_FundamentalMatrix.py | 71 ++++++++++++++++++-- 1 file changed, 64 insertions(+), 7 deletions(-) diff --git a/python/gtsam/tests/test_FundamentalMatrix.py b/python/gtsam/tests/test_FundamentalMatrix.py index 4b30658b5e..4e6efc99d7 100644 --- a/python/gtsam/tests/test_FundamentalMatrix.py +++ b/python/gtsam/tests/test_FundamentalMatrix.py @@ -18,7 +18,7 @@ import gtsam from gtsam import (Cal3_S2, EssentialMatrix, FundamentalMatrix, - PinholeCameraCal3_S2, Point2, Point3, Pose3, Rot3, + PinholeCameraCal3_S2, Point2, Point3, Rot3, SimpleFundamentalMatrix, Unit3) @@ -216,12 +216,69 @@ def test_Transfer(self): 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) + assert_almost_equal(p[0], self.transferToA(p[1], p[2]), decimal=9) + assert_almost_equal(p[1], self.transferToB(p[0], p[2]), decimal=9) + assert_almost_equal(p[2], self.transferToC(p[0], p[1]), decimal=9) + + +class TestManyCamerasCircle(unittest.TestCase): + N = 6 + + def setUp(self): + # Generate six cameras on a circle, looking in + self.cameraPoses = SFMdata.posesOnCircle(self.N, 1.0) + self.focalLength = 1000.0 + self.principalPoint = Point2(640 / 2, 480 / 2) + self.manyFs = self.generateManyFs(self.cameraPoses) + + def generateManyFs(self, cameraPoses): + F = [] + for i in range(self.N): + j = (i + 1) % self.N + 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 F + + def test_Conversion(self): + for i in range(self.N): + expected = self.manyFs[i].matrix() + convertedF = FundamentalMatrix(self.manyFs[i].matrix()) + # Check equality of F-matrices up to a scale + actual = convertedF.matrix() + scale = expected[1, 2] / actual[1, 2] + actual *= scale + # print(f"\n{np.round(expected, 3)}", f"\n{np.round(actual, 3)}") + assert_almost_equal(expected, actual, decimal=4) + + def test_Transfer(self): + # Now project a point into the six 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(self.N): + # 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 + for a in range(self.N): + b = (a + 1) % self.N + c = (a + 2) % self.N + # We transfer from a to b and from c to b, + # and check that the result lines up with the projected point in b. + transferred = gtsam.EpipolarTransfer( + self.manyFs[a].matrix().transpose(), # need to transpose for a->b + p[a], + self.manyFs[c].matrix(), + p[c], + ) + assert_almost_equal(p[b], transferred, decimal=9) if __name__ == "__main__": From ced6d5721d50dc1415e6c76c5d4d878445b22048 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 28 Oct 2024 11:59:29 -0700 Subject: [PATCH 08/14] Fix call w new API --- examples/ViewGraphExample.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/examples/ViewGraphExample.cpp b/examples/ViewGraphExample.cpp index c23ac084c9..23393fa20c 100644 --- a/examples/ViewGraphExample.cpp +++ b/examples/ViewGraphExample.cpp @@ -38,7 +38,7 @@ using namespace gtsam; /* ************************************************************************* */ int main(int argc, char* argv[]) { // Define the camera calibration parameters - Cal3_S2 K(50.0, 50.0, 0.0, 50.0, 50.0); + Cal3_S2 cal(50.0, 50.0, 0.0, 50.0, 50.0); // Create the set of 8 ground-truth landmarks vector points = createPoints(); @@ -47,13 +47,13 @@ int main(int argc, char* argv[]) { vector poses = posesOnCircle(4, 30); // Calculate ground truth fundamental matrices, 1 and 2 poses apart - auto F1 = FundamentalMatrix(K, poses[0].between(poses[1]), K); - auto F2 = FundamentalMatrix(K, poses[0].between(poses[2]), K); + auto F1 = FundamentalMatrix(cal.K(), poses[0].between(poses[1]), cal.K()); + auto F2 = FundamentalMatrix(cal.K(), poses[0].between(poses[2]), cal.K()); // Simulate measurements from each camera pose std::array, 4> p; for (size_t i = 0; i < 4; ++i) { - PinholeCamera camera(poses[i], K); + PinholeCamera camera(poses[i], cal); for (size_t j = 0; j < 8; ++j) { p[i][j] = camera.project(points[j]); } From b45c55c9f4a98f801dfce0e6be04f5fc27ef7a4b Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 28 Oct 2024 12:00:54 -0700 Subject: [PATCH 09/14] Fix issues with SVD constructor --- gtsam/geometry/FundamentalMatrix.cpp | 57 +++++++++++++------ gtsam/geometry/FundamentalMatrix.h | 41 +++++++------ .../geometry/tests/testFundamentalMatrix.cpp | 37 ++++++++++-- 3 files changed, 96 insertions(+), 39 deletions(-) diff --git a/gtsam/geometry/FundamentalMatrix.cpp b/gtsam/geometry/FundamentalMatrix.cpp index d65053d196..5a50b4fd2d 100644 --- a/gtsam/geometry/FundamentalMatrix.cpp +++ b/gtsam/geometry/FundamentalMatrix.cpp @@ -26,6 +26,11 @@ Point2 EpipolarTransfer(const Matrix3& Fca, const Point2& pa, // } //************************************************************************* +FundamentalMatrix::FundamentalMatrix(const Matrix& U, double s, + const Matrix& V) { + initialize(U, s, V); +} + FundamentalMatrix::FundamentalMatrix(const Matrix3& F) { // Perform SVD Eigen::JacobiSVD svd(F, Eigen::ComputeFullU | Eigen::ComputeFullV); @@ -47,28 +52,44 @@ FundamentalMatrix::FundamentalMatrix(const Matrix3& F) { "The input matrix does not represent a valid fundamental matrix."); } - // Ensure the second singular value is recorded as s - s_ = singularValues(1); + initialize(U, singularValues(1), V); +} - // Check if U is a reflection - if (U.determinant() < 0) { - U = -U; - s_ = -s_; // Change sign of scalar if U is a reflection - } +void FundamentalMatrix::initialize(const Matrix3& U, double s, + const Matrix3& V) { + s_ = s; + sign_ = 1.0; - // Check if V is a reflection - if (V.determinant() < 0) { - V = -V; - s_ = -s_; // Change sign of scalar if U is a reflection + // Check if U is a reflection and its determinant is close to -1 or 1 + double detU = U.determinant(); + if (std::abs(std::abs(detU) - 1.0) > 1e-9) { + throw std::invalid_argument( + "Matrix U does not have a determinant close to -1 or 1."); + } + if (detU < 0) { + U_ = Rot3(-U); + sign_ = -sign_; // Flip sign if U is a reflection + } else { + U_ = Rot3(U); } - // Assign the rotations - U_ = Rot3(U); - V_ = Rot3(V); + // Check if V is a reflection and its determinant is close to -1 or 1 + double detV = V.determinant(); + if (std::abs(std::abs(detV) - 1.0) > 1e-9) { + throw std::invalid_argument( + "Matrix V does not have a determinant close to -1 or 1."); + } + if (detV < 0) { + V_ = Rot3(-V); + sign_ = -sign_; // Flip sign if V is a reflection + } else { + V_ = Rot3(V); + } } Matrix3 FundamentalMatrix::matrix() const { - return U_.matrix() * Vector3(1, s_, 0).asDiagonal() * V_.transpose().matrix(); + return sign_ * U_.matrix() * Vector3(1.0, s_, 0).asDiagonal() * + V_.transpose().matrix(); } void FundamentalMatrix::print(const std::string& s) const { @@ -77,8 +98,8 @@ void FundamentalMatrix::print(const std::string& s) const { bool FundamentalMatrix::equals(const FundamentalMatrix& other, double tol) const { - return U_.equals(other.U_, tol) && std::abs(s_ - other.s_) < tol && - V_.equals(other.V_, tol); + return U_.equals(other.U_, tol) && sign_ == other.sign_ && + std::abs(s_ - other.s_) < tol && V_.equals(other.V_, tol); } Vector FundamentalMatrix::localCoordinates(const FundamentalMatrix& F) const { @@ -93,7 +114,7 @@ FundamentalMatrix FundamentalMatrix::retract(const Vector& delta) const { Rot3 newU = U_.retract(delta.head<3>()); double newS = s_ + delta(3); // Update scalar Rot3 newV = V_.retract(delta.tail<3>()); - return FundamentalMatrix(newU, newS, newV); + return FundamentalMatrix(newU, sign_, newS, newV); } //************************************************************************* diff --git a/gtsam/geometry/FundamentalMatrix.h b/gtsam/geometry/FundamentalMatrix.h index 12cc20b3eb..7e87ba62a7 100644 --- a/gtsam/geometry/FundamentalMatrix.h +++ b/gtsam/geometry/FundamentalMatrix.h @@ -15,34 +15,36 @@ namespace gtsam { /** * @class FundamentalMatrix - * @brief Represents a general fundamental matrix. + * @brief Represents a fundamental matrix in computer vision, which encodes the + * epipolar geometry between two views. * - * This class represents a general fundamental matrix, which is a 3x3 matrix - * that describes the relationship between two images. It is parameterized by a - * left rotation U, a scalar s, and a right rotation V. + * The FundamentalMatrix class encapsulates the fundamental matrix, which + * relates corresponding points in stereo images. It is parameterized by two + * rotation matrices (U and V), a scalar parameter (s), and a sign. + * Using these values, the fundamental matrix is represented as + * + * F = sign * U * diag(1, s, 0) * V^T + * + * We need the `sign` because we use SO(3) for U and V, not O(3). */ class GTSAM_EXPORT FundamentalMatrix { private: - Rot3 U_; ///< Left rotation - double s_; ///< Scalar parameter for S - Rot3 V_; ///< Right rotation + Rot3 U_; ///< Left rotation + double sign_; ///< Whether to flip the sign or not + double s_; ///< Scalar parameter for S + Rot3 V_; ///< Right rotation public: /// Default constructor - FundamentalMatrix() : U_(Rot3()), s_(1.0), V_(Rot3()) {} + FundamentalMatrix() : U_(Rot3()), sign_(1.0), s_(1.0), V_(Rot3()) {} /** * @brief Construct from U, V, and scalar s * - * Initializes the FundamentalMatrix with the given left rotation U, - * scalar s, and right rotation V. - * - * @param U Left rotation matrix - * @param s Scalar parameter for the fundamental matrix - * @param V Right rotation matrix + * Initializes the FundamentalMatrix From the SVD representation + * U*diag(1,s,0)*V^T. It will internally convert to using SO(3). */ - FundamentalMatrix(const Rot3& U, double s, const Rot3& V) - : U_(U), s_(s), V_(V) {} + FundamentalMatrix(const Matrix& U, double s, const Matrix& V); /** * @brief Construct from a 3x3 matrix using SVD @@ -107,6 +109,13 @@ class GTSAM_EXPORT FundamentalMatrix { /// Retract the given vector to get a new FundamentalMatrix FundamentalMatrix retract(const Vector& delta) const; /// @} + private: + /// Private constructor for internal use + FundamentalMatrix(const Rot3& U, double sign, double s, const Rot3& V) + : U_(U), sign_(sign), s_(s), V_(V) {} + + /// Initialize SO(3) matrices from general O(3) matrices + void initialize(const Matrix3& U, double s, const Matrix3& V); }; /** diff --git a/gtsam/geometry/tests/testFundamentalMatrix.cpp b/gtsam/geometry/tests/testFundamentalMatrix.cpp index a8884e791e..3e136caa7c 100644 --- a/gtsam/geometry/tests/testFundamentalMatrix.cpp +++ b/gtsam/geometry/tests/testFundamentalMatrix.cpp @@ -24,21 +24,48 @@ GTSAM_CONCEPT_MANIFOLD_INST(FundamentalMatrix) Rot3 trueU = Rot3::Yaw(M_PI_2); Rot3 trueV = Rot3::Yaw(M_PI_4); double trueS = 0.5; -FundamentalMatrix trueF(trueU, trueS, trueV); +FundamentalMatrix trueF(trueU.matrix(), trueS, trueV.matrix()); //************************************************************************* -TEST(FundamentalMatrix, localCoordinates) { +TEST(FundamentalMatrix, LocalCoordinates) { Vector expected = Z_7x1; // Assuming 7 dimensions for U, V, and s Vector actual = trueF.localCoordinates(trueF); EXPECT(assert_equal(expected, actual, 1e-8)); } //************************************************************************* -TEST(FundamentalMatrix, retract) { +TEST(FundamentalMatrix, Retract) { FundamentalMatrix actual = trueF.retract(Z_7x1); EXPECT(assert_equal(trueF, actual)); } +//************************************************************************* +// Test conversion from an F-matrix +TEST(FundamentalMatrix, Conversion) { + const Matrix3 F = trueU.matrix() * Vector3(1, trueS, 0).asDiagonal() * + trueV.matrix().transpose(); + FundamentalMatrix actual(F); + EXPECT(assert_equal(trueF, actual)); +} + +//************************************************************************* +// Test conversion with a *non-rotation* U +TEST(FlippedFundamentalMatrix, Conversion1) { + FundamentalMatrix trueF(trueU.matrix(), trueS, -trueV.matrix()); + const Matrix3 F = trueF.matrix(); + FundamentalMatrix actual(F); + CHECK(assert_equal(F, actual.matrix())); +} + +//************************************************************************* +// Test conversion with a *non-rotation* U +TEST(FlippedFundamentalMatrix, Conversion2) { + FundamentalMatrix trueF(-trueU.matrix(), trueS, trueV.matrix()); + const Matrix3 F = trueF.matrix(); + FundamentalMatrix actual(F); + CHECK(assert_equal(F, actual.matrix())); +} + //************************************************************************* TEST(FundamentalMatrix, RoundTrip) { Vector7 d; @@ -61,14 +88,14 @@ TEST(SimpleStereo, Conversion) { } //************************************************************************* -TEST(SimpleStereo, localCoordinates) { +TEST(SimpleStereo, LocalCoordinates) { Vector expected = Z_7x1; Vector actual = stereoF.localCoordinates(stereoF); EXPECT(assert_equal(expected, actual, 1e-8)); } //************************************************************************* -TEST(SimpleStereo, retract) { +TEST(SimpleStereo, Retract) { SimpleFundamentalMatrix actual = stereoF.retract(Z_9x1); EXPECT(assert_equal(stereoF, actual)); } From 2d170e4cace4e008c9547c029488d9fb1f66be11 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 28 Oct 2024 12:15:50 -0700 Subject: [PATCH 10/14] Fix wrapper --- gtsam/geometry/geometry.i | 2 +- python/gtsam/tests/test_FundamentalMatrix.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 3328a05bbc..a90de48e55 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -911,7 +911,7 @@ class Cal3Bundler { class FundamentalMatrix { // Constructors FundamentalMatrix(); - FundamentalMatrix(const gtsam::Rot3& U, double s, const gtsam::Rot3& V); + FundamentalMatrix(const gtsam::Matrix3& U, double s, const gtsam::Matrix3& V); FundamentalMatrix(const gtsam::Matrix3& F); // Overloaded constructors for specific calibration types diff --git a/python/gtsam/tests/test_FundamentalMatrix.py b/python/gtsam/tests/test_FundamentalMatrix.py index 4e6efc99d7..9f486c2986 100644 --- a/python/gtsam/tests/test_FundamentalMatrix.py +++ b/python/gtsam/tests/test_FundamentalMatrix.py @@ -29,7 +29,7 @@ def setUp(self): 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) + self.trueF = FundamentalMatrix(self.trueU.matrix(), self.trueS, self.trueV.matrix()) def test_localCoordinates(self): expected = np.zeros(7) # Assuming 7 dimensions for U, V, and s From 0836852dcb68db8fbfb3c30cd5396647c1c790a5 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 28 Oct 2024 15:14:46 -0700 Subject: [PATCH 11/14] Fix test on windows --- .../geometry/tests/testFundamentalMatrix.cpp | 36 +++++++------------ 1 file changed, 13 insertions(+), 23 deletions(-) diff --git a/gtsam/geometry/tests/testFundamentalMatrix.cpp b/gtsam/geometry/tests/testFundamentalMatrix.cpp index 3e136caa7c..00876e415e 100644 --- a/gtsam/geometry/tests/testFundamentalMatrix.cpp +++ b/gtsam/geometry/tests/testFundamentalMatrix.cpp @@ -40,30 +40,20 @@ TEST(FundamentalMatrix, Retract) { } //************************************************************************* -// Test conversion from an F-matrix +// Test conversion from F matrices, including non-rotations TEST(FundamentalMatrix, Conversion) { - const Matrix3 F = trueU.matrix() * Vector3(1, trueS, 0).asDiagonal() * - trueV.matrix().transpose(); - FundamentalMatrix actual(F); - EXPECT(assert_equal(trueF, actual)); -} - -//************************************************************************* -// Test conversion with a *non-rotation* U -TEST(FlippedFundamentalMatrix, Conversion1) { - FundamentalMatrix trueF(trueU.matrix(), trueS, -trueV.matrix()); - const Matrix3 F = trueF.matrix(); - FundamentalMatrix actual(F); - CHECK(assert_equal(F, actual.matrix())); -} - -//************************************************************************* -// Test conversion with a *non-rotation* U -TEST(FlippedFundamentalMatrix, Conversion2) { - FundamentalMatrix trueF(-trueU.matrix(), trueS, trueV.matrix()); - const Matrix3 F = trueF.matrix(); - FundamentalMatrix actual(F); - CHECK(assert_equal(F, actual.matrix())); + Matrix3 U = trueU.matrix(); + Matrix3 V = trueV.matrix(); + std::vector Fs = { + FundamentalMatrix(U, trueS, V), FundamentalMatrix(U, trueS, -V), + FundamentalMatrix(-U, trueS, V), FundamentalMatrix(-U, trueS, -V)}; + + for (const auto& trueF : Fs) { + const Matrix3 F = trueF.matrix(); + FundamentalMatrix actual(F); + // We check the matrices as the underlying representation is not unique + CHECK(assert_equal(F, actual.matrix())); + } } //************************************************************************* From 64579373be8bd1d84536ccce93517315c9297b20 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 29 Oct 2024 10:58:08 -0700 Subject: [PATCH 12/14] Fix small issues and store scaled s_ --- gtsam/geometry/FundamentalMatrix.cpp | 30 ++++++++----------- gtsam/geometry/FundamentalMatrix.h | 16 ++++++---- .../geometry/tests/testFundamentalMatrix.cpp | 3 ++ 3 files changed, 25 insertions(+), 24 deletions(-) diff --git a/gtsam/geometry/FundamentalMatrix.cpp b/gtsam/geometry/FundamentalMatrix.cpp index 5a50b4fd2d..94c72673d3 100644 --- a/gtsam/geometry/FundamentalMatrix.cpp +++ b/gtsam/geometry/FundamentalMatrix.cpp @@ -2,10 +2,12 @@ * @file FundamentalMatrix.cpp * @brief FundamentalMatrix classes * @author Frank Dellaert - * @date Oct 23, 2024 + * @date October 2024 */ +#include #include +#include namespace gtsam { @@ -26,8 +28,8 @@ Point2 EpipolarTransfer(const Matrix3& Fca, const Point2& pa, // } //************************************************************************* -FundamentalMatrix::FundamentalMatrix(const Matrix& U, double s, - const Matrix& V) { +FundamentalMatrix::FundamentalMatrix(const Matrix3& U, double s, + const Matrix3& V) { initialize(U, s, V); } @@ -57,38 +59,30 @@ FundamentalMatrix::FundamentalMatrix(const Matrix3& F) { void FundamentalMatrix::initialize(const Matrix3& U, double s, const Matrix3& V) { - s_ = s; + s_ = s / kScale; sign_ = 1.0; - // Check if U is a reflection and its determinant is close to -1 or 1 - double detU = U.determinant(); - if (std::abs(std::abs(detU) - 1.0) > 1e-9) { - throw std::invalid_argument( - "Matrix U does not have a determinant close to -1 or 1."); - } + // Check if U is a reflection and flip U and sign_ if so + double detU = U.determinant(); // detU will be -1 or 1 if (detU < 0) { U_ = Rot3(-U); - sign_ = -sign_; // Flip sign if U is a reflection + sign_ = -sign_; } else { U_ = Rot3(U); } - // Check if V is a reflection and its determinant is close to -1 or 1 + // Same check for V double detV = V.determinant(); - if (std::abs(std::abs(detV) - 1.0) > 1e-9) { - throw std::invalid_argument( - "Matrix V does not have a determinant close to -1 or 1."); - } if (detV < 0) { V_ = Rot3(-V); - sign_ = -sign_; // Flip sign if V is a reflection + sign_ = -sign_; } else { V_ = Rot3(V); } } Matrix3 FundamentalMatrix::matrix() const { - return sign_ * U_.matrix() * Vector3(1.0, s_, 0).asDiagonal() * + return sign_ * U_.matrix() * Vector3(1.0, s_ * kScale, 0).asDiagonal() * V_.transpose().matrix(); } diff --git a/gtsam/geometry/FundamentalMatrix.h b/gtsam/geometry/FundamentalMatrix.h index 7e87ba62a7..a36c815da1 100644 --- a/gtsam/geometry/FundamentalMatrix.h +++ b/gtsam/geometry/FundamentalMatrix.h @@ -2,7 +2,7 @@ * @file FundamentalMatrix.h * @brief FundamentalMatrix classes * @author Frank Dellaert - * @date Oct 23, 2024 + * @date October 2024 */ #pragma once @@ -34,9 +34,11 @@ class GTSAM_EXPORT FundamentalMatrix { double s_; ///< Scalar parameter for S Rot3 V_; ///< Right rotation + static constexpr double kScale = 1000; // s is stored in s_ as s/kScale + public: /// Default constructor - FundamentalMatrix() : U_(Rot3()), sign_(1.0), s_(1.0), V_(Rot3()) {} + FundamentalMatrix() : U_(Rot3()), sign_(1.0), s_(1.0 / kScale), V_(Rot3()) {} /** * @brief Construct from U, V, and scalar s @@ -44,7 +46,7 @@ class GTSAM_EXPORT FundamentalMatrix { * Initializes the FundamentalMatrix From the SVD representation * U*diag(1,s,0)*V^T. It will internally convert to using SO(3). */ - FundamentalMatrix(const Matrix& U, double s, const Matrix& V); + FundamentalMatrix(const Matrix3& U, double s, const Matrix3& V); /** * @brief Construct from a 3x3 matrix using SVD @@ -60,7 +62,9 @@ class GTSAM_EXPORT FundamentalMatrix { * @brief Construct from essential matrix and calibration matrices * * Initializes the FundamentalMatrix from the given essential matrix E - * and calibration matrices Ka and Kb. + * and calibration matrices Ka and Kb, using + * F = Ka^(-T) * E * Kb^(-1) + * and then calls constructor that decomposes F via SVD. * * @param E Essential matrix * @param Ka Calibration matrix for the left camera @@ -111,8 +115,8 @@ class GTSAM_EXPORT FundamentalMatrix { /// @} private: /// Private constructor for internal use - FundamentalMatrix(const Rot3& U, double sign, double s, const Rot3& V) - : U_(U), sign_(sign), s_(s), V_(V) {} + FundamentalMatrix(const Rot3& U, double sign, double scaled_s, const Rot3& V) + : U_(U), sign_(sign), s_(scaled_s), V_(V) {} /// Initialize SO(3) matrices from general O(3) matrices void initialize(const Matrix3& U, double s, const Matrix3& V); diff --git a/gtsam/geometry/tests/testFundamentalMatrix.cpp b/gtsam/geometry/tests/testFundamentalMatrix.cpp index 00876e415e..eed5da734c 100644 --- a/gtsam/geometry/tests/testFundamentalMatrix.cpp +++ b/gtsam/geometry/tests/testFundamentalMatrix.cpp @@ -6,12 +6,15 @@ */ #include +#include #include #include #include #include #include +#include + using namespace std::placeholders; using namespace std; using namespace gtsam; From 5a2f1f889331f004a621610a3583646221732f4a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 29 Oct 2024 11:55:55 -0700 Subject: [PATCH 13/14] Get rid of scale --- gtsam/geometry/FundamentalMatrix.cpp | 4 ++-- gtsam/geometry/FundamentalMatrix.h | 4 +--- 2 files changed, 3 insertions(+), 5 deletions(-) diff --git a/gtsam/geometry/FundamentalMatrix.cpp b/gtsam/geometry/FundamentalMatrix.cpp index 94c72673d3..406269ff57 100644 --- a/gtsam/geometry/FundamentalMatrix.cpp +++ b/gtsam/geometry/FundamentalMatrix.cpp @@ -59,7 +59,7 @@ FundamentalMatrix::FundamentalMatrix(const Matrix3& F) { void FundamentalMatrix::initialize(const Matrix3& U, double s, const Matrix3& V) { - s_ = s / kScale; + s_ = s; sign_ = 1.0; // Check if U is a reflection and flip U and sign_ if so @@ -82,7 +82,7 @@ void FundamentalMatrix::initialize(const Matrix3& U, double s, } Matrix3 FundamentalMatrix::matrix() const { - return sign_ * U_.matrix() * Vector3(1.0, s_ * kScale, 0).asDiagonal() * + return sign_ * U_.matrix() * Vector3(1.0, s_, 0).asDiagonal() * V_.transpose().matrix(); } diff --git a/gtsam/geometry/FundamentalMatrix.h b/gtsam/geometry/FundamentalMatrix.h index a36c815da1..beb2947e2c 100644 --- a/gtsam/geometry/FundamentalMatrix.h +++ b/gtsam/geometry/FundamentalMatrix.h @@ -34,11 +34,9 @@ class GTSAM_EXPORT FundamentalMatrix { double s_; ///< Scalar parameter for S Rot3 V_; ///< Right rotation - static constexpr double kScale = 1000; // s is stored in s_ as s/kScale - public: /// Default constructor - FundamentalMatrix() : U_(Rot3()), sign_(1.0), s_(1.0 / kScale), V_(Rot3()) {} + FundamentalMatrix() : U_(Rot3()), sign_(1.0), s_(1.0), V_(Rot3()) {} /** * @brief Construct from U, V, and scalar s From 56943f7be8cc22c5a44d5dd0ecc0183428068730 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 31 Oct 2024 10:24:37 -0700 Subject: [PATCH 14/14] Better SVD handling --- gtsam/geometry/FundamentalMatrix.cpp | 42 ++++++++++------------------ gtsam/geometry/FundamentalMatrix.h | 21 ++++++-------- 2 files changed, 24 insertions(+), 39 deletions(-) diff --git a/gtsam/geometry/FundamentalMatrix.cpp b/gtsam/geometry/FundamentalMatrix.cpp index 406269ff57..5e8c26e627 100644 --- a/gtsam/geometry/FundamentalMatrix.cpp +++ b/gtsam/geometry/FundamentalMatrix.cpp @@ -57,32 +57,20 @@ FundamentalMatrix::FundamentalMatrix(const Matrix3& F) { initialize(U, singularValues(1), V); } -void FundamentalMatrix::initialize(const Matrix3& U, double s, - const Matrix3& V) { - s_ = s; - sign_ = 1.0; - - // Check if U is a reflection and flip U and sign_ if so - double detU = U.determinant(); // detU will be -1 or 1 - if (detU < 0) { - U_ = Rot3(-U); - sign_ = -sign_; - } else { - U_ = Rot3(U); - } +void FundamentalMatrix::initialize(Matrix3 U, double s, Matrix3 V) { + // Check if U is a reflection and flip third column if so + if (U.determinant() < 0) U.col(2) *= -1; // Same check for V - double detV = V.determinant(); - if (detV < 0) { - V_ = Rot3(-V); - sign_ = -sign_; - } else { - V_ = Rot3(V); - } + if (V.determinant() < 0) V.col(2) *= -1; + + U_ = Rot3(U); + s_ = s; + V_ = Rot3(V); } Matrix3 FundamentalMatrix::matrix() const { - return sign_ * U_.matrix() * Vector3(1.0, s_, 0).asDiagonal() * + return U_.matrix() * Vector3(1.0, s_, 0).asDiagonal() * V_.transpose().matrix(); } @@ -92,8 +80,8 @@ void FundamentalMatrix::print(const std::string& s) const { bool FundamentalMatrix::equals(const FundamentalMatrix& other, double tol) const { - return U_.equals(other.U_, tol) && sign_ == other.sign_ && - std::abs(s_ - other.s_) < tol && V_.equals(other.V_, tol); + return U_.equals(other.U_, tol) && std::abs(s_ - other.s_) < tol && + V_.equals(other.V_, tol); } Vector FundamentalMatrix::localCoordinates(const FundamentalMatrix& F) const { @@ -105,10 +93,10 @@ Vector FundamentalMatrix::localCoordinates(const FundamentalMatrix& F) const { } FundamentalMatrix FundamentalMatrix::retract(const Vector& delta) const { - Rot3 newU = U_.retract(delta.head<3>()); - double newS = s_ + delta(3); // Update scalar - Rot3 newV = V_.retract(delta.tail<3>()); - return FundamentalMatrix(newU, sign_, newS, newV); + const Rot3 newU = U_.retract(delta.head<3>()); + const double newS = s_ + delta(3); + const Rot3 newV = V_.retract(delta.tail<3>()); + return FundamentalMatrix(newU, newS, newV); } //************************************************************************* diff --git a/gtsam/geometry/FundamentalMatrix.h b/gtsam/geometry/FundamentalMatrix.h index beb2947e2c..c114c2b5be 100644 --- a/gtsam/geometry/FundamentalMatrix.h +++ b/gtsam/geometry/FundamentalMatrix.h @@ -20,23 +20,20 @@ namespace gtsam { * * The FundamentalMatrix class encapsulates the fundamental matrix, which * relates corresponding points in stereo images. It is parameterized by two - * rotation matrices (U and V), a scalar parameter (s), and a sign. + * rotation matrices (U and V) and a scalar parameter (s). * Using these values, the fundamental matrix is represented as * - * F = sign * U * diag(1, s, 0) * V^T - * - * We need the `sign` because we use SO(3) for U and V, not O(3). + * F = U * diag(1, s, 0) * V^T */ class GTSAM_EXPORT FundamentalMatrix { private: - Rot3 U_; ///< Left rotation - double sign_; ///< Whether to flip the sign or not - double s_; ///< Scalar parameter for S - Rot3 V_; ///< Right rotation + Rot3 U_; ///< Left rotation + double s_; ///< Scalar parameter for S + Rot3 V_; ///< Right rotation public: /// Default constructor - FundamentalMatrix() : U_(Rot3()), sign_(1.0), s_(1.0), V_(Rot3()) {} + FundamentalMatrix() : U_(Rot3()), s_(1.0), V_(Rot3()) {} /** * @brief Construct from U, V, and scalar s @@ -113,11 +110,11 @@ class GTSAM_EXPORT FundamentalMatrix { /// @} private: /// Private constructor for internal use - FundamentalMatrix(const Rot3& U, double sign, double scaled_s, const Rot3& V) - : U_(U), sign_(sign), s_(scaled_s), V_(V) {} + FundamentalMatrix(const Rot3& U, double s, const Rot3& V) + : U_(U), s_(s), V_(V) {} /// Initialize SO(3) matrices from general O(3) matrices - void initialize(const Matrix3& U, double s, const Matrix3& V); + void initialize(Matrix3 U, double s, Matrix3 V); }; /**