From 43addb5ee91cdcfdd8d8cc97488c25a0311abf3d Mon Sep 17 00:00:00 2001
From: p-zach <porterdzach@gmail.com>
Date: Sat, 26 Apr 2025 20:15:54 -0400
Subject: [PATCH] Wrap many SLAM classes

---
 gtsam/slam/slam.i | 178 +++++++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 176 insertions(+), 2 deletions(-)

diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i
index d8a2e49bf..89c9943ce 100644
--- a/gtsam/slam/slam.i
+++ b/gtsam/slam/slam.i
@@ -162,12 +162,79 @@ enum DegeneracyMode { IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY };
 
 class SmartProjectionParams {
   SmartProjectionParams();
+  SmartProjectionParams(gtsam::LinearizationMode linMode = gtsam::LinearizationMode::HESSIAN,
+    gtsam::DegeneracyMode degMode = gtsam::DegeneracyMode::IGNORE_DEGENERACY, bool throwCheirality = false,
+    bool verboseCheirality = false, double retriangulationTh = 1e-5);
+
   void setLinearizationMode(gtsam::LinearizationMode linMode);
   void setDegeneracyMode(gtsam::DegeneracyMode degMode);
   void setRankTolerance(double rankTol);
   void setEnableEPI(bool enableEPI);
   void setLandmarkDistanceThreshold(bool landmarkDistanceThreshold);
   void setDynamicOutlierRejectionThreshold(bool dynOutRejectionThreshold);
+
+  void print(const std::string& str = "") const;
+};
+
+template <CAMERA = {gtsam::PinholeCamera<gtsam::Cal3_S2>}>
+class SmartProjectionFactor : gtsam::NonlinearFactor {
+  SmartProjectionFactor();
+
+  SmartProjectionFactor(
+      const gtsam::noiseModel::Base* sharedNoiseModel,
+      const gtsam::SmartProjectionParams& params = gtsam::SmartProjectionParams());
+
+  void print(const std::string& s = "", const gtsam::KeyFormatter& keyFormatter =
+    gtsam::DefaultKeyFormatter) const;
+  bool equals(const gtsam::NonlinearFactor& p, double tol = 1e-9) const;
+
+  bool decideIfTriangulate(const gtsam::CameraSet<CAMERA>& cameras) const;
+  gtsam::TriangulationResult triangulateSafe(const gtsam::CameraSet<CAMERA>& cameras) const;
+  bool triangulateForLinearize(const gtsam::CameraSet<CAMERA>& cameras) const;
+
+  gtsam::HessianFactor createHessianFactor(
+      const gtsam::CameraSet<CAMERA>& cameras, const double lambda = 0.0,
+      bool diagonalDamping = false) const;
+  gtsam::JacobianFactor createJacobianQFactor(
+      const gtsam::CameraSet<CAMERA>& cameras, double lambda) const;
+  gtsam::JacobianFactor createJacobianQFactor(
+      const gtsam::Values& values, double lambda) const;
+  gtsam::JacobianFactor createJacobianSVDFactor(
+      const gtsam::CameraSet<CAMERA>& cameras, double lambda) const;
+  gtsam::HessianFactor linearizeToHessian(
+      const gtsam::Values& values, double lambda = 0.0) const;
+  gtsam::JacobianFactor linearizeToJacobian(
+      const gtsam::Values& values, double lambda = 0.0) const;
+
+  gtsam::GaussianFactor linearizeDamped(const gtsam::CameraSet<CAMERA>& cameras,
+      const double lambda = 0.0) const;
+
+  gtsam::GaussianFactor linearizeDamped(const gtsam::Values& values,
+      const double lambda = 0.0) const;
+
+  gtsam::GaussianFactor linearize(
+      const gtsam::Values& values) const;
+
+  bool triangulateAndComputeE(gtsam::Matrix& E, const gtsam::CameraSet<CAMERA>& cameras) const;
+
+  bool triangulateAndComputeE(gtsam::Matrix& E, const gtsam::Values& values) const;
+
+  gtsam::Vector reprojectionErrorAfterTriangulation(const gtsam::Values& values) const;
+
+  double totalReprojectionError(const gtsam::CameraSet<CAMERA>& cameras,
+    gtsam::Point3 externalPoint) const;
+
+  double error(const gtsam::Values& values) const;
+
+  gtsam::TriangulationResult point() const;
+
+  gtsam::TriangulationResult point(const gtsam::Values& values) const;
+
+  bool isValid() const;
+  bool isDegenerate() const;
+  bool isPointBehindCamera() const;
+  bool isOutlier() const;
+  bool isFarPoint() const;
 };
 
 #include <gtsam/slam/SmartProjectionPoseFactor.h>
