wrap TriangulationFactor

borglab · Sep 25, 2024 · 832ef77 · 832ef77
1 parent 798db54
commit 832ef77
Showing 1 changed file with 41 additions and 2 deletions.
diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i
@@ -347,11 +347,50 @@ virtual class FrobeniusBetweenFactor : gtsam::NoiseModelFactor {
 
   gtsam::Vector evaluateError(const T& R1, const T& R2);
 };
-
+
+#include <gtsam/slam/TriangulationFactor.h>
+template <CAMERA>
+virtual class TriangulationFactor : gtsam::NoiseModelFactor {
+  TriangulationFactor();
+  TriangulationFactor(const CAMERA& camera, const gtsam::This::Measurement& measured,
+                      const gtsam::noiseModel::Base* model, gtsam::Key pointKey,
+                      bool throwCheirality = false,
+                      bool verboseCheirality = false);
+
+  void print(const string& s = "", const gtsam::KeyFormatter& keyFormatter =
+                                       gtsam::DefaultKeyFormatter) const;
+  bool equals(const This& p, double tol = 1e-9) const;
+
+  gtsam::Vector evaluateError(const gtsam::Point3& point) const;
+
+  const gtsam::This::Measurement& measured() const;
+};
+typedef gtsam::TriangulationFactor<gtsam::PinholeCamera<gtsam::Cal3_S2>>
+    TriangulationFactorCal3_S2;
+typedef gtsam::TriangulationFactor<gtsam::PinholeCamera<gtsam::Cal3DS2>>
+    TriangulationFactorCal3DS2;
+typedef gtsam::TriangulationFactor<gtsam::PinholeCamera<gtsam::Cal3Bundler>>
+    TriangulationFactorCal3Bundler;
+typedef gtsam::TriangulationFactor<gtsam::PinholeCamera<gtsam::Cal3Fisheye>>
+    TriangulationFactorCal3Fisheye;
+typedef gtsam::TriangulationFactor<gtsam::PinholeCamera<gtsam::Cal3Unified>>
+    TriangulationFactorCal3Unified;
+
+typedef gtsam::TriangulationFactor<gtsam::PinholePose<gtsam::Cal3_S2>>
+    TriangulationFactorPoseCal3_S2;
+typedef gtsam::TriangulationFactor<gtsam::PinholePose<gtsam::Cal3DS2>>
+    TriangulationFactorPoseCal3DS2;
+typedef gtsam::TriangulationFactor<gtsam::PinholePose<gtsam::Cal3Bundler>>
+    TriangulationFactorPoseCal3Bundler;
+typedef gtsam::TriangulationFactor<gtsam::PinholePose<gtsam::Cal3Fisheye>>
+    TriangulationFactorPoseCal3Fisheye;
+typedef gtsam::TriangulationFactor<gtsam::PinholePose<gtsam::Cal3Unified>>
+    TriangulationFactorPoseCal3Unified;
+
 #include <gtsam/slam/lago.h>
 namespace lago {
   gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, bool useOdometricPath = true);
   gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialGuess);
 }
-  
+
 }  // namespace gtsam