diff --git a/gtsam.h b/gtsam.h index 68367393f..c0ed4d20f 100644 --- a/gtsam.h +++ b/gtsam.h @@ -875,6 +875,7 @@ class Cal3_S2 { double py() const; gtsam::Point2 principalPoint() const; Vector vector() const; + Matrix K() const; Matrix matrix() const; Matrix matrix_inverse() const; @@ -1164,6 +1165,9 @@ class StereoCamera { gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, gtsam::Cal3_S2* sharedCal, const gtsam::Point2Vector& measurements, double rank_tol, bool optimize); +gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, + gtsam::Cal3DS2* sharedCal, const gtsam::Point2Vector& measurements, + double rank_tol, bool optimize); gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, gtsam::Cal3Bundler* sharedCal, const gtsam::Point2Vector& measurements, double rank_tol, bool optimize);