From 6baa7e102b29ae5009f0b9fb4f63f5c8ae972b8e Mon Sep 17 00:00:00 2001
From: Dan McGann <dmcgann@andrew.cmu.edu>
Date: Mon, 27 Mar 2023 17:28:45 -0400
Subject: [PATCH 01/15] Adds jacobians for Pose2 component extraction

Adds jacobians to translation() and rotation() for Pose2 to bring it
into spec with Pose3's equilivent functions. Also adds tests.
---
 gtsam/geometry/Pose2.cpp           | 15 +++++++++++++++
 gtsam/geometry/Pose2.h             |  8 ++++----
 gtsam/geometry/tests/testPose2.cpp | 27 +++++++++++++++++++++++++++
 3 files changed, 46 insertions(+), 4 deletions(-)

diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp
index 0d9f1bc01..4892af60a 100644
--- a/gtsam/geometry/Pose2.cpp
+++ b/gtsam/geometry/Pose2.cpp
@@ -32,6 +32,21 @@ GTSAM_CONCEPT_POSE_INST(Pose2)
 
 static const Rot2 R_PI_2(Rot2::fromCosSin(0., 1.));
 
+/* ************************************************************************* */
+const Point2 &Pose2::translation(OptionalJacobian<2, 3> Hself) const {
+  if (Hself) {
+    *Hself = Matrix::Zero(2, 3);
+    (*Hself).block<2, 2>(0, 0) = rotation().matrix();
+  }
+  return t_;
+}
+
+/* ************************************************************************* */
+const Rot2& Pose2::rotation(OptionalJacobian<1, 3> Hself) const {
+  if (Hself) *Hself << 0, 0, 1;
+  return r_;
+}
+
 /* ************************************************************************* */
 Matrix3 Pose2::matrix() const {
   Matrix2 R = r_.matrix();
diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h
index c3f588dcc..fa148b1c5 100644
--- a/gtsam/geometry/Pose2.h
+++ b/gtsam/geometry/Pose2.h
@@ -252,16 +252,16 @@ public:
   inline double theta() const { return r_.theta(); }
 
   /// translation
-  inline const Point2& t() const { return t_; }
+  inline const Point2& t() const { return translation(); }
 
   /// rotation
-  inline const Rot2&   r() const { return r_; }
+  inline const Rot2&   r() const { return rotation(); }
 
   /// translation
-  inline const Point2& translation() const { return t_; }
+  const Point2& translation(OptionalJacobian<2, 3> Hself={}) const;
 
   /// rotation
-  inline const Rot2&   rotation() const { return r_; }
+  const Rot2&   rotation(OptionalJacobian<1, 3> Hself={}) const;
 
   //// return transformation matrix
   GTSAM_EXPORT Matrix3 matrix() const;
diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp
index 8c1bdccc0..b52a6e590 100644
--- a/gtsam/geometry/tests/testPose2.cpp
+++ b/gtsam/geometry/tests/testPose2.cpp
@@ -474,6 +474,33 @@ TEST( Pose2, compose_matrix )
   EXPECT(assert_equal(gM1*_1M2,matrix(gT1.compose(_1T2)))); // RIGHT DOES NOT
 }
 
