Merge pull request #2313 from dvorak0/yzf/add_smart_factor_python_export

add python exports for SmartStereoProjectionFactor

Author: ProfFan

gtsam_unstable/gtsam_unstable.i

@@ -10,11 +10,15 @@ class gtsam::Point2Vector;
 class gtsam::Rot2;
 class gtsam::Pose2;
 class gtsam::Point3;
+class gtsam::StereoCamera;
 class gtsam::SO3;
 class gtsam::SO4;
 class gtsam::SOn;
 class gtsam::Rot3;
 class gtsam::Pose3;
+class gtsam::StereoCamera;
+class gtsam::SmartProjectionParams;
+class gtsam::SmartStereoProjectionFactor;
 virtual class gtsam::noiseModel::Base;
 virtual class gtsam::noiseModel::Gaussian;
 virtual class gtsam::noiseModel::Isotropic;
@@ -27,6 +31,7 @@ virtual class gtsam::HessianFactor;
 virtual class gtsam::JacobianFactor;
 class gtsam::Cal3_S2;
 class gtsam::Cal3DS2;
+class gtsam::Cal3_S2Stereo;
 class gtsam::GaussianFactorGraph;
 class gtsam::NonlinearFactorGraph;
 class gtsam::Ordering;
@@ -41,6 +46,7 @@ class gtsam::ISAM2Params;
 class gtsam::GaussianDensity;
 class gtsam::LevenbergMarquardtOptimizer;
 class gtsam::FixedLagSmoother;
