diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index 7b92be5d5..4bfbc8b2d 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -121,6 +121,19 @@ public: return _project(pw, Dpose, Dpoint, Dcal); } + /// project, but for Python use + Point2 project(const Point3& pw, Eigen::Ref Dpose, Eigen::Ref Dpoint, Eigen::Ref Dcal) const { + Eigen::Matrix Dpose_; + Eigen::Matrix Dpoint_; + Eigen::Matrix Dcal_; + + auto ret = _project(pw, Dpose_, Dpoint_, Dcal_); + Dpose = Dpose_; + Dpoint = Dpoint_; + Dcal = Dcal_; + return ret; + } + /// project a 3D point from world coordinates into the image Point2 reprojectionError(const Point3& pw, const Point2& measured, OptionalJacobian<2, 6> Dpose = boost::none, OptionalJacobian<2, 3> Dpoint = boost::none, diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 858270c00..46efebde3 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -824,6 +824,7 @@ template class PinholeCamera { // Standard Constructors and Named Constructors PinholeCamera(); + PinholeCamera(const gtsam::PinholeCamera other); PinholeCamera(const gtsam::Pose3& pose); PinholeCamera(const gtsam::Pose3& pose, const CALIBRATION& K); static This Level(const CALIBRATION& K, const gtsam::Pose2& pose, @@ -850,6 +851,7 @@ class PinholeCamera { static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); pair projectSafe(const gtsam::Point3& pw) const; gtsam::Point2 project(const gtsam::Point3& point); + gtsam::Point2 project(const gtsam::Point3& point, Eigen::Ref Dpose, Eigen::Ref Dpoint, Eigen::Ref Dcal); gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; double range(const gtsam::Point3& point); double range(const gtsam::Pose3& pose);