diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 75c0847a4..7818058a3 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -837,12 +837,25 @@ class CalibratedCamera { // Action on Point3 gtsam::Point2 project(const gtsam::Point3& point) const; + gtsam::Point2 project(const gtsam::Point3& point, + Eigen::Ref Dcamera, + Eigen::Ref Dpoint); + gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; + gtsam::Point3 backproject(const gtsam::Point2& p, double depth, + Eigen::Ref Dresult_dpose, + Eigen::Ref Dresult_dp, + Eigen::Ref Dresult_ddepth); + static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); // Standard Interface gtsam::Pose3 pose() const; double range(const gtsam::Point3& point) const; + double range(const gtsam::Point3& point, Eigen::Ref Dcamera, + Eigen::Ref Dpoint); double range(const gtsam::Pose3& pose) const; + double range(const gtsam::Pose3& point, Eigen::Ref Dcamera, + Eigen::Ref Dpose); double range(const gtsam::CalibratedCamera& camera) const; // enabling serialization functionality @@ -965,9 +978,18 @@ class StereoCamera { static size_t Dim(); // Transformations and measurement functions - gtsam::StereoPoint2 project(const gtsam::Point3& point); + gtsam::StereoPoint2 project(const gtsam::Point3& point) const; gtsam::Point3 backproject(const gtsam::StereoPoint2& p) const; + // project with Jacobian + gtsam::StereoPoint2 project2(const gtsam::Point3& point, + Eigen::Ref H1, + Eigen::Ref H2) const; + + gtsam::Point3 backproject2(const gtsam::StereoPoint2& p, + Eigen::Ref H1, + Eigen::Ref H2) const; + // enabling serialization functionality void serialize() const; };