diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 01161817b3..3cf78989ca 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -69,6 +69,7 @@ class StereoPoint2 { // Standard Constructors StereoPoint2(); StereoPoint2(double uL, double uR, double v); + StereoPoint2(const gtsam::Vector3 &v); // Testable void print(string s = "") const; @@ -836,6 +837,12 @@ class Cal3_S2Stereo { Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b); Cal3_S2Stereo(Vector v); + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::Cal3_S2Stereo retract(Vector v) const; + Vector localCoordinates(const gtsam::Cal3_S2Stereo& c) const; + // Testable void print(string s = "") const; bool equals(const gtsam::Cal3_S2Stereo& K, double tol) const; @@ -846,8 +853,11 @@ class Cal3_S2Stereo { double skew() const; double px() const; double py() const; + Matrix K() const; gtsam::Point2 principalPoint() const; double baseline() const; + Vector6 vector() const; + Matrix inverse() const; }; #include diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index 92864c18a0..aad6f98514 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -200,7 +200,7 @@ class PreintegratedCombinedMeasurements { const gtsam::imuBias::ConstantBias& bias) const; }; -virtual class CombinedImuFactor: gtsam::NonlinearFactor { +virtual class CombinedImuFactor: gtsam::NoiseModelFactor { CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias_i, size_t bias_j, const gtsam::PreintegratedCombinedMeasurements& CombinedPreintegratedMeasurements); diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 3f5fb1dd54..c39cfd22a8 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -130,6 +130,7 @@ virtual class NonlinearFactor : gtsam::Factor { virtual class NoiseModelFactor : gtsam::NonlinearFactor { bool equals(const gtsam::NoiseModelFactor& other, double tol) const; gtsam::noiseModel::Base* noiseModel() const; + gtsam::NoiseModelFactor* cloneWithNewNoiseModel(gtsam::noiseModel::Base* newNoise) const; Vector unwhitenedError(const gtsam::Values& x) const; Vector whitenedError(const gtsam::Values& x) const; }; @@ -320,6 +321,8 @@ virtual class GncParams { enum Verbosity { SILENT, SUMMARY, + MU, + WEIGHTS, VALUES }; }; diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h index a2d428d111..5adafcf3a8 100644 --- a/gtsam/slam/StereoFactor.h +++ b/gtsam/slam/StereoFactor.h @@ -169,6 +169,11 @@ class GenericStereoFactor: public NoiseModelFactorN { /** 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 64977a2a5f..97b198b2f4 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -171,6 +171,20 @@ 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;