Merge pull request #1846 from borglab/wrap-triangulation-factor

borglab · Sep 26, 2024 · d72d23c · d72d23c
2 parents 8d5ff15 + 580dcb2
commit d72d23c
Show file tree

Hide file tree

Showing 5 changed files with 55 additions and 15 deletions.
diff --git a/cmake/Config.cmake.in b/cmake/Config.cmake.in
@@ -15,10 +15,12 @@ endif()
 # Find dependencies, required by cmake exported targets:
 include(CMakeFindDependencyMacro)
 # Allow using cmake < 3.8
-if(${CMAKE_VERSION} VERSION_LESS "3.8.0")
-find_package(Boost @BOOST_FIND_MINIMUM_VERSION@ COMPONENTS @BOOST_FIND_MINIMUM_COMPONENTS@)
-else()
-find_dependency(Boost @BOOST_FIND_MINIMUM_VERSION@ COMPONENTS @BOOST_FIND_MINIMUM_COMPONENTS@)
+if (@GTSAM_ENABLE_BOOST_SERIALIZATION@ OR @GTSAM_USE_BOOST_FEATURES@)
+  if(${CMAKE_VERSION} VERSION_LESS "3.8.0")
+    find_package(Boost @BOOST_FIND_MINIMUM_VERSION@ COMPONENTS @BOOST_FIND_MINIMUM_COMPONENTS@)
+  else()
+    find_dependency(Boost @BOOST_FIND_MINIMUM_VERSION@ COMPONENTS @BOOST_FIND_MINIMUM_COMPONENTS@)
+  endif()
 endif()
 
 if(@GTSAM_USE_TBB@)

diff --git a/gtsam/inference/Factor.h b/gtsam/inference/Factor.h
@@ -24,13 +24,13 @@
 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/nvp.hpp>
 #endif
-#include <memory>
-#include <algorithm>
-
-#include <gtsam/base/types.h>
 #include <gtsam/base/FastVector.h>
+#include <gtsam/base/types.h>
 #include <gtsam/inference/Key.h>
 
+#include <algorithm>
+#include <memory>
+
 namespace gtsam {
 
   /// Define collection types:

diff --git a/gtsam/slam/TriangulationFactor.h b/gtsam/slam/TriangulationFactor.h
@@ -34,6 +34,8 @@ class TriangulationFactor: public NoiseModelFactorN<Point3> {
 
   /// CAMERA type
   using Camera = CAMERA;
+  /// shorthand for measurement type, e.g. Point2 or StereoPoint2
+  using Measurement = typename CAMERA::Measurement;
 
 protected:
 
@@ -43,9 +45,6 @@ class TriangulationFactor: public NoiseModelFactorN<Point3> {
   /// shorthand for this class
   using This = TriangulationFactor<CAMERA>;
 
-  /// shorthand for measurement type, e.g. Point2 or StereoPoint2
-  using Measurement = typename CAMERA::Measurement;
-
   // Keep a copy of measurement and calibration for I/O
   const CAMERA camera_; ///< CAMERA in which this landmark was seen
   const Measurement measured_; ///< 2D measurement

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
diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt
@@ -257,7 +257,7 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON)
         COMMAND
         ${CMAKE_COMMAND} -E env
         "PYTHONPATH=${GTSAM_PYTHON_BUILD_DIRECTORY}/$ENV{PYTHONPATH}"
-        pybind11-stubgen -o . --enum-class-locations \"KernelFunctionType|NoiseFormat:gtsam.gtsam\" --enum-class-locations \"OrderingType:gtsam.gtsam.Ordering\" --numpy-array-use-type-var gtsam_unstable
+        pybind11-stubgen -o . --enum-class-locations \"KernelFunctionType|NoiseFormat:gtsam.gtsam\" --enum-class-locations \"OrderingType:gtsam.gtsam.Ordering\" --numpy-array-use-type-var --ignore-all-errors gtsam_unstable
         DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} ${GTSAM_PYTHON_TEST_FILES} ${GTSAM_PYTHON_UNSTABLE_TARGET}
         WORKING_DIRECTORY "${GTSAM_PYTHON_BUILD_DIRECTORY}/"
     )
@@ -284,7 +284,7 @@ add_custom_target(
         COMMAND
         ${CMAKE_COMMAND} -E env
         "PYTHONPATH=${GTSAM_PYTHON_BUILD_DIRECTORY}/$ENV{PYTHONPATH}"
-        pybind11-stubgen -o . --enum-class-locations \"KernelFunctionType|NoiseFormat:gtsam.gtsam\" --enum-class-locations \"OrderingType:gtsam.gtsam.Ordering\" --numpy-array-use-type-var gtsam
+        pybind11-stubgen -o . --enum-class-locations \"KernelFunctionType|NoiseFormat:gtsam.gtsam\" --enum-class-locations \"OrderingType:gtsam.gtsam.Ordering\" --numpy-array-use-type-var --ignore-all-errors gtsam
         DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} ${GTSAM_PYTHON_TEST_FILES} ${GTSAM_PYTHON_TARGET}
         WORKING_DIRECTORY "${GTSAM_PYTHON_BUILD_DIRECTORY}/"
 )