Merge pull request #1755 from contagon/python-helpers

Tiny Updates to Python Wrapper
borglab · Jun 10, 2024 · aba0561 · aba0561
2 parents 50021ed + 32e8a43
commit aba0561
Show file tree

Hide file tree

Showing 5 changed files with 33 additions and 1 deletion.
diff --git 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 <gtsam/geometry/Cal3Bundler.h>

diff --git 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
@@ -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
@@ -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
@@ -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;