diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h index a2d428d11..5adafcf3a 100644 --- a/gtsam/slam/StereoFactor.h +++ b/gtsam/slam/StereoFactor.h @@ -169,6 +169,11 @@ public: /** 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& 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 index 8dc10e51c..97b198b2f 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -168,10 +168,23 @@ typedef gtsam::SmartProjectionPoseFactor #include template 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;