+class gtsam::StereoPoint2;
 
 namespace gtsam {
 
@@ -731,6 +737,58 @@ typedef gtsam::ProjectionFactorPPPC<gtsam::Pose3, gtsam::Point3, gtsam::Cal3_S2>
 typedef gtsam::ProjectionFactorPPPC<gtsam::Pose3, gtsam::Point3, gtsam::Cal3DS2> ProjectionFactorPPPCCal3DS2;
 typedef gtsam::ProjectionFactorPPPC<gtsam::Pose3, gtsam::Point3, gtsam::Cal3Fisheye> ProjectionFactorPPPCCal3Fisheye;
 
+#include <gtsam_unstable/slam/SmartStereoProjectionFactor.h>
+virtual class SmartStereoProjectionFactor : gtsam::NonlinearFactor {
+  SmartStereoProjectionFactor(const gtsam::noiseModel::Base* sharedNoiseModel,
+      const gtsam::SmartProjectionParams& params,
+      const gtsam::Pose3& body_P_sensor);
+  SmartStereoProjectionFactor(const gtsam::noiseModel::Base* sharedNoiseModel,
+      const gtsam::SmartProjectionParams& params);
+  SmartStereoProjectionFactor(const gtsam::noiseModel::Base* sharedNoiseModel);
+
+  void print(string s) const;
+  bool equals(const gtsam::NonlinearFactor& p, double tol) const;
+  double error(const gtsam::Values& values) const;
+
+  gtsam::TriangulationResult point() const;
+  gtsam::TriangulationResult point(const gtsam::Values& values) const;
+
+  bool isValid() const;
+  bool isDegenerate() const;
+  bool isPointBehindCamera() const;
+  bool isOutlier() const;
+  bool isFarPoint() const;
+
+  gtsam::GaussianFactor linearizeDamped(const gtsam::CameraSet<gtsam::StereoCamera>& cameras,
+      const double lambda) const;
+  gtsam::GaussianFactor linearizeDamped(const gtsam::Values& values,
+      const double lambda) const;
+};
+
+#include <gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h>
+virtual class SmartStereoProjectionPoseFactor : gtsam::SmartStereoProjectionFactor {
+  SmartStereoProjectionPoseFactor(const gtsam::noiseModel::Base* sharedNoiseModel,
+      const gtsam::SmartProjectionParams& params,
+      const gtsam::Pose3& body_P_sensor);
+  SmartStereoProjectionPoseFactor(const gtsam::noiseModel::Base* sharedNoiseModel,
+      const gtsam::SmartProjectionParams& params);
+  SmartStereoProjectionPoseFactor(const gtsam::noiseModel::Base* sharedNoiseModel);
+
+  void add(const gtsam::StereoPoint2& measured, gtsam::Key poseKey,
+      const std::shared_ptr<gtsam::Cal3_S2Stereo>& K);
+  void add(const std::vector<gtsam::StereoPoint2>& measurements,
+      const gtsam::KeyVector& poseKeys,
+      const std::vector<std::shared_ptr<gtsam::Cal3_S2Stereo>>& Ks);
+  void add(const std::vector<gtsam::StereoPoint2>& measurements,
+      const gtsam::KeyVector& poseKeys,
+      const std::shared_ptr<gtsam::Cal3_S2Stereo>& K);
+
+  void print(string s) const;
+  bool equals(const gtsam::NonlinearFactor& p, double tol) const;
+  double error(const gtsam::Values& values) const;
+  std::vector<std::shared_ptr<gtsam::Cal3_S2Stereo>> calibration() const;
+};
+
 #include <gtsam_unstable/slam/ProjectionFactorRollingShutter.h>
 virtual class ProjectionFactorRollingShutter : gtsam::NoiseModelFactor {
   ProjectionFactorRollingShutter(const gtsam::Point2& measured, double alpha, const gtsam::noiseModel::Base* noiseModel,

python/gtsam_unstable/examples/SmartStereoProjectionPoseFactorExample.py

@@ -0,0 +1,55 @@
+"""
+Minimal smoke test for SmartStereoProjectionPoseFactor.
+
+Constructs two stereo measurements of a single 3D landmark from two poses and
+verifies the factor evaluates to (near) zero error given consistent geometry.
+"""
+
+import pathlib
+import sys
+
+import gtsam  # type: ignore
+
+from gtsam_unstable import SmartStereoProjectionPoseFactor
+
+
+def main() -> None:
+    # Simple stereo calibration
+    K = gtsam.Cal3_S2Stereo(500, 500, 0.0, 320, 240, 0.2)
+
+    # Two camera poses observing the same landmark
+    pose_a = gtsam.Pose3()  # identity
+    pose_b = gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(0.2, 0.0, 0.0))
+    landmark = gtsam.Point3(0.0, 0.1, 1.2)
+
+    # Generate synthetic stereo measurements
+    cam_a = gtsam.StereoCamera(pose_a, K)
+    cam_b = gtsam.StereoCamera(pose_b, K)
+    z_a = cam_a.project(landmark)
+    z_b = cam_b.project(landmark)
+
+    # Add a small pixel noise to the second measurement to verify non-zero error
+    z_b = gtsam.StereoPoint2(z_b.uL() + 0.2, z_b.uR() + 0.2, z_b.v() + 0.1)
+    print(f"z_a, uL: {z_a.uL():.2f}, uR: {z_a.uR():.2f}, v: {z_a.v():.2f}")
+    print(f"z_b, uL: {z_b.uL():.2f}, uR: {z_b.uR():.2f}, v: {z_b.v():.2f}")
+
+    # Build factor with modest noise and default smart projection params
+    noise = gtsam.noiseModel.Isotropic.Sigma(3, 1.0)
+    params = gtsam.SmartProjectionParams()
+    factor = SmartStereoProjectionPoseFactor(noise, params)
+
+    key_a = gtsam.symbol("x", 0)
+    key_b = gtsam.symbol("x", 1)
+    factor.add(z_a, key_a, K)
+    factor.add(z_b, key_b, K)
+
+    values = gtsam.Values()
+    values.insert(key_a, pose_a)
+    values.insert(key_b, pose_b)
+
+    err = factor.error(values)
+    print(f"SmartStereoProjectionPoseFactor error: {err:.6f}")
+
+
+if __name__ == "__main__":
+    main()