Expanded stereofactor python constructor

borglab · May 27, 2024 · d7e2e1c · d7e2e1c
1 parent e116123
commit d7e2e1c
Show file tree

Hide file tree

Showing 2 changed files with 18 additions and 0 deletions.
diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h
@@ -169,6 +169,11 @@ class GenericStereoFactor: public NoiseModelFactorN<POSE, LANDMARK> {
   /** return flag for throwing cheirality exceptions */
   inline bool throwCheirality() const { return throwCheirality_; }
 
+  /** return the (optional) sensor pose with respect to the vehicle frame */
+  const std::optional<POSE>& body_P_sensor() const {
+    return body_P_sensor_;
+  }
+
 private:
 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */

diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i
@@ -168,9 +168,22 @@ typedef gtsam::SmartProjectionPoseFactor<gtsam::Cal3_S2>
 #include <gtsam/slam/StereoFactor.h>
 template <POSE, LANDMARK>
 virtual class GenericStereoFactor : gtsam::NoiseModelFactor {
+  GenericStereoFactor(const gtsam::StereoPoint2& measured,
+                      const gtsam::noiseModel::Base* noiseModel, size_t poseKey,
+                      size_t landmarkKey, const gtsam::Cal3_S2Stereo* K);
+  GenericStereoFactor(const gtsam::StereoPoint2& measured,
+                      const gtsam::noiseModel::Base* noiseModel, size_t poseKey,
+                      size_t landmarkKey, const gtsam::Cal3_S2Stereo* K,
+                      POSE body_P_sensor);
+
+  GenericStereoFactor(const gtsam::StereoPoint2& measured,
+                      const gtsam::noiseModel::Base* noiseModel, size_t poseKey,
+                      size_t landmarkKey, const gtsam::Cal3_S2Stereo* K,
+                      bool throwCheirality, bool verboseCheirality);
   GenericStereoFactor(const gtsam::StereoPoint2& measured,
                       const gtsam::noiseModel::Base* noiseModel, size_t poseKey,
                       size_t landmarkKey, const gtsam::Cal3_S2Stereo* K,
+                      bool throwCheirality, bool verboseCheirality,
                       POSE body_P_sensor);
   gtsam::StereoPoint2 measured() const;
   gtsam::Cal3_S2Stereo* calibration() const;