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

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")
+if (@GTSAM_ENABLE_BOOST_SERIALIZATION@ OR @GTSAM_USE_BOOST_FEATURES@)
-find_package(Boost @BOOST_FIND_MINIMUM_VERSION@ COMPONENTS @BOOST_FIND_MINIMUM_COMPONENTS@)
+  if(${CMAKE_VERSION} VERSION_LESS "3.8.0")
-else()
+    find_package(Boost @BOOST_FIND_MINIMUM_VERSION@ COMPONENTS @BOOST_FIND_MINIMUM_COMPONENTS@)
-find_dependency(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@)

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:

gtsam/slam/TriangulationFactor.h

@@ -34,6 +34,8 @@ public:
 
   /// CAMERA type
   using Camera = CAMERA;
+  /// shorthand for measurement type, e.g. Point2 or StereoPoint2
+  using Measurement = typename CAMERA::Measurement;
 
 protected:
 
@@ -43,9 +45,6 @@ protected:
   /// 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

gtsam/slam/slam.i

@@ -348,6 +348,45 @@ 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);

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}/"
 )