+
+
+/* ************************************************************************* */
+TEST( Pose2, translation )  {
+  Pose2 pose(3.5, -8.2, 4.2);
+
+  Matrix actualH;
+  EXPECT(assert_equal((Vector2() << 3.5, -8.2).finished(), pose.translation(actualH), 1e-8));
+
+  std::function<Point2(const Pose2&)> f = [](const Pose2& T) { return T.translation(); };
+  Matrix numericalH = numericalDerivative11<Point2, Pose2>(f, pose);
+  EXPECT(assert_equal(numericalH, actualH, 1e-6));
+}
+
+/* ************************************************************************* */
+TEST( Pose2, rotation ) {
+  Pose2 pose(3.5, -8.2, 4.2);
+
+  Matrix actualH(4, 3);
+  EXPECT(assert_equal(Rot2(4.2), pose.rotation(actualH), 1e-8));
+
+  std::function<Rot2(const Pose2&)> f = [](const Pose2& T) { return T.rotation(); };
+  Matrix numericalH = numericalDerivative11<Rot2, Pose2>(f, pose);
+  EXPECT(assert_equal(numericalH, actualH, 1e-6));
+}
+
+
 /* ************************************************************************* */
 TEST( Pose2, between )
 {

From d6a24847f182e76f9140f8e0365fee3de8410c2f Mon Sep 17 00:00:00 2001
From: Dan McGann <dmcgann@andrew.cmu.edu>
Date: Mon, 27 Mar 2023 18:05:18 -0400
Subject: [PATCH 02/15] Extends python iface Pose2 component jacobians

---
 gtsam/geometry/geometry.i | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i
index e9929227a..4ea322fa7 100644
--- a/gtsam/geometry/geometry.i
+++ b/gtsam/geometry/geometry.i
@@ -424,7 +424,9 @@ class Pose2 {
   gtsam::Rot2 bearing(const gtsam::Point2& point) const;
   double range(const gtsam::Point2& point) const;
   gtsam::Point2 translation() const;
+  gtsam::Point2 translation(Eigen::Ref<Eigen::MatrixXd> Hself) const;
   gtsam::Rot2 rotation() const;
+  gtsam::Rot2 rotation(Eigen::Ref<Eigen::MatrixXd> Hself) const;
   Matrix matrix() const;
 
   // enabling serialization functionality

From 7e30cc7c9d4ec295ea0a08171ad238681b52ce08 Mon Sep 17 00:00:00 2001
From: Dan McGann <dmcgann@andrew.cmu.edu>
Date: Sun, 9 Apr 2023 11:38:40 -0400
Subject: [PATCH 03/15] Convert Pose2 r/t accessors to inline

---
 gtsam/geometry/Pose2.cpp | 15 ---------------
 gtsam/geometry/Pose2.h   | 13 +++++++++++--
 2 files changed, 11 insertions(+), 17 deletions(-)

diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp
index 4892af60a..0d9f1bc01 100644
--- a/gtsam/geometry/Pose2.cpp
+++ b/gtsam/geometry/Pose2.cpp
@@ -32,21 +32,6 @@ GTSAM_CONCEPT_POSE_INST(Pose2)
 
 static const Rot2 R_PI_2(Rot2::fromCosSin(0., 1.));
 
-/* ************************************************************************* */
-const Point2 &Pose2::translation(OptionalJacobian<2, 3> Hself) const {
-  if (Hself) {
-    *Hself = Matrix::Zero(2, 3);
-    (*Hself).block<2, 2>(0, 0) = rotation().matrix();
-  }
-  return t_;
-}
-
-/* ************************************************************************* */
-const Rot2& Pose2::rotation(OptionalJacobian<1, 3> Hself) const {
-  if (Hself) *Hself << 0, 0, 1;
-  return r_;
-}
-
 /* ************************************************************************* */
 Matrix3 Pose2::matrix() const {
   Matrix2 R = r_.matrix();
diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h
index fa148b1c5..80418745e 100644
--- a/gtsam/geometry/Pose2.h
+++ b/gtsam/geometry/Pose2.h
@@ -258,10 +258,19 @@ public:
   inline const Rot2&   r() const { return rotation(); }
 
   /// translation
-  const Point2& translation(OptionalJacobian<2, 3> Hself={}) const;
+  inline const Point2& translation(OptionalJacobian<2, 3> Hself={}) const {
+    if (Hself) {
+      *Hself = Matrix::Zero(2, 3);
+      (*Hself).block<2, 2>(0, 0) = rotation().matrix();
+    }
+    return t_;
+  }
 
   /// rotation
-  const Rot2&   rotation(OptionalJacobian<1, 3> Hself={}) const;
+  inline const Rot2&   rotation(OptionalJacobian<1, 3> Hself={}) const {
+    if (Hself) *Hself << 0, 0, 1;
+    return r_;
+  }
 
   //// return transformation matrix
   GTSAM_EXPORT Matrix3 matrix() const;

From 2175f804ddb46d9e0731e2b92899e7f226a8f089 Mon Sep 17 00:00:00 2001
From: Dan McGann <dmcgann@andrew.cmu.edu>
Date: Sun, 9 Apr 2023 14:18:53 -0400
Subject: [PATCH 04/15] Revert changes to Pose2 r(), t()

---
 gtsam/geometry/Pose2.h | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h
index 80418745e..f1b38c5a6 100644
--- a/gtsam/geometry/Pose2.h
+++ b/gtsam/geometry/Pose2.h
@@ -252,10 +252,10 @@ public:
   inline double theta() const { return r_.theta(); }
 
   /// translation
-  inline const Point2& t() const { return translation(); }
+  inline const Point2& t() const { return t_; }
 
   /// rotation
-  inline const Rot2&   r() const { return rotation(); }
+  inline const Rot2&   r() const { return r_; }
 
   /// translation
   inline const Point2& translation(OptionalJacobian<2, 3> Hself={}) const {

From 3eec88f60eaae0c1db1cecdab6d20d24a64c9030 Mon Sep 17 00:00:00 2001
From: Travis Driver <drivertp@gmail.com>
Date: Fri, 2 Jun 2023 15:44:51 -0700
Subject: [PATCH 05/15] Added rank threshold to triangulateLOST

---
 gtsam/geometry/triangulation.cpp |  57 +++---
 gtsam/geometry/triangulation.h   | 305 +++++++++++++++----------------
 2 files changed, 173 insertions(+), 189 deletions(-)

diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp
index 06fb8fafe..2049ce384 100644
--- a/gtsam/geometry/triangulation.cpp
+++ b/gtsam/geometry/triangulation.cpp
@@ -25,9 +25,9 @@
 namespace gtsam {
 
 Vector4 triangulateHomogeneousDLT(
-    const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>&
-        projection_matrices,
-    const Point2Vector& measurements, double rank_tol) {
+    const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
+    const Point2Vector& measurements,
+    double rank_tol) {
   // number of cameras
   size_t m = projection_matrices.size();
 
@@ -56,9 +56,9 @@ Vector4 triangulateHomogeneousDLT(
 }
 
 Vector4 triangulateHomogeneousDLT(
-    const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>&
-        projection_matrices,
-    const std::vector<Unit3>& measurements, double rank_tol) {
+    const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
+    const std::vector<Unit3>& measurements,
+    double rank_tol) {
   // number of cameras
   size_t m = projection_matrices.size();
 
@@ -68,9 +68,7 @@ Vector4 triangulateHomogeneousDLT(
   for (size_t i = 0; i < m; i++) {
     size_t row = i * 2;
     const Matrix34& projection = projection_matrices.at(i);
-    const Point3& p =
-        measurements.at(i)
-            .point3();  // to get access to x,y,z of the bearing vector
+    const Point3& p = measurements.at(i).point3();  // to get access to x,y,z of the bearing vector
 
     // build system of equations
     A.row(row) = p.x() * projection.row(2) - p.z() * projection.row(0);
@@ -85,7 +83,8 @@ Vector4 triangulateHomogeneousDLT(
 
 Point3 triangulateLOST(const std::vector<Pose3>& poses,
                        const Point3Vector& calibratedMeasurements,
-                       const SharedIsotropic& measurementNoise) {
+                       const SharedIsotropic& measurementNoise,
+                       double rank_tol) {
   size_t m = calibratedMeasurements.size();
   assert(m == poses.size());
 
@@ -105,36 +104,39 @@ Point3 triangulateLOST(const std::vector<Pose3>& poses,
     const Point3 wZj = wTj.rotation().rotate(calibratedMeasurements[j]);
 
     // Note: Setting q_i = 1.0 gives same results as DLT.
-    const double q_i = wZi.cross(wZj).norm() /
-                       (measurementNoise->sigma() * d_ij.cross(wZj).norm());
+    const double q_i = wZi.cross(wZj).norm() / (measurementNoise->sigma() * d_ij.cross(wZj).norm());
 
-    const Matrix23 coefficientMat =
-        q_i * skewSymmetric(calibratedMeasurements[i]).topLeftCorner(2, 3) *
-        wTi.rotation().matrix().transpose();
+    const Matrix23 coefficientMat = q_i *
+                                    skewSymmetric(calibratedMeasurements[i]).topLeftCorner(2, 3) *
+                                    wTi.rotation().matrix().transpose();
 
     A.block<2, 3>(2 * i, 0) << coefficientMat;
     b.block<2, 1>(2 * i, 0) << coefficientMat * wTi.translation();
   }
-  return A.colPivHouseholderQr().solve(b);
+
+  Eigen::ColPivHouseholderQR<Matrix> A_Qr = A.colPivHouseholderQr();
+  A_Qr.setThreshold(rank_tol);
+
+  // if (A_Qr.rank() < 3) throw(TriangulationUnderconstrainedException());
+
+  return A_Qr.solve(b);
 }
 
 Point3 triangulateDLT(
-    const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>&
-        projection_matrices,
-    const Point2Vector& measurements, double rank_tol) {
-  Vector4 v =
-      triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol);
+    const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
+    const Point2Vector& measurements,
+    double rank_tol) {
+  Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol);
   // Create 3D point from homogeneous coordinates
   return Point3(v.head<3>() / v[3]);
 }
 
 Point3 triangulateDLT(
-    const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>&
-        projection_matrices,
-    const std::vector<Unit3>& measurements, double rank_tol) {
+    const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
+    const std::vector<Unit3>& measurements,
+    double rank_tol) {
   // contrary to previous triangulateDLT, this is now taking Unit3 inputs
-  Vector4 v =
-      triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol);
+  Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol);
   // Create 3D point from homogeneous coordinates
   return Point3(v.head<3>() / v[3]);
 }
@@ -146,8 +148,7 @@ Point3 triangulateDLT(
  * @param landmarkKey to refer to landmark
  * @return refined Point3
  */
-Point3 optimize(const NonlinearFactorGraph& graph, const Values& values,
-                Key landmarkKey) {
+Point3 optimize(const NonlinearFactorGraph& graph, const Values& values, Key landmarkKey) {
   // Maybe we should consider Gauss-Newton?
   LevenbergMarquardtParams params;
   params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h
index 7e58cee2d..61ca5588a 100644
--- a/gtsam/geometry/triangulation.h
+++ b/gtsam/geometry/triangulation.h
@@ -20,39 +20,38 @@
 
 #pragma once
 
-#include "gtsam/geometry/Point3.h"
 #include <gtsam/geometry/Cal3Bundler.h>
+#include <gtsam/geometry/Cal3DS2.h>
 #include <gtsam/geometry/Cal3Fisheye.h>
 #include <gtsam/geometry/Cal3Unified.h>
 #include <gtsam/geometry/Cal3_S2.h>
-#include <gtsam/geometry/Cal3DS2.h>
 #include <gtsam/geometry/CameraSet.h>
 #include <gtsam/geometry/PinholeCamera.h>
-#include <gtsam/geometry/SphericalCamera.h>
 #include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/SphericalCamera.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/slam/TriangulationFactor.h>
+#include "gtsam/geometry/Point3.h"
 
 #include <optional>
 
 namespace gtsam {
 
 /// Exception thrown by triangulateDLT when SVD returns rank < 3
-class GTSAM_EXPORT TriangulationUnderconstrainedException: public std::runtime_error {
-public:
-  TriangulationUnderconstrainedException() :
-      std::runtime_error("Triangulation Underconstrained Exception.") {
-  }
+class GTSAM_EXPORT TriangulationUnderconstrainedException : public std::runtime_error {
+ public:
+  TriangulationUnderconstrainedException()
+      : std::runtime_error("Triangulation Underconstrained Exception.") {}
 };
 
 /// Exception thrown by triangulateDLT when landmark is behind one or more of the cameras
-class GTSAM_EXPORT TriangulationCheiralityException: public std::runtime_error {
-public:
-  TriangulationCheiralityException() :
-      std::runtime_error(
-          "Triangulation Cheirality Exception: The resulting landmark is behind one or more cameras.") {
-  }
+class GTSAM_EXPORT TriangulationCheiralityException : public std::runtime_error {
+ public:
+  TriangulationCheiralityException()
+      : std::runtime_error(
+            "Triangulation Cheirality Exception: The resulting landmark is behind one or more "
+            "cameras.") {}
 };
 
 /**
@@ -64,11 +63,13 @@ public:
  */
 GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(
     const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
-    const Point2Vector& measurements, double rank_tol = 1e-9);
+    const Point2Vector& measurements,
+    double rank_tol = 1e-9);
 
 /**
  * Same math as Hartley and Zisserman, 2nd Ed., page 312, but with unit-norm bearing vectors
- * (contrarily to pinhole projection, the z entry is not assumed to be 1 as in Hartley and Zisserman)
+ * (contrarily to pinhole projection, the z entry is not assumed to be 1 as in Hartley and
+ * Zisserman)
  * @param projection_matrices Projection matrices (K*P^-1)
  * @param measurements Unit3 bearing measurements
  * @param rank_tol SVD rank tolerance
@@ -76,7 +77,8 @@ GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(
  */
 GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(
     const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
-    const std::vector<Unit3>& measurements, double rank_tol = 1e-9);
+    const std::vector<Unit3>& measurements,
+    double rank_tol = 1e-9);
 
 /**
  * DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312
@@ -85,18 +87,18 @@ GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(
  * @param rank_tol SVD rank tolerance
  * @return Triangulated Point3
  */
-GTSAM_EXPORT Point3 triangulateDLT(
-    const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
-    const Point2Vector& measurements,
-    double rank_tol = 1e-9);
+GTSAM_EXPORT Point3
+triangulateDLT(const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
+               const Point2Vector& measurements,
+               double rank_tol = 1e-9);
 
 /**
  * overload of previous function to work with Unit3 (projected to canonical camera)
  */
-GTSAM_EXPORT Point3 triangulateDLT(
-    const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
-    const std::vector<Unit3>& measurements,
-    double rank_tol = 1e-9);
+GTSAM_EXPORT Point3
+triangulateDLT(const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
+               const std::vector<Unit3>& measurements,
+               double rank_tol = 1e-9);
 
 /**
  * @brief Triangulation using the LOST (Linear Optimal Sine Triangulation)
@@ -110,7 +112,8 @@ GTSAM_EXPORT Point3 triangulateDLT(
  */
 GTSAM_EXPORT Point3 triangulateLOST(const std::vector<Pose3>& poses,
                                     const Point3Vector& calibratedMeasurements,
-                                    const SharedIsotropic& measurementNoise);
+                                    const SharedIsotropic& measurementNoise,
+                                    double rank_tol = 1e-9);
 
 /**
  * Create a factor graph with projection factors from poses and one calibration
@@ -121,20 +124,22 @@ GTSAM_EXPORT Point3 triangulateLOST(const std::vector<Pose3>& poses,
  * @param initialEstimate
  * @return graph and initial values
  */
-template<class CALIBRATION>
+template <class CALIBRATION>
 std::pair<NonlinearFactorGraph, Values> triangulationGraph(
-    const std::vector<Pose3>& poses, std::shared_ptr<CALIBRATION> sharedCal,
-    const Point2Vector& measurements, Key landmarkKey,
+    const std::vector<Pose3>& poses,
+    std::shared_ptr<CALIBRATION> sharedCal,
+    const Point2Vector& measurements,
+    Key landmarkKey,
     const Point3& initialEstimate,
     const SharedNoiseModel& model = noiseModel::Unit::Create(2)) {
   Values values;
-  values.insert(landmarkKey, initialEstimate); // Initial landmark value
+  values.insert(landmarkKey, initialEstimate);  // Initial landmark value
   NonlinearFactorGraph graph;
   for (size_t i = 0; i < measurements.size(); i++) {
     const Pose3& pose_i = poses[i];
     typedef PinholePose<CALIBRATION> Camera;
     Camera camera_i(pose_i, sharedCal);
-    graph.emplace_shared<TriangulationFactor<Camera> > //
+    graph.emplace_shared<TriangulationFactor<Camera>>  //
         (camera_i, measurements[i], model, landmarkKey);
   }
   return {graph, values};
@@ -149,21 +154,22 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
  * @param initialEstimate
  * @return graph and initial values
  */
-template<class CAMERA>
+template <class CAMERA>
 std::pair<NonlinearFactorGraph, Values> triangulationGraph(
     const CameraSet<CAMERA>& cameras,
-    const typename CAMERA::MeasurementVector& measurements, Key landmarkKey,
+    const typename CAMERA::MeasurementVector& measurements,
+    Key landmarkKey,
     const Point3& initialEstimate,
     const SharedNoiseModel& model = nullptr) {
   Values values;
-  values.insert(landmarkKey, initialEstimate); // Initial landmark value
+  values.insert(landmarkKey, initialEstimate);  // Initial landmark value
   NonlinearFactorGraph graph;
-  static SharedNoiseModel unit(noiseModel::Unit::Create(
-      traits<typename CAMERA::Measurement>::dimension));
+  static SharedNoiseModel unit(
+      noiseModel::Unit::Create(traits<typename CAMERA::Measurement>::dimension));
   for (size_t i = 0; i < measurements.size(); i++) {
     const CAMERA& camera_i = cameras[i];
-    graph.emplace_shared<TriangulationFactor<CAMERA> > //
-        (camera_i, measurements[i], model? model : unit, landmarkKey);
+    graph.emplace_shared<TriangulationFactor<CAMERA>>  //
+        (camera_i, measurements[i], model ? model : unit, landmarkKey);
   }
   return {graph, values};
 }
@@ -176,7 +182,8 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
  * @return refined Point3
  */
 GTSAM_EXPORT Point3 optimize(const NonlinearFactorGraph& graph,
-    const Values& values, Key landmarkKey);
+                             const Values& values,
+                             Key landmarkKey);
 
 /**
  * Given an initial estimate , refine a point using measurements in several cameras
@@ -186,14 +193,14 @@ GTSAM_EXPORT Point3 optimize(const NonlinearFactorGraph& graph,
  * @param initialEstimate
  * @return refined Point3
  */
-template<class CALIBRATION>
+template <class CALIBRATION>
 Point3 triangulateNonlinear(const std::vector<Pose3>& poses,
-    std::shared_ptr<CALIBRATION> sharedCal,
-    const Point2Vector& measurements, const Point3& initialEstimate,
-    const SharedNoiseModel& model = nullptr) {
-
+                            std::shared_ptr<CALIBRATION> sharedCal,
+                            const Point2Vector& measurements,
+                            const Point3& initialEstimate,
+                            const SharedNoiseModel& model = nullptr) {
   // Create a factor graph and initial values
-  const auto [graph, values] = triangulationGraph<CALIBRATION> //
+  const auto [graph, values] = triangulationGraph<CALIBRATION>  //
       (poses, sharedCal, measurements, Symbol('p', 0), initialEstimate, model);
 
   return optimize(graph, values, Symbol('p', 0));
@@ -206,33 +213,32 @@ Point3 triangulateNonlinear(const std::vector<Pose3>& poses,
  * @param initialEstimate
  * @return refined Point3
  */
-template<class CAMERA>
-Point3 triangulateNonlinear(
-    const CameraSet<CAMERA>& cameras,
-    const typename CAMERA::MeasurementVector& measurements, const Point3& initialEstimate,
-    const SharedNoiseModel& model = nullptr) {
-
+template <class CAMERA>
+Point3 triangulateNonlinear(const CameraSet<CAMERA>& cameras,
+                            const typename CAMERA::MeasurementVector& measurements,
+                            const Point3& initialEstimate,
+                            const SharedNoiseModel& model = nullptr) {
   // Create a factor graph and initial values
-  const auto [graph, values] = triangulationGraph<CAMERA> //
+  const auto [graph, values] = triangulationGraph<CAMERA>  //
       (cameras, measurements, Symbol('p', 0), initialEstimate, model);
 
   return optimize(graph, values, Symbol('p', 0));
 }
 
-template<class CAMERA>
-std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>
-projectionMatricesFromCameras(const CameraSet<CAMERA> &cameras) {
+template <class CAMERA>
+std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projectionMatricesFromCameras(
+    const CameraSet<CAMERA>& cameras) {
   std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices;
-  for (const CAMERA &camera: cameras) {
+  for (const CAMERA& camera : cameras) {
     projection_matrices.push_back(camera.cameraProjectionMatrix());
   }
   return projection_matrices;
 }
 
 // overload, assuming pinholePose
-template<class CALIBRATION>
+template <class CALIBRATION>
 std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projectionMatricesFromPoses(
-        const std::vector<Pose3> &poses, std::shared_ptr<CALIBRATION> sharedCal) {
+    const std::vector<Pose3>& poses, std::shared_ptr<CALIBRATION> sharedCal) {
   std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices;
   for (size_t i = 0; i < poses.size(); i++) {
     PinholePose<CALIBRATION> camera(poses.at(i), sharedCal);
@@ -257,9 +263,9 @@ Cal3_S2 createPinholeCalibration(const CALIBRATION& cal) {
 /** Internal undistortMeasurement to be used by undistortMeasurement and
  * undistortMeasurements */
 template <class CALIBRATION, class MEASUREMENT>
-MEASUREMENT undistortMeasurementInternal(
-    const CALIBRATION& cal, const MEASUREMENT& measurement,
-    std::optional<Cal3_S2> pinholeCal = {}) {
+MEASUREMENT undistortMeasurementInternal(const CALIBRATION& cal,
+                                         const MEASUREMENT& measurement,
+                                         std::optional<Cal3_S2> pinholeCal = {}) {
   if (!pinholeCal) {
     pinholeCal = createPinholeCalibration(cal);
   }
@@ -278,13 +284,13 @@ MEASUREMENT undistortMeasurementInternal(
  * @return measurements with the effect of the distortion of sharedCal removed.
  */
 template <class CALIBRATION>
-Point2Vector undistortMeasurements(const CALIBRATION& cal,
-                                   const Point2Vector& measurements) {
+Point2Vector undistortMeasurements(const CALIBRATION& cal, const Point2Vector& measurements) {
   Cal3_S2 pinholeCalibration = createPinholeCalibration(cal);
   Point2Vector undistortedMeasurements;
   // Calibrate with cal and uncalibrate with pinhole version of cal so that
   // measurements are undistorted.
-  std::transform(measurements.begin(), measurements.end(),
+  std::transform(measurements.begin(),
+                 measurements.end(),
                  std::back_inserter(undistortedMeasurements),
                  [&cal, &pinholeCalibration](const Point2& measurement) {
                    return undistortMeasurementInternal<CALIBRATION>(
@@ -295,8 +301,7 @@ Point2Vector undistortMeasurements(const CALIBRATION& cal,
 
 /** Specialization for Cal3_S2 as it doesn't need to be undistorted. */
 template <>
-inline Point2Vector undistortMeasurements(const Cal3_S2& cal,
-                                          const Point2Vector& measurements) {
+inline Point2Vector undistortMeasurements(const Cal3_S2& cal, const Point2Vector& measurements) {
   return measurements;
 }
 
@@ -313,17 +318,15 @@ inline Point2Vector undistortMeasurements(const Cal3_S2& cal,
  */
 template <class CAMERA>
 typename CAMERA::MeasurementVector undistortMeasurements(
-    const CameraSet<CAMERA>& cameras,
-    const typename CAMERA::MeasurementVector& measurements) {
+    const CameraSet<CAMERA>& cameras, const typename CAMERA::MeasurementVector& measurements) {
   const size_t nrMeasurements = measurements.size();
   assert(nrMeasurements == cameras.size());
   typename CAMERA::MeasurementVector undistortedMeasurements(nrMeasurements);
   for (size_t ii = 0; ii < nrMeasurements; ++ii) {
     // Calibrate with cal and uncalibrate with pinhole version of cal so that
     // measurements are undistorted.
-    undistortedMeasurements[ii] =
-        undistortMeasurementInternal<typename CAMERA::CalibrationType>(
-            cameras[ii].calibration(), measurements[ii]);
+    undistortedMeasurements[ii] = undistortMeasurementInternal<typename CAMERA::CalibrationType>(
+        cameras[ii].calibration(), measurements[ii]);
   }
   return undistortedMeasurements;
 }
@@ -353,12 +356,13 @@ inline SphericalCamera::MeasurementVector undistortMeasurements(
  * @return homogeneous measurements in image plane
  */
 template <class CALIBRATION>
-inline Point3Vector calibrateMeasurementsShared(
-    const CALIBRATION& cal, const Point2Vector& measurements) {
+inline Point3Vector calibrateMeasurementsShared(const CALIBRATION& cal,
+                                                const Point2Vector& measurements) {
   Point3Vector calibratedMeasurements;
   // Calibrate with cal and uncalibrate with pinhole version of cal so that
   // measurements are undistorted.
-  std::transform(measurements.begin(), measurements.end(),
+  std::transform(measurements.begin(),
+                 measurements.end(),
                  std::back_inserter(calibratedMeasurements),
                  [&cal](const Point2& measurement) {
                    Point3 p;
@@ -377,25 +381,21 @@ inline Point3Vector calibrateMeasurementsShared(
  * @return homogeneous measurements in image plane
  */
 template <class CAMERA>
-inline Point3Vector calibrateMeasurements(
-    const CameraSet<CAMERA>& cameras,
-    const typename CAMERA::MeasurementVector& measurements) {
+inline Point3Vector calibrateMeasurements(const CameraSet<CAMERA>& cameras,
+                                          const typename CAMERA::MeasurementVector& measurements) {
   const size_t nrMeasurements = measurements.size();
   assert(nrMeasurements == cameras.size());
   Point3Vector calibratedMeasurements(nrMeasurements);
   for (size_t ii = 0; ii < nrMeasurements; ++ii) {
-    calibratedMeasurements[ii]
-        << cameras[ii].calibration().calibrate(measurements[ii]),
-        1.0;
+    calibratedMeasurements[ii] << cameras[ii].calibration().calibrate(measurements[ii]), 1.0;
   }
   return calibratedMeasurements;
 }
 
 /** Specialize for SphericalCamera to do nothing. */
 template <class CAMERA = SphericalCamera>
-inline Point3Vector calibrateMeasurements(
-    const CameraSet<SphericalCamera>& cameras,
-    const SphericalCamera::MeasurementVector& measurements) {
+inline Point3Vector calibrateMeasurements(const CameraSet<SphericalCamera>& cameras,
+                                          const SphericalCamera::MeasurementVector& measurements) {
   Point3Vector calibratedMeasurements(measurements.size());
   for (size_t ii = 0; ii < measurements.size(); ++ii) {
     calibratedMeasurements[ii] << measurements[ii].point3();
@@ -420,7 +420,8 @@ template <class CALIBRATION>
 Point3 triangulatePoint3(const std::vector<Pose3>& poses,
                          std::shared_ptr<CALIBRATION> sharedCal,
                          const Point2Vector& measurements,
-                         double rank_tol = 1e-9, bool optimize = false,
+                         double rank_tol = 1e-9,
+                         bool optimize = false,
                          const SharedNoiseModel& model = nullptr,
                          const bool useLOST = false) {
   assert(poses.size() == measurements.size());
@@ -432,24 +433,21 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
     // Reduce input noise model to an isotropic noise model using the mean of
     // the diagonal.
     const double measurementSigma = model ? model->sigmas().mean() : 1e-4;
-    SharedIsotropic measurementNoise =
-        noiseModel::Isotropic::Sigma(2, measurementSigma);
+    SharedIsotropic measurementNoise = noiseModel::Isotropic::Sigma(2, measurementSigma);
     // calibrate the measurements to obtain homogenous coordinates in image
     // plane.
     auto calibratedMeasurements =
         calibrateMeasurementsShared<CALIBRATION>(*sharedCal, measurements);
 
-    point = triangulateLOST(poses, calibratedMeasurements, measurementNoise);
+    point = triangulateLOST(poses, calibratedMeasurements, measurementNoise, rank_tol);
   } else {
     // construct projection matrices from poses & calibration
     auto projection_matrices = projectionMatricesFromPoses(poses, sharedCal);
 
     // Undistort the measurements, leaving only the pinhole elements in effect.
-    auto undistortedMeasurements =
-        undistortMeasurements<CALIBRATION>(*sharedCal, measurements);
+    auto undistortedMeasurements = undistortMeasurements<CALIBRATION>(*sharedCal, measurements);
 
-    point =
-        triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
+    point = triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
   }
 
   // Then refine using non-linear optimization
@@ -485,7 +483,8 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
 template <class CAMERA>
 Point3 triangulatePoint3(const CameraSet<CAMERA>& cameras,
                          const typename CAMERA::MeasurementVector& measurements,
-                         double rank_tol = 1e-9, bool optimize = false,
+                         double rank_tol = 1e-9,
+                         bool optimize = false,
                          const SharedNoiseModel& model = nullptr,
                          const bool useLOST = false) {
   size_t m = cameras.size();
@@ -499,8 +498,7 @@ Point3 triangulatePoint3(const CameraSet<CAMERA>& cameras,
     // Reduce input noise model to an isotropic noise model using the mean of
     // the diagonal.
     const double measurementSigma = model ? model->sigmas().mean() : 1e-4;
-    SharedIsotropic measurementNoise =
-        noiseModel::Isotropic::Sigma(2, measurementSigma);
+    SharedIsotropic measurementNoise = noiseModel::Isotropic::Sigma(2, measurementSigma);
 
     // construct poses from cameras.
     std::vector<Pose3> poses;
@@ -509,20 +507,17 @@ Point3 triangulatePoint3(const CameraSet<CAMERA>& cameras,
 
     // calibrate the measurements to obtain homogenous coordinates in image
     // plane.
-    auto calibratedMeasurements =
-        calibrateMeasurements<CAMERA>(cameras, measurements);
+    auto calibratedMeasurements = calibrateMeasurements<CAMERA>(cameras, measurements);
 
-    point = triangulateLOST(poses, calibratedMeasurements, measurementNoise);
+    point = triangulateLOST(poses, calibratedMeasurements, measurementNoise, rank_tol);
   } else {
     // construct projection matrices from poses & calibration
     auto projection_matrices = projectionMatricesFromCameras(cameras);
 
     // Undistort the measurements, leaving only the pinhole elements in effect.
-    auto undistortedMeasurements =
-        undistortMeasurements<CAMERA>(cameras, measurements);
+    auto undistortedMeasurements = undistortMeasurements<CAMERA>(cameras, measurements);
 
-    point =
-        triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
+    point = triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
   }
 
   // Then refine using non-linear optimization
@@ -545,7 +540,8 @@ Point3 triangulatePoint3(const CameraSet<CAMERA>& cameras,
 template <class CALIBRATION>
 Point3 triangulatePoint3(const CameraSet<PinholeCamera<CALIBRATION>>& cameras,
                          const Point2Vector& measurements,
-                         double rank_tol = 1e-9, bool optimize = false,
+                         double rank_tol = 1e-9,
+                         bool optimize = false,
                          const SharedNoiseModel& model = nullptr,
                          const bool useLOST = false) {
   return triangulatePoint3<PinholeCamera<CALIBRATION>>  //
@@ -553,16 +549,16 @@ Point3 triangulatePoint3(const CameraSet<PinholeCamera<CALIBRATION>>& cameras,
 }
 
 struct GTSAM_EXPORT TriangulationParameters {
-
-  double rankTolerance; ///< threshold to decide whether triangulation is result.degenerate
-  ///< (the rank is the number of singular values of the triangulation matrix which are larger than rankTolerance)
-  bool enableEPI; ///< if set to true, will refine triangulation using LM
+  double rankTolerance;  ///< threshold to decide whether triangulation is result.degenerate
+  ///< (the rank is the number of singular values of the triangulation matrix which are larger than
+  ///< rankTolerance)
+  bool enableEPI;  ///< if set to true, will refine triangulation using LM
 
   /**
    * if the landmark is triangulated at distance larger than this,
    * result is flagged as degenerate.
    */
-  double landmarkDistanceThreshold; //
+  double landmarkDistanceThreshold;  //
 
   /**
    * If this is nonnegative the we will check if the average reprojection error
@@ -571,7 +567,7 @@ struct GTSAM_EXPORT TriangulationParameters {
    */
   double dynamicOutlierRejectionThreshold;
 
-  SharedNoiseModel noiseModel; ///< used in the nonlinear triangulation
+  SharedNoiseModel noiseModel;  ///< used in the nonlinear triangulation
 
   /**
    * Constructor
@@ -583,39 +579,36 @@ struct GTSAM_EXPORT TriangulationParameters {
    *
    */
   TriangulationParameters(const double _rankTolerance = 1.0,
-      const bool _enableEPI = false, double _landmarkDistanceThreshold = -1,
-      double _dynamicOutlierRejectionThreshold = -1,
-      const SharedNoiseModel& _noiseModel = nullptr) :
-      rankTolerance(_rankTolerance), enableEPI(_enableEPI), //
-      landmarkDistanceThreshold(_landmarkDistanceThreshold), //
-      dynamicOutlierRejectionThreshold(_dynamicOutlierRejectionThreshold),
-      noiseModel(_noiseModel){
-  }
+                          const bool _enableEPI = false,
+                          double _landmarkDistanceThreshold = -1,
+                          double _dynamicOutlierRejectionThreshold = -1,
+                          const SharedNoiseModel& _noiseModel = nullptr)
+      : rankTolerance(_rankTolerance),
+        enableEPI(_enableEPI),                                  //
+        landmarkDistanceThreshold(_landmarkDistanceThreshold),  //
+        dynamicOutlierRejectionThreshold(_dynamicOutlierRejectionThreshold),
+        noiseModel(_noiseModel) {}
 
   // stream to output
-  friend std::ostream &operator<<(std::ostream &os,
-      const TriangulationParameters& p) {
+  friend std::ostream& operator<<(std::ostream& os, const TriangulationParameters& p) {
     os << "rankTolerance = " << p.rankTolerance << std::endl;
     os << "enableEPI = " << p.enableEPI << std::endl;
-    os << "landmarkDistanceThreshold = " << p.landmarkDistanceThreshold
-        << std::endl;
-    os << "dynamicOutlierRejectionThreshold = "
-        << p.dynamicOutlierRejectionThreshold << std::endl;
+    os << "landmarkDistanceThreshold = " << p.landmarkDistanceThreshold << std::endl;
+    os << "dynamicOutlierRejectionThreshold = " << p.dynamicOutlierRejectionThreshold << std::endl;
     os << "noise model" << std::endl;
     return os;
   }
 
-private:
-
+ private:
 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   /// Serialization function
   friend class boost::serialization::access;
-  template<class ARCHIVE>
-  void serialize(ARCHIVE & ar, const unsigned int version) {
-    ar & BOOST_SERIALIZATION_NVP(rankTolerance);
-    ar & BOOST_SERIALIZATION_NVP(enableEPI);
-    ar & BOOST_SERIALIZATION_NVP(landmarkDistanceThreshold);
-    ar & BOOST_SERIALIZATION_NVP(dynamicOutlierRejectionThreshold);
+  template <class ARCHIVE>
+  void serialize(ARCHIVE& ar, const unsigned int version) {
+    ar& BOOST_SERIALIZATION_NVP(rankTolerance);
+    ar& BOOST_SERIALIZATION_NVP(enableEPI);
+    ar& BOOST_SERIALIZATION_NVP(landmarkDistanceThreshold);
+    ar& BOOST_SERIALIZATION_NVP(dynamicOutlierRejectionThreshold);
   }
 #endif
 };
@@ -642,16 +635,10 @@ class TriangulationResult : public std::optional<Point3> {
    * Constructor
    */
   TriangulationResult(const Point3& p) : status(VALID) { emplace(p); }
-  static TriangulationResult Degenerate() {
-    return TriangulationResult(DEGENERATE);
-  }
+  static TriangulationResult Degenerate() { return TriangulationResult(DEGENERATE); }
   static TriangulationResult Outlier() { return TriangulationResult(OUTLIER); }
-  static TriangulationResult FarPoint() {
-    return TriangulationResult(FAR_POINT);
-  }
-  static TriangulationResult BehindCamera() {
-    return TriangulationResult(BEHIND_CAMERA);
-  }
+  static TriangulationResult FarPoint() { return TriangulationResult(FAR_POINT); }
+  static TriangulationResult BehindCamera() { return TriangulationResult(BEHIND_CAMERA); }
   bool valid() const { return status == VALID; }
   bool degenerate() const { return status == DEGENERATE; }
   bool outlier() const { return status == OUTLIER; }
@@ -662,8 +649,7 @@ class TriangulationResult : public std::optional<Point3> {
     return value();
   }
   // stream to output
-  friend std::ostream& operator<<(std::ostream& os,
-                                  const TriangulationResult& result) {
+  friend std::ostream& operator<<(std::ostream& os, const TriangulationResult& result) {
     if (result)
       os << "point = " << *result << std::endl;
     else
@@ -683,11 +669,10 @@ class TriangulationResult : public std::optional<Point3> {
 };
 
 /// triangulateSafe: extensive checking of the outcome
-template<class CAMERA>
+template <class CAMERA>
 TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
-    const typename CAMERA::MeasurementVector& measured,
-    const TriangulationParameters& params) {
-
+                                    const typename CAMERA::MeasurementVector& measured,
+                                    const TriangulationParameters& params) {
   size_t m = cameras.size();
 
   // if we have a single pose the corresponding factor is uninformative
@@ -696,25 +681,22 @@ TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
   else
     // We triangulate the 3D position of the landmark
     try {
-      Point3 point =
-          triangulatePoint3<CAMERA>(cameras, measured, params.rankTolerance,
-                                    params.enableEPI, params.noiseModel);
+      Point3 point = triangulatePoint3<CAMERA>(
+          cameras, measured, params.rankTolerance, params.enableEPI, params.noiseModel);
 
       // Check landmark distance and re-projection errors to avoid outliers
       size_t i = 0;
       double maxReprojError = 0.0;
-      for(const CAMERA& camera: cameras) {
+      for (const CAMERA& camera : cameras) {
         const Pose3& pose = camera.pose();
-        if (params.landmarkDistanceThreshold > 0
-            && distance3(pose.translation(), point)
-                > params.landmarkDistanceThreshold)
+        if (params.landmarkDistanceThreshold > 0 &&
+            distance3(pose.translation(), point) > params.landmarkDistanceThreshold)
           return TriangulationResult::FarPoint();
 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
         // verify that the triangulated point lies in front of all cameras
         // Only needed if this was not yet handled by exception
         const Point3& p_local = pose.transformTo(point);
-        if (p_local.z() <= 0)
-          return TriangulationResult::BehindCamera();
+        if (p_local.z() <= 0) return TriangulationResult::BehindCamera();
 #endif
         // Check reprojection error
         if (params.dynamicOutlierRejectionThreshold > 0) {
@@ -725,19 +707,21 @@ TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
         i += 1;
       }
       // Flag as degenerate if average reprojection error is too large
-      if (params.dynamicOutlierRejectionThreshold > 0
-          && maxReprojError > params.dynamicOutlierRejectionThreshold)
+      if (params.dynamicOutlierRejectionThreshold > 0 &&
+          maxReprojError > params.dynamicOutlierRejectionThreshold)
         return TriangulationResult::Outlier();
 
       // all good!
       return TriangulationResult(point);
     } catch (TriangulationUnderconstrainedException&) {
       // This exception is thrown if
-      // 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before
-      // 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark)
+      // 1) There is a single pose for triangulation - this should not happen because we checked the
+      // number of poses before 2) The rank of the matrix used for triangulation is < 3:
+      // rotation-only, parallel cameras (or motion towards the landmark)
       return TriangulationResult::Degenerate();
     } catch (TriangulationCheiralityException&) {
-      // point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers
+      // point is behind one of the cameras: can be the case of close-to-parallel cameras or may
+      // depend on outliers
       return TriangulationResult::BehindCamera();
     }
 }
@@ -749,5 +733,4 @@ using CameraSetCal3DS2 = CameraSet<PinholeCamera<Cal3DS2>>;
 using CameraSetCal3Fisheye = CameraSet<PinholeCamera<Cal3Fisheye>>;
 using CameraSetCal3Unified = CameraSet<PinholeCamera<Cal3Unified>>;
 using CameraSetSpherical = CameraSet<SphericalCamera>;
-} // \namespace gtsam
-
+}  // namespace gtsam

From fcee29e62898d15bf64d93fafbb35f6735d997d1 Mon Sep 17 00:00:00 2001
From: Travis Driver <drivertp@gmail.com>
Date: Mon, 5 Jun 2023 19:11:41 -0700
Subject: [PATCH 06/15] Handle q_i = 0 (or NaN) for LOST

---
 gtsam/geometry/triangulation.cpp | 33 ++++++++++++++++++++++++++------
 1 file changed, 27 insertions(+), 6 deletions(-)

diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp
index 2049ce384..d7be29a9f 100644
--- a/gtsam/geometry/triangulation.cpp
+++ b/gtsam/geometry/triangulation.cpp
@@ -95,16 +95,37 @@ Point3 triangulateLOST(const std::vector<Pose3>& poses,
   for (size_t i = 0; i < m; i++) {
     const Pose3& wTi = poses[i];
     // TODO(akshay-krishnan): are there better ways to select j?
-    const int j = (i + 1) % m;
+    int j = (i + 1) % m;
     const Pose3& wTj = poses[j];
 
-    const Point3 d_ij = wTj.translation() - wTi.translation();
+    Point3 d_ij = wTj.translation() - wTi.translation();
+    Point3 wZi = wTi.rotation().rotate(calibratedMeasurements[i]);
+    Point3 wZj = wTj.rotation().rotate(calibratedMeasurements[j]);
+    double num_i = wZi.cross(wZj).norm();
+    double den_i = d_ij.cross(wZj).norm();
 
-    const Point3 wZi = wTi.rotation().rotate(calibratedMeasurements[i]);
-    const Point3 wZj = wTj.rotation().rotate(calibratedMeasurements[j]);
+    // Handle q_i = 0 (or NaN), which arises if the measurement vectors, wZi and wZj, coincide
+    // (or the baseline vector coincides with the jth measurement vector).
+    if (num_i == 0 || den_i == 0) {
+      bool success = false;
+      for (size_t k = 2; k < m; k++) {
+        j = (i + k) % m;
+        const Pose3& wTj = poses[j];
+
+        d_ij = wTj.translation() - wTi.translation();
+        wZj = wTj.rotation().rotate(calibratedMeasurements[j]);
+        num_i = wZi.cross(wZj).norm();
+        den_i = d_ij.cross(wZj).norm();
+        if (num_i > 0 && den_i > 0) {
+          success = true;
+          break;
+        }
+      }
+      if (!success) throw(TriangulationUnderconstrainedException());
+    }
 
     // Note: Setting q_i = 1.0 gives same results as DLT.
-    const double q_i = wZi.cross(wZj).norm() / (measurementNoise->sigma() * d_ij.cross(wZj).norm());
+    const double q_i = num_i / (measurementNoise->sigma() * den_i);
 
     const Matrix23 coefficientMat = q_i *
                                     skewSymmetric(calibratedMeasurements[i]).topLeftCorner(2, 3) *
@@ -117,7 +138,7 @@ Point3 triangulateLOST(const std::vector<Pose3>& poses,
   Eigen::ColPivHouseholderQR<Matrix> A_Qr = A.colPivHouseholderQr();
   A_Qr.setThreshold(rank_tol);
 
-  // if (A_Qr.rank() < 3) throw(TriangulationUnderconstrainedException());
+  if (A_Qr.rank() < 3) throw(TriangulationUnderconstrainedException());
 
   return A_Qr.solve(b);
 }

From f6f91ce231eef1855085444031470856bd42e1af Mon Sep 17 00:00:00 2001
From: Travis Driver <drivertp@gmail.com>
Date: Tue, 6 Jun 2023 09:38:19 -0700
Subject: [PATCH 07/15] Revert formatting for triangulation.cpp

---
 gtsam/geometry/triangulation.cpp | 48 ++++++++++++++++++--------------
 1 file changed, 27 insertions(+), 21 deletions(-)

diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp
index d7be29a9f..e714bb432 100644
--- a/gtsam/geometry/triangulation.cpp
+++ b/gtsam/geometry/triangulation.cpp
@@ -25,9 +25,9 @@
 namespace gtsam {
 
 Vector4 triangulateHomogeneousDLT(
-    const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
-    const Point2Vector& measurements,
-    double rank_tol) {
+    const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& 
+    projection_matrices,
+    const Point2Vector& measurements, double rank_tol) {
   // number of cameras
   size_t m = projection_matrices.size();
 
@@ -56,9 +56,9 @@ Vector4 triangulateHomogeneousDLT(
 }
 
 Vector4 triangulateHomogeneousDLT(
-    const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
-    const std::vector<Unit3>& measurements,
-    double rank_tol) {
+    const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& 
+    projection_matrices,
+    const std::vector<Unit3>& measurements, double rank_tol) {
   // number of cameras
   size_t m = projection_matrices.size();
 
@@ -68,7 +68,9 @@ Vector4 triangulateHomogeneousDLT(
   for (size_t i = 0; i < m; i++) {
     size_t row = i * 2;
     const Matrix34& projection = projection_matrices.at(i);
-    const Point3& p = measurements.at(i).point3();  // to get access to x,y,z of the bearing vector
+    const Point3& p = 
+        measurements.at(i)
+            .point3();  // to get access to x,y,z of the bearing vector
 
     // build system of equations
     A.row(row) = p.x() * projection.row(2) - p.z() * projection.row(0);
@@ -104,8 +106,9 @@ Point3 triangulateLOST(const std::vector<Pose3>& poses,
     double num_i = wZi.cross(wZj).norm();
     double den_i = d_ij.cross(wZj).norm();
 
-    // Handle q_i = 0 (or NaN), which arises if the measurement vectors, wZi and wZj, coincide
-    // (or the baseline vector coincides with the jth measurement vector).
+    // Handle q_i = 0 (or NaN), which arises if the measurement vectors, wZi and 
+    // wZj, coincide (or the baseline vector coincides with the jth measurement 
+    // vector).
     if (num_i == 0 || den_i == 0) {
       bool success = false;
       for (size_t k = 2; k < m; k++) {
@@ -127,9 +130,9 @@ Point3 triangulateLOST(const std::vector<Pose3>& poses,
     // Note: Setting q_i = 1.0 gives same results as DLT.
     const double q_i = num_i / (measurementNoise->sigma() * den_i);
 
-    const Matrix23 coefficientMat = q_i *
-                                    skewSymmetric(calibratedMeasurements[i]).topLeftCorner(2, 3) *
-                                    wTi.rotation().matrix().transpose();
+    const Matrix23 coefficientMat = 
+        q_i * skewSymmetric(calibratedMeasurements[i]).topLeftCorner(2, 3) *
+        wTi.rotation().matrix().transpose();
 
     A.block<2, 3>(2 * i, 0) << coefficientMat;
     b.block<2, 1>(2 * i, 0) << coefficientMat * wTi.translation();
@@ -144,20 +147,22 @@ Point3 triangulateLOST(const std::vector<Pose3>& poses,
 }
 
 Point3 triangulateDLT(
-    const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
-    const Point2Vector& measurements,
-    double rank_tol) {
-  Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol);
+    const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& 
+    projection_matrices,
+    const Point2Vector& measurements, double rank_tol) {
+  Vector4 v = 
+      triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol);
   // Create 3D point from homogeneous coordinates
   return Point3(v.head<3>() / v[3]);
 }
 
 Point3 triangulateDLT(
-    const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
-    const std::vector<Unit3>& measurements,
-    double rank_tol) {
+    const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& 
+    projection_matrices,
+    const std::vector<Unit3>& measurements, double rank_tol) {
   // contrary to previous triangulateDLT, this is now taking Unit3 inputs
-  Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol);
+  Vector4 v = 
+      triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol);
   // Create 3D point from homogeneous coordinates
   return Point3(v.head<3>() / v[3]);
 }
@@ -169,7 +174,8 @@ Point3 triangulateDLT(
  * @param landmarkKey to refer to landmark
  * @return refined Point3
  */
-Point3 optimize(const NonlinearFactorGraph& graph, const Values& values, Key landmarkKey) {
+Point3 optimize(const NonlinearFactorGraph& graph, const Values& values, 
+                Key landmarkKey) {
   // Maybe we should consider Gauss-Newton?
   LevenbergMarquardtParams params;
   params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;

From 8d134fd120525562dead2f9e9cbe3e93bc7e4395 Mon Sep 17 00:00:00 2001
From: Travis Driver <drivertp@gmail.com>
Date: Tue, 6 Jun 2023 09:48:05 -0700
Subject: [PATCH 08/15] Revert formatting for triangulation.h

---
 gtsam/geometry/triangulation.h | 303 ++++++++++++++++++---------------
 1 file changed, 161 insertions(+), 142 deletions(-)

diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h
index 61ca5588a..3a8398804 100644
--- a/gtsam/geometry/triangulation.h
+++ b/gtsam/geometry/triangulation.h
@@ -20,38 +20,39 @@
 
 #pragma once
 
+#include "gtsam/geometry/Point3.h"
 #include <gtsam/geometry/Cal3Bundler.h>
-#include <gtsam/geometry/Cal3DS2.h>
 #include <gtsam/geometry/Cal3Fisheye.h>
 #include <gtsam/geometry/Cal3Unified.h>
 #include <gtsam/geometry/Cal3_S2.h>
+#include <gtsam/geometry/Cal3DS2.h>
 #include <gtsam/geometry/CameraSet.h>
 #include <gtsam/geometry/PinholeCamera.h>
-#include <gtsam/geometry/Pose2.h>
 #include <gtsam/geometry/SphericalCamera.h>
+#include <gtsam/geometry/Pose2.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/slam/TriangulationFactor.h>
-#include "gtsam/geometry/Point3.h"
 
 #include <optional>
 
 namespace gtsam {
 
 /// Exception thrown by triangulateDLT when SVD returns rank < 3
-class GTSAM_EXPORT TriangulationUnderconstrainedException : public std::runtime_error {
- public:
-  TriangulationUnderconstrainedException()
-      : std::runtime_error("Triangulation Underconstrained Exception.") {}
+class GTSAM_EXPORT TriangulationUnderconstrainedException: public std::runtime_error {
+public:
+  TriangulationUnderconstrainedException() :
+      std::runtime_error("Triangulation Underconstrained Exception.") {
+  }
 };
 
 /// Exception thrown by triangulateDLT when landmark is behind one or more of the cameras
-class GTSAM_EXPORT TriangulationCheiralityException : public std::runtime_error {
- public:
-  TriangulationCheiralityException()
-      : std::runtime_error(
-            "Triangulation Cheirality Exception: The resulting landmark is behind one or more "
-            "cameras.") {}
+class GTSAM_EXPORT TriangulationCheiralityException: public std::runtime_error {
+public:
+  TriangulationCheiralityException() :
+      std::runtime_error(
+          "Triangulation Cheirality Exception: The resulting landmark is behind one or more cameras.") {
+  }
 };
 
 /**
@@ -63,13 +64,11 @@ class GTSAM_EXPORT TriangulationCheiralityException : public std::runtime_error
  */
 GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(
     const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
-    const Point2Vector& measurements,
-    double rank_tol = 1e-9);
+    const Point2Vector& measurements, double rank_tol = 1e-9);
 
 /**
  * Same math as Hartley and Zisserman, 2nd Ed., page 312, but with unit-norm bearing vectors
- * (contrarily to pinhole projection, the z entry is not assumed to be 1 as in Hartley and
- * Zisserman)
+ * (contrarily to pinhole projection, the z entry is not assumed to be 1 as in Hartley and Zisserman)
  * @param projection_matrices Projection matrices (K*P^-1)
  * @param measurements Unit3 bearing measurements
  * @param rank_tol SVD rank tolerance
@@ -77,8 +76,7 @@ GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(
  */
 GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(
     const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
-    const std::vector<Unit3>& measurements,
-    double rank_tol = 1e-9);
+    const std::vector<Unit3>& measurements, double rank_tol = 1e-9);
 
 /**
  * DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312
@@ -87,18 +85,18 @@ GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(
  * @param rank_tol SVD rank tolerance
  * @return Triangulated Point3
  */
-GTSAM_EXPORT Point3
-triangulateDLT(const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
-               const Point2Vector& measurements,
-               double rank_tol = 1e-9);
+GTSAM_EXPORT Point3 triangulateDLT(
+    const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
+    const Point2Vector& measurements,
+    double rank_tol = 1e-9);
 
 /**
  * overload of previous function to work with Unit3 (projected to canonical camera)
  */
-GTSAM_EXPORT Point3
-triangulateDLT(const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
-               const std::vector<Unit3>& measurements,
-               double rank_tol = 1e-9);
+GTSAM_EXPORT Point3 triangulateDLT(
+    const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
+    const std::vector<Unit3>& measurements,
+    double rank_tol = 1e-9);
 
 /**
  * @brief Triangulation using the LOST (Linear Optimal Sine Triangulation)
@@ -124,22 +122,20 @@ GTSAM_EXPORT Point3 triangulateLOST(const std::vector<Pose3>& poses,
  * @param initialEstimate
  * @return graph and initial values
  */
-template <class CALIBRATION>
+template<class CALIBRATION>
 std::pair<NonlinearFactorGraph, Values> triangulationGraph(
-    const std::vector<Pose3>& poses,
-    std::shared_ptr<CALIBRATION> sharedCal,
-    const Point2Vector& measurements,
-    Key landmarkKey,
+    const std::vector<Pose3>& poses, std::shared_ptr<CALIBRATION> sharedCal,
+    const Point2Vector& measurements, Key landmarkKey,
     const Point3& initialEstimate,
     const SharedNoiseModel& model = noiseModel::Unit::Create(2)) {
   Values values;
-  values.insert(landmarkKey, initialEstimate);  // Initial landmark value
+  values.insert(landmarkKey, initialEstimate); // Initial landmark value
   NonlinearFactorGraph graph;
   for (size_t i = 0; i < measurements.size(); i++) {
     const Pose3& pose_i = poses[i];
     typedef PinholePose<CALIBRATION> Camera;
     Camera camera_i(pose_i, sharedCal);
-    graph.emplace_shared<TriangulationFactor<Camera>>  //
+    graph.emplace_shared<TriangulationFactor<Camera> > //
         (camera_i, measurements[i], model, landmarkKey);
   }
   return {graph, values};
@@ -154,22 +150,21 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
  * @param initialEstimate
  * @return graph and initial values
  */
-template <class CAMERA>
+template<class CAMERA>
 std::pair<NonlinearFactorGraph, Values> triangulationGraph(
     const CameraSet<CAMERA>& cameras,
-    const typename CAMERA::MeasurementVector& measurements,
-    Key landmarkKey,
+    const typename CAMERA::MeasurementVector& measurements, Key landmarkKey,
     const Point3& initialEstimate,
     const SharedNoiseModel& model = nullptr) {
   Values values;
-  values.insert(landmarkKey, initialEstimate);  // Initial landmark value
+  values.insert(landmarkKey, initialEstimate); // Initial landmark value
   NonlinearFactorGraph graph;
-  static SharedNoiseModel unit(
-      noiseModel::Unit::Create(traits<typename CAMERA::Measurement>::dimension));
+  static SharedNoiseModel unit(noiseModel::Unit::Create(
+      traits<typename CAMERA::Measurement>::dimension));
   for (size_t i = 0; i < measurements.size(); i++) {
     const CAMERA& camera_i = cameras[i];
-    graph.emplace_shared<TriangulationFactor<CAMERA>>  //
-        (camera_i, measurements[i], model ? model : unit, landmarkKey);
+    graph.emplace_shared<TriangulationFactor<CAMERA> > //
+        (camera_i, measurements[i], model? model : unit, landmarkKey);
   }
   return {graph, values};
 }
@@ -182,8 +177,7 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
  * @return refined Point3
  */
 GTSAM_EXPORT Point3 optimize(const NonlinearFactorGraph& graph,
-                             const Values& values,
-                             Key landmarkKey);
+    const Values& values, Key landmarkKey);
 
 /**
  * Given an initial estimate , refine a point using measurements in several cameras
@@ -193,14 +187,14 @@ GTSAM_EXPORT Point3 optimize(const NonlinearFactorGraph& graph,
  * @param initialEstimate
  * @return refined Point3
  */
-template <class CALIBRATION>
+template<class CALIBRATION>
 Point3 triangulateNonlinear(const std::vector<Pose3>& poses,
-                            std::shared_ptr<CALIBRATION> sharedCal,
-                            const Point2Vector& measurements,
-                            const Point3& initialEstimate,
-                            const SharedNoiseModel& model = nullptr) {
+    std::shared_ptr<CALIBRATION> sharedCal,
+    const Point2Vector& measurements, const Point3& initialEstimate,
+    const SharedNoiseModel& model = nullptr) {
+
   // Create a factor graph and initial values
-  const auto [graph, values] = triangulationGraph<CALIBRATION>  //
+  const auto [graph, values] = triangulationGraph<CALIBRATION> //
       (poses, sharedCal, measurements, Symbol('p', 0), initialEstimate, model);
 
   return optimize(graph, values, Symbol('p', 0));
@@ -213,32 +207,33 @@ Point3 triangulateNonlinear(const std::vector<Pose3>& poses,
  * @param initialEstimate
  * @return refined Point3
  */
-template <class CAMERA>
-Point3 triangulateNonlinear(const CameraSet<CAMERA>& cameras,
-                            const typename CAMERA::MeasurementVector& measurements,
-                            const Point3& initialEstimate,
-                            const SharedNoiseModel& model = nullptr) {
+template<class CAMERA>
+Point3 triangulateNonlinear(
+    const CameraSet<CAMERA>& cameras,
+    const typename CAMERA::MeasurementVector& measurements, const Point3& initialEstimate,
+    const SharedNoiseModel& model = nullptr) {
+
   // Create a factor graph and initial values
-  const auto [graph, values] = triangulationGraph<CAMERA>  //
+  const auto [graph, values] = triangulationGraph<CAMERA> //
       (cameras, measurements, Symbol('p', 0), initialEstimate, model);
 
   return optimize(graph, values, Symbol('p', 0));
 }
 
-template <class CAMERA>
-std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projectionMatricesFromCameras(
-    const CameraSet<CAMERA>& cameras) {
+template<class CAMERA>
+std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>
+projectionMatricesFromCameras(const CameraSet<CAMERA> &cameras) {
   std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices;
-  for (const CAMERA& camera : cameras) {
+  for (const CAMERA &camera: cameras) {
     projection_matrices.push_back(camera.cameraProjectionMatrix());
   }
   return projection_matrices;
 }
 
 // overload, assuming pinholePose
-template <class CALIBRATION>
+template<class CALIBRATION>
 std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projectionMatricesFromPoses(
-    const std::vector<Pose3>& poses, std::shared_ptr<CALIBRATION> sharedCal) {
+        const std::vector<Pose3> &poses, std::shared_ptr<CALIBRATION> sharedCal) {
   std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices;
   for (size_t i = 0; i < poses.size(); i++) {
     PinholePose<CALIBRATION> camera(poses.at(i), sharedCal);
@@ -263,9 +258,9 @@ Cal3_S2 createPinholeCalibration(const CALIBRATION& cal) {
 /** Internal undistortMeasurement to be used by undistortMeasurement and
  * undistortMeasurements */
 template <class CALIBRATION, class MEASUREMENT>
-MEASUREMENT undistortMeasurementInternal(const CALIBRATION& cal,
-                                         const MEASUREMENT& measurement,
-                                         std::optional<Cal3_S2> pinholeCal = {}) {
+MEASUREMENT undistortMeasurementInternal(
+    const CALIBRATION& cal, const MEASUREMENT& measurement,
+    std::optional<Cal3_S2> pinholeCal = {}) {
   if (!pinholeCal) {
     pinholeCal = createPinholeCalibration(cal);
   }
@@ -284,13 +279,13 @@ MEASUREMENT undistortMeasurementInternal(const CALIBRATION& cal,
  * @return measurements with the effect of the distortion of sharedCal removed.
  */
 template <class CALIBRATION>
-Point2Vector undistortMeasurements(const CALIBRATION& cal, const Point2Vector& measurements) {
+Point2Vector undistortMeasurements(const CALIBRATION& cal,
+                                   const Point2Vector& measurements) {
   Cal3_S2 pinholeCalibration = createPinholeCalibration(cal);
   Point2Vector undistortedMeasurements;
   // Calibrate with cal and uncalibrate with pinhole version of cal so that
   // measurements are undistorted.
-  std::transform(measurements.begin(),
-                 measurements.end(),
+  std::transform(measurements.begin(), measurements.end(),
                  std::back_inserter(undistortedMeasurements),
                  [&cal, &pinholeCalibration](const Point2& measurement) {
                    return undistortMeasurementInternal<CALIBRATION>(
@@ -301,7 +296,8 @@ Point2Vector undistortMeasurements(const CALIBRATION& cal, const Point2Vector& m
 
 /** Specialization for Cal3_S2 as it doesn't need to be undistorted. */
 template <>
-inline Point2Vector undistortMeasurements(const Cal3_S2& cal, const Point2Vector& measurements) {
+inline Point2Vector undistortMeasurements(const Cal3_S2& cal,
+                                          const Point2Vector& measurements) {
   return measurements;
 }
 
@@ -318,15 +314,17 @@ inline Point2Vector undistortMeasurements(const Cal3_S2& cal, const Point2Vector
  */
 template <class CAMERA>
 typename CAMERA::MeasurementVector undistortMeasurements(
-    const CameraSet<CAMERA>& cameras, const typename CAMERA::MeasurementVector& measurements) {
+    const CameraSet<CAMERA>& cameras,
+    const typename CAMERA::MeasurementVector& measurements) {
   const size_t nrMeasurements = measurements.size();
   assert(nrMeasurements == cameras.size());
   typename CAMERA::MeasurementVector undistortedMeasurements(nrMeasurements);
   for (size_t ii = 0; ii < nrMeasurements; ++ii) {
     // Calibrate with cal and uncalibrate with pinhole version of cal so that
     // measurements are undistorted.
-    undistortedMeasurements[ii] = undistortMeasurementInternal<typename CAMERA::CalibrationType>(
-        cameras[ii].calibration(), measurements[ii]);
+    undistortedMeasurements[ii] =
+        undistortMeasurementInternal<typename CAMERA::CalibrationType>(
+            cameras[ii].calibration(), measurements[ii]);
   }
   return undistortedMeasurements;
 }
@@ -356,13 +354,12 @@ inline SphericalCamera::MeasurementVector undistortMeasurements(
  * @return homogeneous measurements in image plane
  */
 template <class CALIBRATION>
-inline Point3Vector calibrateMeasurementsShared(const CALIBRATION& cal,
-                                                const Point2Vector& measurements) {
+inline Point3Vector calibrateMeasurementsShared(
+    const CALIBRATION& cal, const Point2Vector& measurements) {
   Point3Vector calibratedMeasurements;
   // Calibrate with cal and uncalibrate with pinhole version of cal so that
   // measurements are undistorted.
-  std::transform(measurements.begin(),
-                 measurements.end(),
+  std::transform(measurements.begin(), measurements.end(),
                  std::back_inserter(calibratedMeasurements),
                  [&cal](const Point2& measurement) {
                    Point3 p;
@@ -381,21 +378,25 @@ inline Point3Vector calibrateMeasurementsShared(const CALIBRATION& cal,
  * @return homogeneous measurements in image plane
  */
 template <class CAMERA>
-inline Point3Vector calibrateMeasurements(const CameraSet<CAMERA>& cameras,
-                                          const typename CAMERA::MeasurementVector& measurements) {
+inline Point3Vector calibrateMeasurements(
+    const CameraSet<CAMERA>& cameras,
+    const typename CAMERA::MeasurementVector& measurements) {
   const size_t nrMeasurements = measurements.size();
   assert(nrMeasurements == cameras.size());
   Point3Vector calibratedMeasurements(nrMeasurements);
   for (size_t ii = 0; ii < nrMeasurements; ++ii) {
-    calibratedMeasurements[ii] << cameras[ii].calibration().calibrate(measurements[ii]), 1.0;
+    calibratedMeasurements[ii]
+        << cameras[ii].calibration().calibrate(measurements[ii]),
+        1.0;
   }
   return calibratedMeasurements;
 }
 
 /** Specialize for SphericalCamera to do nothing. */
 template <class CAMERA = SphericalCamera>
-inline Point3Vector calibrateMeasurements(const CameraSet<SphericalCamera>& cameras,
-                                          const SphericalCamera::MeasurementVector& measurements) {
+inline Point3Vector calibrateMeasurements(
+    const CameraSet<SphericalCamera>& cameras,
+    const SphericalCamera::MeasurementVector& measurements) {
   Point3Vector calibratedMeasurements(measurements.size());
   for (size_t ii = 0; ii < measurements.size(); ++ii) {
     calibratedMeasurements[ii] << measurements[ii].point3();
@@ -420,8 +421,7 @@ template <class CALIBRATION>
 Point3 triangulatePoint3(const std::vector<Pose3>& poses,
                          std::shared_ptr<CALIBRATION> sharedCal,
                          const Point2Vector& measurements,
-                         double rank_tol = 1e-9,
-                         bool optimize = false,
+                         double rank_tol = 1e-9, bool optimize = false,
                          const SharedNoiseModel& model = nullptr,
                          const bool useLOST = false) {
   assert(poses.size() == measurements.size());
@@ -433,21 +433,25 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
     // Reduce input noise model to an isotropic noise model using the mean of
     // the diagonal.
     const double measurementSigma = model ? model->sigmas().mean() : 1e-4;
-    SharedIsotropic measurementNoise = noiseModel::Isotropic::Sigma(2, measurementSigma);
+    SharedIsotropic measurementNoise =
+        noiseModel::Isotropic::Sigma(2, measurementSigma);
     // calibrate the measurements to obtain homogenous coordinates in image
     // plane.
     auto calibratedMeasurements =
         calibrateMeasurementsShared<CALIBRATION>(*sharedCal, measurements);
 
-    point = triangulateLOST(poses, calibratedMeasurements, measurementNoise, rank_tol);
+    point = triangulateLOST(poses, calibratedMeasurements, measurementNoise, 
+                            rank_tol);
   } else {
     // construct projection matrices from poses & calibration
     auto projection_matrices = projectionMatricesFromPoses(poses, sharedCal);
 
     // Undistort the measurements, leaving only the pinhole elements in effect.
-    auto undistortedMeasurements = undistortMeasurements<CALIBRATION>(*sharedCal, measurements);
+    auto undistortedMeasurements =
+        undistortMeasurements<CALIBRATION>(*sharedCal, measurements);
 
-    point = triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
+    point =
+        triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
   }
 
   // Then refine using non-linear optimization
@@ -483,8 +487,7 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
 template <class CAMERA>
 Point3 triangulatePoint3(const CameraSet<CAMERA>& cameras,
                          const typename CAMERA::MeasurementVector& measurements,
-                         double rank_tol = 1e-9,
-                         bool optimize = false,
+                         double rank_tol = 1e-9, bool optimize = false,
                          const SharedNoiseModel& model = nullptr,
                          const bool useLOST = false) {
   size_t m = cameras.size();
@@ -498,7 +501,8 @@ Point3 triangulatePoint3(const CameraSet<CAMERA>& cameras,
     // Reduce input noise model to an isotropic noise model using the mean of
     // the diagonal.
     const double measurementSigma = model ? model->sigmas().mean() : 1e-4;
-    SharedIsotropic measurementNoise = noiseModel::Isotropic::Sigma(2, measurementSigma);
+    SharedIsotropic measurementNoise =
+        noiseModel::Isotropic::Sigma(2, measurementSigma);
 
     // construct poses from cameras.
     std::vector<Pose3> poses;
@@ -507,17 +511,21 @@ Point3 triangulatePoint3(const CameraSet<CAMERA>& cameras,
 
     // calibrate the measurements to obtain homogenous coordinates in image
     // plane.
-    auto calibratedMeasurements = calibrateMeasurements<CAMERA>(cameras, measurements);
+    auto calibratedMeasurements =
+        calibrateMeasurements<CAMERA>(cameras, measurements);
 
-    point = triangulateLOST(poses, calibratedMeasurements, measurementNoise, rank_tol);
+    point = triangulateLOST(poses, calibratedMeasurements, measurementNoise, 
+                            rank_tol);
   } else {
     // construct projection matrices from poses & calibration
     auto projection_matrices = projectionMatricesFromCameras(cameras);
 
     // Undistort the measurements, leaving only the pinhole elements in effect.
-    auto undistortedMeasurements = undistortMeasurements<CAMERA>(cameras, measurements);
+    auto undistortedMeasurements =
+        undistortMeasurements<CAMERA>(cameras, measurements);
 
-    point = triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
+    point =
+        triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
   }
 
   // Then refine using non-linear optimization
@@ -540,8 +548,7 @@ Point3 triangulatePoint3(const CameraSet<CAMERA>& cameras,
 template <class CALIBRATION>
 Point3 triangulatePoint3(const CameraSet<PinholeCamera<CALIBRATION>>& cameras,
                          const Point2Vector& measurements,
-                         double rank_tol = 1e-9,
-                         bool optimize = false,
+                         double rank_tol = 1e-9, bool optimize = false,
                          const SharedNoiseModel& model = nullptr,
                          const bool useLOST = false) {
   return triangulatePoint3<PinholeCamera<CALIBRATION>>  //
@@ -549,16 +556,16 @@ Point3 triangulatePoint3(const CameraSet<PinholeCamera<CALIBRATION>>& cameras,
 }
 
 struct GTSAM_EXPORT TriangulationParameters {
-  double rankTolerance;  ///< threshold to decide whether triangulation is result.degenerate
-  ///< (the rank is the number of singular values of the triangulation matrix which are larger than
-  ///< rankTolerance)
-  bool enableEPI;  ///< if set to true, will refine triangulation using LM
+
+  double rankTolerance; ///< threshold to decide whether triangulation is result.degenerate
+  ///< (the rank is the number of singular values of the triangulation matrix which are larger than rankTolerance)
+  bool enableEPI; ///< if set to true, will refine triangulation using LM
 
   /**
    * if the landmark is triangulated at distance larger than this,
    * result is flagged as degenerate.
    */
-  double landmarkDistanceThreshold;  //
+  double landmarkDistanceThreshold; //
 
   /**
    * If this is nonnegative the we will check if the average reprojection error
@@ -567,7 +574,7 @@ struct GTSAM_EXPORT TriangulationParameters {
    */
   double dynamicOutlierRejectionThreshold;
 
-  SharedNoiseModel noiseModel;  ///< used in the nonlinear triangulation
+  SharedNoiseModel noiseModel; ///< used in the nonlinear triangulation
 
   /**
    * Constructor
@@ -579,36 +586,39 @@ struct GTSAM_EXPORT TriangulationParameters {
    *
    */
   TriangulationParameters(const double _rankTolerance = 1.0,
-                          const bool _enableEPI = false,
-                          double _landmarkDistanceThreshold = -1,
-                          double _dynamicOutlierRejectionThreshold = -1,
-                          const SharedNoiseModel& _noiseModel = nullptr)
-      : rankTolerance(_rankTolerance),
-        enableEPI(_enableEPI),                                  //
-        landmarkDistanceThreshold(_landmarkDistanceThreshold),  //
-        dynamicOutlierRejectionThreshold(_dynamicOutlierRejectionThreshold),
-        noiseModel(_noiseModel) {}
+      const bool _enableEPI = false, double _landmarkDistanceThreshold = -1,
+      double _dynamicOutlierRejectionThreshold = -1,
+      const SharedNoiseModel& _noiseModel = nullptr) :
+      rankTolerance(_rankTolerance), enableEPI(_enableEPI), //
+      landmarkDistanceThreshold(_landmarkDistanceThreshold), //
+      dynamicOutlierRejectionThreshold(_dynamicOutlierRejectionThreshold),
+      noiseModel(_noiseModel){
+  }
 
   // stream to output
-  friend std::ostream& operator<<(std::ostream& os, const TriangulationParameters& p) {
+  friend std::ostream &operator<<(std::ostream &os,
+      const TriangulationParameters& p) {
     os << "rankTolerance = " << p.rankTolerance << std::endl;
     os << "enableEPI = " << p.enableEPI << std::endl;
-    os << "landmarkDistanceThreshold = " << p.landmarkDistanceThreshold << std::endl;
-    os << "dynamicOutlierRejectionThreshold = " << p.dynamicOutlierRejectionThreshold << std::endl;
+    os << "landmarkDistanceThreshold = " << p.landmarkDistanceThreshold
+        << std::endl;
+    os << "dynamicOutlierRejectionThreshold = "
+        << p.dynamicOutlierRejectionThreshold << std::endl;
     os << "noise model" << std::endl;
     return os;
   }
 
- private:
+private:
+
 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION  ///
   /// Serialization function
   friend class boost::serialization::access;
-  template <class ARCHIVE>
-  void serialize(ARCHIVE& ar, const unsigned int version) {
-    ar& BOOST_SERIALIZATION_NVP(rankTolerance);
-    ar& BOOST_SERIALIZATION_NVP(enableEPI);
-    ar& BOOST_SERIALIZATION_NVP(landmarkDistanceThreshold);
-    ar& BOOST_SERIALIZATION_NVP(dynamicOutlierRejectionThreshold);
+  template<class ARCHIVE>
+  void serialize(ARCHIVE & ar, const unsigned int version) {
+    ar & BOOST_SERIALIZATION_NVP(rankTolerance);
+    ar & BOOST_SERIALIZATION_NVP(enableEPI);
+    ar & BOOST_SERIALIZATION_NVP(landmarkDistanceThreshold);
+    ar & BOOST_SERIALIZATION_NVP(dynamicOutlierRejectionThreshold);
   }
 #endif
 };
@@ -635,10 +645,16 @@ class TriangulationResult : public std::optional<Point3> {
    * Constructor
    */
   TriangulationResult(const Point3& p) : status(VALID) { emplace(p); }
-  static TriangulationResult Degenerate() { return TriangulationResult(DEGENERATE); }
+  static TriangulationResult Degenerate() {
+    return TriangulationResult(DEGENERATE);
+  }
   static TriangulationResult Outlier() { return TriangulationResult(OUTLIER); }
-  static TriangulationResult FarPoint() { return TriangulationResult(FAR_POINT); }
-  static TriangulationResult BehindCamera() { return TriangulationResult(BEHIND_CAMERA); }
+  static TriangulationResult FarPoint() {
+    return TriangulationResult(FAR_POINT);
+  }
+  static TriangulationResult BehindCamera() {
+    return TriangulationResult(BEHIND_CAMERA);
+  }
   bool valid() const { return status == VALID; }
   bool degenerate() const { return status == DEGENERATE; }
   bool outlier() const { return status == OUTLIER; }
@@ -649,7 +665,8 @@ class TriangulationResult : public std::optional<Point3> {
     return value();
   }
   // stream to output
-  friend std::ostream& operator<<(std::ostream& os, const TriangulationResult& result) {
+  friend std::ostream& operator<<(std::ostream& os,
+                                  const TriangulationResult& result) {
     if (result)
       os << "point = " << *result << std::endl;
     else
@@ -669,10 +686,11 @@ class TriangulationResult : public std::optional<Point3> {
 };
 
 /// triangulateSafe: extensive checking of the outcome
-template <class CAMERA>
+template<class CAMERA>
 TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
-                                    const typename CAMERA::MeasurementVector& measured,
-                                    const TriangulationParameters& params) {
+    const typename CAMERA::MeasurementVector& measured,
+    const TriangulationParameters& params) {
+
   size_t m = cameras.size();
 
   // if we have a single pose the corresponding factor is uninformative
@@ -681,22 +699,25 @@ TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
   else
     // We triangulate the 3D position of the landmark
     try {
-      Point3 point = triangulatePoint3<CAMERA>(
-          cameras, measured, params.rankTolerance, params.enableEPI, params.noiseModel);
+      Point3 point =
+          triangulatePoint3<CAMERA>(cameras, measured, params.rankTolerance,
+                                    params.enableEPI, params.noiseModel);
 
       // Check landmark distance and re-projection errors to avoid outliers
       size_t i = 0;
       double maxReprojError = 0.0;
-      for (const CAMERA& camera : cameras) {
+      for(const CAMERA& camera: cameras) {
         const Pose3& pose = camera.pose();
-        if (params.landmarkDistanceThreshold > 0 &&
-            distance3(pose.translation(), point) > params.landmarkDistanceThreshold)
+        if (params.landmarkDistanceThreshold > 0
+            && distance3(pose.translation(), point)
+                > params.landmarkDistanceThreshold)
           return TriangulationResult::FarPoint();
 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
         // verify that the triangulated point lies in front of all cameras
         // Only needed if this was not yet handled by exception
         const Point3& p_local = pose.transformTo(point);
-        if (p_local.z() <= 0) return TriangulationResult::BehindCamera();
+        if (p_local.z() <= 0)
+          return TriangulationResult::BehindCamera();
 #endif
         // Check reprojection error
         if (params.dynamicOutlierRejectionThreshold > 0) {
@@ -707,21 +728,19 @@ TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
         i += 1;
       }
       // Flag as degenerate if average reprojection error is too large
-      if (params.dynamicOutlierRejectionThreshold > 0 &&
-          maxReprojError > params.dynamicOutlierRejectionThreshold)
+      if (params.dynamicOutlierRejectionThreshold > 0
+          && maxReprojError > params.dynamicOutlierRejectionThreshold)
         return TriangulationResult::Outlier();
 
       // all good!
       return TriangulationResult(point);
     } catch (TriangulationUnderconstrainedException&) {
       // This exception is thrown if
-      // 1) There is a single pose for triangulation - this should not happen because we checked the
-      // number of poses before 2) The rank of the matrix used for triangulation is < 3:
-      // rotation-only, parallel cameras (or motion towards the landmark)
+      // 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before
+      // 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark)
       return TriangulationResult::Degenerate();
     } catch (TriangulationCheiralityException&) {
-      // point is behind one of the cameras: can be the case of close-to-parallel cameras or may
-      // depend on outliers
+      // point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers
       return TriangulationResult::BehindCamera();
     }
 }
@@ -733,4 +752,4 @@ using CameraSetCal3DS2 = CameraSet<PinholeCamera<Cal3DS2>>;
 using CameraSetCal3Fisheye = CameraSet<PinholeCamera<Cal3Fisheye>>;
 using CameraSetCal3Unified = CameraSet<PinholeCamera<Cal3Unified>>;
 using CameraSetSpherical = CameraSet<SphericalCamera>;
-}  // namespace gtsam
+} // \namespace gtsam

From bba4b8731f68652398e4d1b85224e13e89517917 Mon Sep 17 00:00:00 2001
From: Travis Driver <drivertp@gmail.com>
Date: Tue, 6 Jun 2023 10:01:46 -0700
Subject: [PATCH 09/15] Final cleanup

---
 gtsam/geometry/triangulation.cpp | 20 ++++++++++----------
 1 file changed, 10 insertions(+), 10 deletions(-)

diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp
index e714bb432..c8af2ea72 100644
--- a/gtsam/geometry/triangulation.cpp
+++ b/gtsam/geometry/triangulation.cpp
@@ -25,8 +25,8 @@
 namespace gtsam {
 
 Vector4 triangulateHomogeneousDLT(
-    const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& 
-    projection_matrices,
+    const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>&
+        projection_matrices,
     const Point2Vector& measurements, double rank_tol) {
   // number of cameras
   size_t m = projection_matrices.size();
@@ -56,8 +56,8 @@ Vector4 triangulateHomogeneousDLT(
 }
 
 Vector4 triangulateHomogeneousDLT(
-    const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& 
-    projection_matrices,
+    const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>&
+        projection_matrices,
     const std::vector<Unit3>& measurements, double rank_tol) {
   // number of cameras
   size_t m = projection_matrices.size();
@@ -68,7 +68,7 @@ Vector4 triangulateHomogeneousDLT(
   for (size_t i = 0; i < m; i++) {
     size_t row = i * 2;
     const Matrix34& projection = projection_matrices.at(i);
-    const Point3& p = 
+    const Point3& p =
         measurements.at(i)
             .point3();  // to get access to x,y,z of the bearing vector
 
@@ -130,7 +130,7 @@ Point3 triangulateLOST(const std::vector<Pose3>& poses,
     // Note: Setting q_i = 1.0 gives same results as DLT.
     const double q_i = num_i / (measurementNoise->sigma() * den_i);
 
-    const Matrix23 coefficientMat = 
+    const Matrix23 coefficientMat =
         q_i * skewSymmetric(calibratedMeasurements[i]).topLeftCorner(2, 3) *
         wTi.rotation().matrix().transpose();
 
@@ -147,8 +147,8 @@ Point3 triangulateLOST(const std::vector<Pose3>& poses,
 }
 
 Point3 triangulateDLT(
-    const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& 
-    projection_matrices,
+    const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>&
+        projection_matrices,
     const Point2Vector& measurements, double rank_tol) {
   Vector4 v = 
       triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol);
@@ -161,7 +161,7 @@ Point3 triangulateDLT(
     projection_matrices,
     const std::vector<Unit3>& measurements, double rank_tol) {
   // contrary to previous triangulateDLT, this is now taking Unit3 inputs
-  Vector4 v = 
+  Vector4 v =
       triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol);
   // Create 3D point from homogeneous coordinates
   return Point3(v.head<3>() / v[3]);
@@ -174,7 +174,7 @@ Point3 triangulateDLT(
  * @param landmarkKey to refer to landmark
  * @return refined Point3
  */
-Point3 optimize(const NonlinearFactorGraph& graph, const Values& values, 
+Point3 optimize(const NonlinearFactorGraph& graph, const Values& values,
                 Key landmarkKey) {
   // Maybe we should consider Gauss-Newton?
   LevenbergMarquardtParams params;

From 1e6f1b757cdcc02a810d3e425009017fcf62bbcc Mon Sep 17 00:00:00 2001
From: Travis Driver <drivertp@gmail.com>
Date: Tue, 6 Jun 2023 10:11:49 -0700
Subject: [PATCH 10/15] Final final cleanup

---
 gtsam/geometry/triangulation.cpp | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp
index c8af2ea72..039dfd74d 100644
--- a/gtsam/geometry/triangulation.cpp
+++ b/gtsam/geometry/triangulation.cpp
@@ -150,15 +150,15 @@ Point3 triangulateDLT(
     const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>&
         projection_matrices,
     const Point2Vector& measurements, double rank_tol) {
-  Vector4 v = 
+  Vector4 v =
       triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol);
   // Create 3D point from homogeneous coordinates
   return Point3(v.head<3>() / v[3]);
 }
 
 Point3 triangulateDLT(
-    const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& 
-    projection_matrices,
+    const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>&
+        projection_matrices,
     const std::vector<Unit3>& measurements, double rank_tol) {
   // contrary to previous triangulateDLT, this is now taking Unit3 inputs
   Vector4 v =

From 4203c4d355f82829a881a43dae721b89ad2dcd7f Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 6 Jun 2023 00:51:55 -0400
Subject: [PATCH 11/15] enable windows release build in the CI

---
 .github/workflows/build-windows.yml | 12 ++++++++----
 1 file changed, 8 insertions(+), 4 deletions(-)

diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml
index 3d4bf3faf..17e1e0376 100644
--- a/.github/workflows/build-windows.yml
+++ b/.github/workflows/build-windows.yml
@@ -28,8 +28,7 @@ jobs:
 
         build_type: [
             Debug,
-            #TODO(Varun) The release build takes over 2.5 hours, need to figure out why.
-            # Release
+            Release
           ]
         build_unstable: [ON]
         include:
@@ -93,13 +92,19 @@ jobs:
           # Set the BOOST_ROOT variable
           echo "BOOST_ROOT=$BOOST_PATH" >> $env:GITHUB_ENV
 
+      - name: Install Eigen
+        shell: powershell
+        run: |
+          choco install eigen
+          Get-ChildItem -Force "$env:ChocolateyInstall\lib\eigen\include"
+
       - name: Checkout
         uses: actions/checkout@v3
 
       - name: Configuration
         run: |
           cmake -E remove_directory build
-          cmake -B build -S . -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DBOOST_ROOT="${env:BOOST_ROOT}" -DBOOST_INCLUDEDIR="${env:BOOST_ROOT}\boost\include" -DBOOST_LIBRARYDIR="${env:BOOST_ROOT}\lib"
+          cmake -B build -S . -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DBOOST_ROOT="${env:BOOST_ROOT}" -DBOOST_INCLUDEDIR="${env:BOOST_ROOT}\boost\include" -DBOOST_LIBRARYDIR="${env:BOOST_ROOT}\lib" -DGTSAM_USE_SYSTEM_EIGEN=ON -DEIGEN_INCLUDE_DIR="$env:ChocolateyInstall\lib\eigen\include"
 
       - name: Build
         run: |
@@ -125,4 +130,3 @@ jobs:
 
           # Run GTSAM_UNSTABLE tests
           #cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.base_unstable
-          

From a5b90df471bee77e2ed73fe6f8a169d45a80aea4 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 6 Jun 2023 17:48:58 -0400
Subject: [PATCH 12/15] configure system Eigen

---
 .github/workflows/build-windows.yml | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml
index 17e1e0376..a24e14c74 100644
--- a/.github/workflows/build-windows.yml
+++ b/.github/workflows/build-windows.yml
@@ -96,7 +96,9 @@ jobs:
         shell: powershell
         run: |
           choco install eigen
-          Get-ChildItem -Force "$env:ChocolateyInstall\lib\eigen\include"
+          echo "EIGEN3_INCLUDE_DIR=$env:ChocolateyInstall\lib\eigen\include" >> $env:GITHUB_ENV
+          echo "CMAKE_PREFIX_PATH=$env:CMAKE_PREFIX_PATH;$env:ChocolateyInstall\lib\eigen\cmake\share" >> $env:GITHUB_ENV
+          Get-ChildItem -Force "$env:ChocolateyInstall\lib\eigen\cmake\share"
 
       - name: Checkout
         uses: actions/checkout@v3
@@ -104,7 +106,7 @@ jobs:
       - name: Configuration
         run: |
           cmake -E remove_directory build
-          cmake -B build -S . -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DBOOST_ROOT="${env:BOOST_ROOT}" -DBOOST_INCLUDEDIR="${env:BOOST_ROOT}\boost\include" -DBOOST_LIBRARYDIR="${env:BOOST_ROOT}\lib" -DGTSAM_USE_SYSTEM_EIGEN=ON -DEIGEN_INCLUDE_DIR="$env:ChocolateyInstall\lib\eigen\include"
+          cmake -B build -S . -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DBOOST_ROOT="${env:BOOST_ROOT}" -DBOOST_INCLUDEDIR="${env:BOOST_ROOT}\boost\include" -DBOOST_LIBRARYDIR="${env:BOOST_ROOT}\lib" -DGTSAM_USE_SYSTEM_EIGEN=ON -DEIGEN3_INCLUDE_DIR="$env:ChocolateyInstall\lib\eigen\include"
 
       - name: Build
         run: |

From cdbdc67ee49d54307e22e66f03db9a3d11b66d9f Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 6 Jun 2023 18:32:16 -0400
Subject: [PATCH 13/15] fix eigen cmake search path

---
 .github/workflows/build-windows.yml | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml
index a24e14c74..d4f1d3ba0 100644
--- a/.github/workflows/build-windows.yml
+++ b/.github/workflows/build-windows.yml
@@ -97,8 +97,8 @@ jobs:
         run: |
           choco install eigen
           echo "EIGEN3_INCLUDE_DIR=$env:ChocolateyInstall\lib\eigen\include" >> $env:GITHUB_ENV
-          echo "CMAKE_PREFIX_PATH=$env:CMAKE_PREFIX_PATH;$env:ChocolateyInstall\lib\eigen\cmake\share" >> $env:GITHUB_ENV
-          Get-ChildItem -Force "$env:ChocolateyInstall\lib\eigen\cmake\share"
+          echo "CMAKE_PREFIX_PATH=$env:CMAKE_PREFIX_PATH;$env:ChocolateyInstall\lib\eigen\share\cmake" >> $env:GITHUB_ENV
+          Get-ChildItem -Force "$env:ChocolateyInstall\lib\eigen\share\cmake"
 
       - name: Checkout
         uses: actions/checkout@v3

From b947a7201c5f9be578374d47f833bd5e023e6028 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 7 Jun 2023 12:44:10 -0400
Subject: [PATCH 14/15] remove need for Eigen install since latest packaged
 version should work

---
 .github/workflows/build-windows.yml | 10 +---------
 1 file changed, 1 insertion(+), 9 deletions(-)

diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml
index d4f1d3ba0..849fe8f7b 100644
--- a/.github/workflows/build-windows.yml
+++ b/.github/workflows/build-windows.yml
@@ -92,21 +92,13 @@ jobs:
           # Set the BOOST_ROOT variable
           echo "BOOST_ROOT=$BOOST_PATH" >> $env:GITHUB_ENV
 
-      - name: Install Eigen
-        shell: powershell
-        run: |
-          choco install eigen
-          echo "EIGEN3_INCLUDE_DIR=$env:ChocolateyInstall\lib\eigen\include" >> $env:GITHUB_ENV
-          echo "CMAKE_PREFIX_PATH=$env:CMAKE_PREFIX_PATH;$env:ChocolateyInstall\lib\eigen\share\cmake" >> $env:GITHUB_ENV
-          Get-ChildItem -Force "$env:ChocolateyInstall\lib\eigen\share\cmake"
-
       - name: Checkout
         uses: actions/checkout@v3
 
       - name: Configuration
         run: |
           cmake -E remove_directory build
-          cmake -B build -S . -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DBOOST_ROOT="${env:BOOST_ROOT}" -DBOOST_INCLUDEDIR="${env:BOOST_ROOT}\boost\include" -DBOOST_LIBRARYDIR="${env:BOOST_ROOT}\lib" -DGTSAM_USE_SYSTEM_EIGEN=ON -DEIGEN3_INCLUDE_DIR="$env:ChocolateyInstall\lib\eigen\include"
+          cmake -B build -S . -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DBOOST_ROOT="${env:BOOST_ROOT}" -DBOOST_INCLUDEDIR="${env:BOOST_ROOT}\boost\include" -DBOOST_LIBRARYDIR="${env:BOOST_ROOT}\lib"
 
       - name: Build
         run: |

From e56186c45ab2b28cab85fb671f7e22535482bc08 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 7 Jun 2023 12:52:06 -0400
Subject: [PATCH 15/15] remove deprecated Windows image

---
 .github/workflows/build-windows.yml | 8 --------
 1 file changed, 8 deletions(-)

diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml
index 849fe8f7b..0434577c1 100644
--- a/.github/workflows/build-windows.yml
+++ b/.github/workflows/build-windows.yml
@@ -21,8 +21,6 @@ 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: [
-            #TODO This build fails, need to understand why.
-            # windows-2016-cl,
             windows-2019-cl,
           ]
 
@@ -32,12 +30,6 @@ jobs:
           ]
         build_unstable: [ON]
         include:
-          #TODO This build fails, need to understand why.
-          # - name: windows-2016-cl
-          #   os: windows-2016
-          #   compiler: cl
-          #   platform: 32
-
           - name: windows-2019-cl
             os: windows-2019
             compiler: cl