From 91d1dd3a9c587a833b517c5708f7bf37b0b5739d Mon Sep 17 00:00:00 2001 From: Jade Turner Date: Tue, 22 Oct 2024 12:38:31 +0800 Subject: [PATCH 01/24] Allow parent project to override options Signed-off-by: Jade Turner --- CMakeLists.txt | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 66b1804aff..94bbe5c6ce 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,11 @@ cmake_minimum_required(VERSION 3.0) +# allow parent project to override options +if (POLICY CMP0077) + cmake_policy(SET CMP0077 NEW) +endif(POLICY CMP0077) + + # Set the version number for the library set (GTSAM_VERSION_MAJOR 4) set (GTSAM_VERSION_MINOR 3) From 26a3728d8068e50049a4d70cf5d3cc243d57c8f9 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 28 Oct 2024 08:55:49 -0700 Subject: [PATCH 02/24] 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 03/24] 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 04/24] 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 05/24] 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 06/24] 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 07/24] 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 08/24] 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 09/24] 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 10/24] 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 11/24] 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 12/24] 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 13/24] 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 14/24] 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 15/24] 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); }; /** From 9be3f41ca2164d77fe5d8b9cb46a580569cc2058 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 1 Nov 2024 19:58:23 -0400 Subject: [PATCH 16/24] Correct the second term in the pruner value so that the minNegLogConstant term is set correctly --- gtsam/hybrid/HybridGaussianConditional.cpp | 7 +++++-- gtsam/hybrid/tests/testHybridGaussianConditional.cpp | 8 ++++++++ 2 files changed, 13 insertions(+), 2 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp index ac03bd3a3e..1bec428107 100644 --- a/gtsam/hybrid/HybridGaussianConditional.cpp +++ b/gtsam/hybrid/HybridGaussianConditional.cpp @@ -322,8 +322,11 @@ HybridGaussianConditional::shared_ptr HybridGaussianConditional::prune( const GaussianFactorValuePair &pair) -> GaussianFactorValuePair { if (max->evaluate(choices) == 0.0) return {nullptr, std::numeric_limits::infinity()}; - else - return pair; + else { + // Add negLogConstant_ back so that the minimum negLogConstant in the + // HybridGaussianConditional is set correctly. + return {pair.first, pair.second + negLogConstant_}; + } }; FactorValuePairs prunedConditionals = factors().apply(pruner); diff --git a/gtsam/hybrid/tests/testHybridGaussianConditional.cpp b/gtsam/hybrid/tests/testHybridGaussianConditional.cpp index e29c485afd..350bc91848 100644 --- a/gtsam/hybrid/tests/testHybridGaussianConditional.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianConditional.cpp @@ -275,6 +275,11 @@ TEST(HybridGaussianConditional, Prune) { // Check that the pruned HybridGaussianConditional has 2 conditionals EXPECT_LONGS_EQUAL(2, pruned->nrComponents()); + + // Check that the minimum negLogConstant is set correctly + EXPECT_DOUBLES_EQUAL( + hgc.conditionals()({{M(1), 0}, {M(2), 1}})->negLogConstant(), + pruned->negLogConstant(), 1e-9); } { const std::vector potentials{0.2, 0, 0.3, 0, // @@ -285,6 +290,9 @@ TEST(HybridGaussianConditional, Prune) { // Check that the pruned HybridGaussianConditional has 3 conditionals EXPECT_LONGS_EQUAL(3, pruned->nrComponents()); + + // Check that the minimum negLogConstant is correct + EXPECT_DOUBLES_EQUAL(hgc.negLogConstant(), pruned->negLogConstant(), 1e-9); } } From e52970aa9269c79e1b508cf6cb64b31fe50b13e0 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 1 Nov 2024 20:23:04 -0400 Subject: [PATCH 17/24] negLogConstant methods for HybridBayesNet --- gtsam/hybrid/HybridBayesNet.cpp | 29 +++++++++++++++++++++++++++++ gtsam/hybrid/HybridBayesNet.h | 17 +++++++++++++++++ 2 files changed, 46 insertions(+) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index f57cda28d9..e5748366c3 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -197,6 +197,35 @@ AlgebraicDecisionTree HybridBayesNet::errorTree( return result; } +/* ************************************************************************* */ +double HybridBayesNet::negLogConstant() const { + double negLogNormConst = 0.0; + // Iterate over each conditional. + for (auto &&conditional : *this) { + negLogNormConst += conditional->negLogConstant(); + } + return negLogNormConst; +} + +/* ************************************************************************* */ +double HybridBayesNet::negLogConstant(const DiscreteValues &discrete) const { + double negLogNormConst = 0.0; + // Iterate over each conditional. + for (auto &&conditional : *this) { + if (auto gm = conditional->asHybrid()) { + negLogNormConst += gm->choose(discrete)->negLogConstant(); + } else if (auto gc = conditional->asGaussian()) { + negLogNormConst += gc->negLogConstant(); + } else if (auto dc = conditional->asDiscrete()) { + negLogNormConst += dc->choose(discrete)->negLogConstant(); + } else { + throw std::runtime_error( + "Unknown conditional type when computing negLogConstant"); + } + } + return negLogNormConst; +} + /* ************************************************************************* */ AlgebraicDecisionTree HybridBayesNet::discretePosterior( const VectorValues &continuousValues) const { diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index bba301be2f..451f7f6757 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -237,6 +237,23 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { using BayesNet::logProbability; // expose HybridValues version + /** + * @brief Get the negative log of the normalization constant corresponding + * to the joint density represented by this Bayes net. + * + * @return double + */ + double negLogConstant() const; + + /** + * @brief Get the negative log of the normalization constant + * corresponding to the joint Gaussian density represented by + * this Bayes net indexed by `discrete`. + * + * @return double + */ + double negLogConstant(const DiscreteValues &discrete) const; + /** * @brief Compute normalized posterior P(M|X=x) and return as a tree. * From 44e848536033c951a809a9bb4cabc3bbf7f17fa5 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 1 Nov 2024 20:23:32 -0400 Subject: [PATCH 18/24] get failing tests in testHybridBayesNet to pass --- gtsam/hybrid/tests/testHybridBayesNet.cpp | 42 ++++++++++++++++++----- 1 file changed, 34 insertions(+), 8 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index 16d0ae1a12..135da5dc73 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp @@ -363,10 +363,6 @@ TEST(HybridBayesNet, Pruning) { AlgebraicDecisionTree expected(s.modes, leaves); EXPECT(assert_equal(expected, discretePosterior, 1e-6)); - // Prune and get probabilities - auto prunedBayesNet = posterior->prune(2); - auto prunedTree = prunedBayesNet.discretePosterior(delta.continuous()); - // Verify logProbability computation and check specific logProbability value const DiscreteValues discrete_values{{M(0), 1}, {M(1), 1}}; const HybridValues hybridValues{delta.continuous(), discrete_values}; @@ -381,10 +377,21 @@ TEST(HybridBayesNet, Pruning) { EXPECT_DOUBLES_EQUAL(logProbability, posterior->logProbability(hybridValues), 1e-9); + double negLogConstant = posterior->negLogConstant(discrete_values); + + // The sum of all the mode densities + double normalizer = + AlgebraicDecisionTree(posterior->errorTree(delta.continuous()), + [](double error) { return exp(-error); }) + .sum(); + // Check agreement with discrete posterior - // double density = exp(logProbability); - // FAILS: EXPECT_DOUBLES_EQUAL(density, discretePosterior(discrete_values), - // 1e-6); + double density = exp(logProbability + negLogConstant) / normalizer; + EXPECT_DOUBLES_EQUAL(density, discretePosterior(discrete_values), 1e-6); + + // Prune and get probabilities + auto prunedBayesNet = posterior->prune(2); + auto prunedTree = prunedBayesNet.discretePosterior(delta.continuous()); // Regression test on pruned logProbability tree std::vector pruned_leaves = {0.0, 0.50758422, 0.0, 0.49241578}; @@ -392,7 +399,26 @@ TEST(HybridBayesNet, Pruning) { EXPECT(assert_equal(expected_pruned, prunedTree, 1e-6)); // Regression - // FAILS: EXPECT_DOUBLES_EQUAL(density, prunedTree(discrete_values), 1e-9); + double pruned_logProbability = 0; + pruned_logProbability += + prunedBayesNet.at(0)->asDiscrete()->logProbability(hybridValues); + pruned_logProbability += + prunedBayesNet.at(1)->asHybrid()->logProbability(hybridValues); + pruned_logProbability += + prunedBayesNet.at(2)->asHybrid()->logProbability(hybridValues); + pruned_logProbability += + prunedBayesNet.at(3)->asHybrid()->logProbability(hybridValues); + + double pruned_negLogConstant = prunedBayesNet.negLogConstant(discrete_values); + + // The sum of all the mode densities + double pruned_normalizer = + AlgebraicDecisionTree(prunedBayesNet.errorTree(delta.continuous()), + [](double error) { return exp(-error); }) + .sum(); + double pruned_density = + exp(pruned_logProbability + pruned_negLogConstant) / pruned_normalizer; + EXPECT_DOUBLES_EQUAL(pruned_density, prunedTree(discrete_values), 1e-9); } /* ****************************************************************************/ From 8aacfa95f32ab6459d7888146b406820a25bade0 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 1 Nov 2024 20:24:36 -0400 Subject: [PATCH 19/24] add docstrings for elimination results --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index ceabe0871a..9ca7a3938e 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -59,10 +59,11 @@ using OrphanWrapper = BayesTreeOrphanWrapper; /// Result from elimination. struct Result { + // Gaussian conditional resulting from elimination. GaussianConditional::shared_ptr conditional; - double negLogK; - GaussianFactor::shared_ptr factor; - double scalar; + double negLogK; // Negative log of the normalization constant K. + GaussianFactor::shared_ptr factor; // Leftover factor 𝜏. + double scalar; // Scalar value associated with factor 𝜏. bool operator==(const Result &other) const { return conditional == other.conditional && negLogK == other.negLogK && From 5c63ac833c56d52b1558d0be876065939394d2c6 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 3 Nov 2024 15:32:21 -0500 Subject: [PATCH 20/24] use optional DiscreteValues --- gtsam/hybrid/HybridBayesNet.cpp | 33 ++++++++++++++------------------- gtsam/hybrid/HybridBayesNet.h | 15 ++++----------- 2 files changed, 18 insertions(+), 30 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index e5748366c3..623b82eea7 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -198,29 +198,24 @@ AlgebraicDecisionTree HybridBayesNet::errorTree( } /* ************************************************************************* */ -double HybridBayesNet::negLogConstant() const { +double HybridBayesNet::negLogConstant( + const std::optional &discrete) const { double negLogNormConst = 0.0; // Iterate over each conditional. for (auto &&conditional : *this) { - negLogNormConst += conditional->negLogConstant(); - } - return negLogNormConst; -} - -/* ************************************************************************* */ -double HybridBayesNet::negLogConstant(const DiscreteValues &discrete) const { - double negLogNormConst = 0.0; - // Iterate over each conditional. - for (auto &&conditional : *this) { - if (auto gm = conditional->asHybrid()) { - negLogNormConst += gm->choose(discrete)->negLogConstant(); - } else if (auto gc = conditional->asGaussian()) { - negLogNormConst += gc->negLogConstant(); - } else if (auto dc = conditional->asDiscrete()) { - negLogNormConst += dc->choose(discrete)->negLogConstant(); + if (discrete.has_value()) { + if (auto gm = conditional->asHybrid()) { + negLogNormConst += gm->choose(*discrete)->negLogConstant(); + } else if (auto gc = conditional->asGaussian()) { + negLogNormConst += gc->negLogConstant(); + } else if (auto dc = conditional->asDiscrete()) { + negLogNormConst += dc->choose(*discrete)->negLogConstant(); + } else { + throw std::runtime_error( + "Unknown conditional type when computing negLogConstant"); + } } else { - throw std::runtime_error( - "Unknown conditional type when computing negLogConstant"); + negLogNormConst += conditional->negLogConstant(); } } return negLogNormConst; diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index 451f7f6757..96afb87d6d 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -237,22 +237,15 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { using BayesNet::logProbability; // expose HybridValues version - /** - * @brief Get the negative log of the normalization constant corresponding - * to the joint density represented by this Bayes net. - * - * @return double - */ - double negLogConstant() const; - /** * @brief Get the negative log of the normalization constant - * corresponding to the joint Gaussian density represented by - * this Bayes net indexed by `discrete`. + * corresponding to the joint density represented by this Bayes net. + * Optionally index by `discrete`. * + * @param discrete Optional DiscreteValues * @return double */ - double negLogConstant(const DiscreteValues &discrete) const; + double negLogConstant(const std::optional &discrete) const; /** * @brief Compute normalized posterior P(M|X=x) and return as a tree. From 4f0dcec1b6f09f7bfe13ac63fc662a4475798418 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 4 Nov 2024 14:57:35 -0500 Subject: [PATCH 21/24] update macos images to minimum 13 and add ARM64 support --- .github/workflows/build-macos.yml | 18 +++++++++++++++--- .github/workflows/build-python.yml | 8 ++++---- 2 files changed, 19 insertions(+), 7 deletions(-) diff --git a/.github/workflows/build-macos.yml b/.github/workflows/build-macos.yml index e4c78bf67c..44a29d42c4 100644 --- a/.github/workflows/build-macos.yml +++ b/.github/workflows/build-macos.yml @@ -25,15 +25,22 @@ jobs: # Github Actions requires a single row to be added to the build matrix. # See https://help.github.com/en/articles/workflow-syntax-for-github-actions. name: [ - macos-12-xcode-14.2, + macos-13-xcode-14.2, + macos-13-arm64-xcode-14.2, macos-14-xcode-15.4, + macos-14-arm64-xcode-15.4, ] build_type: [Debug, Release] build_unstable: [ON] include: - - name: macos-12-xcode-14.2 - os: macos-12 + - name: macos-13-xcode-14.2 + os: macos-13 + compiler: xcode + version: "14.2" + + - name: macos-13-arm64-xcode-14.2 + os: macos-13-xlarge compiler: xcode version: "14.2" @@ -42,6 +49,11 @@ jobs: compiler: xcode version: "15.4" + - name: macos-14-arm64-xcode-15.4 + os: macos-14-xlarge + compiler: xcode + version: "15.4" + steps: - name: Checkout uses: actions/checkout@v4 diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index f581a5974f..248b766d54 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -30,7 +30,7 @@ jobs: [ ubuntu-20.04-gcc-9, ubuntu-20.04-clang-9, - macos-12-xcode-14.2, + macos-14-arm64-xcode-15.4, macos-14-xcode-15.4, windows-2022-msbuild, ] @@ -48,10 +48,10 @@ jobs: compiler: clang version: "9" - - name: macos-12-xcode-14.2 - os: macos-12 + - name: macos-14-arm64-xcode-15.4 + os: macos-14-xlarge compiler: xcode - version: "14.2" + version: "15.4" - name: macos-14-xcode-15.4 os: macos-14 From 2fc11f386a4a3c9a9c518258cf1f8042a703fb13 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 4 Nov 2024 19:27:14 -0500 Subject: [PATCH 22/24] fix yaml file --- .github/workflows/build-macos.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/build-macos.yml b/.github/workflows/build-macos.yml index 44a29d42c4..56b1af35c0 100644 --- a/.github/workflows/build-macos.yml +++ b/.github/workflows/build-macos.yml @@ -39,7 +39,7 @@ jobs: compiler: xcode version: "14.2" - - name: macos-13-arm64-xcode-14.2 + - name: macos-13-arm64-xcode-14.2 os: macos-13-xlarge compiler: xcode version: "14.2" @@ -49,7 +49,7 @@ jobs: compiler: xcode version: "15.4" - - name: macos-14-arm64-xcode-15.4 + - name: macos-14-arm64-xcode-15.4 os: macos-14-xlarge compiler: xcode version: "15.4" From 53e78c6b43af0cb478f0e5e87c19b3521202eb7c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 4 Nov 2024 19:35:50 -0500 Subject: [PATCH 23/24] don't use large or xlarge runners since our payment plan doesn't support those --- .github/workflows/build-macos.yml | 12 ------------ .github/workflows/build-python.yml | 6 ------ 2 files changed, 18 deletions(-) diff --git a/.github/workflows/build-macos.yml b/.github/workflows/build-macos.yml index 56b1af35c0..e519164ec3 100644 --- a/.github/workflows/build-macos.yml +++ b/.github/workflows/build-macos.yml @@ -26,9 +26,7 @@ jobs: # See https://help.github.com/en/articles/workflow-syntax-for-github-actions. name: [ macos-13-xcode-14.2, - macos-13-arm64-xcode-14.2, macos-14-xcode-15.4, - macos-14-arm64-xcode-15.4, ] build_type: [Debug, Release] @@ -39,21 +37,11 @@ jobs: compiler: xcode version: "14.2" - - name: macos-13-arm64-xcode-14.2 - os: macos-13-xlarge - compiler: xcode - version: "14.2" - - name: macos-14-xcode-15.4 os: macos-14 compiler: xcode version: "15.4" - - name: macos-14-arm64-xcode-15.4 - os: macos-14-xlarge - compiler: xcode - version: "15.4" - steps: - name: Checkout uses: actions/checkout@v4 diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index 248b766d54..8206ffd3e9 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -30,7 +30,6 @@ jobs: [ ubuntu-20.04-gcc-9, ubuntu-20.04-clang-9, - macos-14-arm64-xcode-15.4, macos-14-xcode-15.4, windows-2022-msbuild, ] @@ -48,11 +47,6 @@ jobs: compiler: clang version: "9" - - name: macos-14-arm64-xcode-15.4 - os: macos-14-xlarge - compiler: xcode - version: "15.4" - - name: macos-14-xcode-15.4 os: macos-14 compiler: xcode From 16f9d4460dcd47be85400d67c0bdb60e9bb9c6df Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 5 Nov 2024 09:35:12 -0500 Subject: [PATCH 24/24] update macos-12 to macos-13 --- .github/workflows/build-python.yml | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index 8206ffd3e9..24a7f6c90e 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -30,6 +30,7 @@ jobs: [ ubuntu-20.04-gcc-9, ubuntu-20.04-clang-9, + macos-13-xcode-14.2, macos-14-xcode-15.4, windows-2022-msbuild, ] @@ -47,6 +48,11 @@ jobs: compiler: clang version: "9" + - name: macos-13-xcode-14.2 + os: macos-13 + compiler: xcode + version: "14.2" + - name: macos-14-xcode-15.4 os: macos-14 compiler: xcode