@@ -198,6 +265,42 @@ virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor {
 typedef gtsam::SmartProjectionPoseFactor<gtsam::Cal3_S2>
     SmartProjectionPose3Factor;
 
+// WIP
+// #include <gtsam/geometry/PinholeCamera.h>
+// typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
+// #include <gtsam/slam/SmartProjectionRigFactor.h>
+// template <CAMERA = {PinholeCameraCal3_S2}>
+// class SmartProjectionRigFactor : gtsam::SmartProjectionFactor<CAMERA> {
+//   SmartProjectionRigFactor();
+
+//   SmartProjectionRigFactor(
+//       const gtsam::noiseModel::Base* sharedNoiseModel,
+//       const gtsam::CameraSet<CAMERA>& cameraRig,
+//       const gtsam::SmartProjectionParams& params = gtsam::SmartProjectionParams());
+
+//   void add(const CAMERA::Measurement& measured, const gtsam::Key& poseKey,
+//            const size_t& cameraId = 0);
+
+//   void add(const CAMERA::MeasurementVector& measurements, const gtsam::KeyVector& poseKeys,
+//            const gtsam::FastVector<size_t>& cameraIds = gtsam::FastVector<size_t>());
+
+//   const gtsam::KeyVector& nonUniqueKeys() const;
+
+//   const gtsam::CameraSet<CAMERA>& cameraRig() const;
+
+//   const gtsam::FastVector<size_t>& cameraIds() const;
+
+//   void print(
+//       const std::string& s = "",
+//       const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const;
+
+//   bool equals(const gtsam::NonlinearFactor& p, double tol = 1e-9) const;
+
+//   gtsam::CameraSet<CAMERA> cameras(const gtsam::Values& values) const;
+  
+//   double error(const gtsam::Values& values) const;
+// };
+
 #include <gtsam/slam/StereoFactor.h>
 template <POSE, LANDMARK>
 virtual class GenericStereoFactor : gtsam::NoiseModelFactor {
@@ -227,11 +330,72 @@ virtual class GenericStereoFactor : gtsam::NoiseModelFactor {
 typedef gtsam::GenericStereoFactor<gtsam::Pose3, gtsam::Point3>
     GenericStereoFactor3D;
 
+#include <gtsam/slam/ReferenceFrameFactor.h>
+template<LANDMARK, POSE>
+class ReferenceFrameFactor : gtsam::NoiseModelFactor {
+  ReferenceFrameFactor(gtsam::Key globalKey, gtsam::Key transKey, 
+                       gtsam::Key localKey, const gtsam::noiseModel::Base* model);
+
+  gtsam::Vector evaluateError(const LANDMARK& global, const POSE& trans, const LANDMARK& local);
+
+  void print(const std::string& s="",
+    const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter);
+};
+
+#include <gtsam/slam/RotateFactor.h>
+class RotateFactor : gtsam::NoiseModelFactor {
+  RotateFactor(gtsam::Key key, const gtsam::Rot3& P, const gtsam::Rot3& Z,
+    const gtsam::noiseModel::Base* model);
+
+  void print(const std::string& s = "",
+    const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const;
+
+  gtsam::Vector evaluateError(const gtsam::Rot3& R) const;
+};
+class RotateDirectionsFactor : gtsam::NoiseModelFactor {
+  RotateDirectionsFactor(gtsam::Key key, const gtsam::Unit3& i_p, const gtsam::Unit3& c_z,
+    const gtsam::noiseModel::Base* model);
+
+  static gtsam::Rot3 Initialize(const gtsam::Unit3& i_p, const gtsam::Unit3& c_z);
+
+  void print(const std::string& s = "",
+      const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const;
+
+  gtsam::Vector evaluateError(const gtsam::Rot3& iRc) const;
+};
+
+#include <gtsam/slam/OrientedPlane3Factor.h>
+class OrientedPlane3Factor : gtsam::NoiseModelFactor {
+  OrientedPlane3Factor();
+  OrientedPlane3Factor(const gtsam::Vector& z, const gtsam::noiseModel::Gaussian* noiseModel,
+      gtsam::Key poseKey, gtsam::Key landmarkKey);
+
+  void print(const std::string& s = "OrientedPlane3Factor",
+      const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const;
+
+  gtsam::Vector evaluateError(
+      const gtsam::Pose3& pose, const gtsam::OrientedPlane3& plane) const;
+};
+class OrientedPlane3DirectionPrior : gtsam::NoiseModelFactor {
+  OrientedPlane3DirectionPrior();
+  OrientedPlane3DirectionPrior(gtsam::Key key, const gtsam::Vector& z,
+                               const gtsam::noiseModel::Gaussian* noiseModel);
+
+  void print(const std::string& s = "OrientedPlane3DirectionPrior",
+      const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const;
+
+  bool equals(const gtsam::NonlinearFactor& expected, double tol = 1e-9) const;
+
+  gtsam::Vector evaluateError(const gtsam::OrientedPlane3& plane) const;
+};
+
 #include <gtsam/slam/PoseTranslationPrior.h>
 template <POSE>
 virtual class PoseTranslationPrior : gtsam::NoiseModelFactor {
+  PoseTranslationPrior(size_t key, const POSE::Translation& measured,
+                       const gtsam::noiseModel::Base* model);
   PoseTranslationPrior(size_t key, const POSE& pose_z,
-                       const gtsam::noiseModel::Base* noiseModel);
+                       const gtsam::noiseModel::Base* model);
   POSE::Translation measured() const;
 
   // enabling serialization functionality
@@ -244,8 +408,10 @@ typedef gtsam::PoseTranslationPrior<gtsam::Pose3> PoseTranslationPrior3D;
 #include <gtsam/slam/PoseRotationPrior.h>
 template <POSE>
 virtual class PoseRotationPrior : gtsam::NoiseModelFactor {
+  PoseRotationPrior(size_t key, const POSE::Rotation& rot_z,
+                    const gtsam::noiseModel::Base* model);
   PoseRotationPrior(size_t key, const POSE& pose_z,
-                    const gtsam::noiseModel::Base* noiseModel);
+                    const gtsam::noiseModel::Base* model);
   POSE::Rotation measured() const;
 };
 
@@ -404,6 +570,14 @@ gtsam::Rot3 FindKarcherMean(const gtsam::Rot3Vector& rotations);
 gtsam::noiseModel::Isotropic* ConvertNoiseModel(gtsam::noiseModel::Base* model,
                                                 size_t d);
 
+template <T = {gtsam::Rot2, gtsam::Rot3, gtsam::SO3, gtsam::SO4, gtsam::Pose2, gtsam::Pose3}>
+class FrobeniusPrior : gtsam::NoiseModelFactor {
+  FrobeniusPrior(gtsam::Key j, const gtsam::Matrix& M,
+    const gtsam::noiseModel::Base* model);
+
+    gtsam::Vector evaluateError(const T& g) const;
+};
+
 template <T = {gtsam::Rot2, gtsam::Rot3, gtsam::SO3, gtsam::SO4, gtsam::Pose2, gtsam::Pose3}>
 virtual class FrobeniusFactor : gtsam::NoiseModelFactor {
   FrobeniusFactor(size_t key1, size_t key2);