diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 095da4daa..b7e9df31b 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -289,6 +289,22 @@ public: return std::make_pair(K_.uncalibrate(pn), pc.z() > 0); } + /** project a point from world coordinate to the image + * @param pw is a point in world coordinates + */ + inline Point2 project(const Point3& pw) const { + + // Transform to camera coordinates and check cheirality + const Point3 pc = pose_.transform_to(pw); + + // Project to normalized image coordinates + const Point2 pn = project_to_camera(pc); + + return K_.uncalibrate(pn); + } + + typedef Eigen::Matrix Matrix2K; + /** project a point from world coordinate to the image * @param pw is a point in world coordinates * @param Dpose is the Jacobian w.r.t. pose3 @@ -297,9 +313,46 @@ public: */ inline Point2 project( const Point3& pw, // - boost::optional Dpose = boost::none, - boost::optional Dpoint = boost::none, - boost::optional Dcal = boost::none) const { + boost::optional Dpose, + boost::optional Dpoint, + boost::optional Dcal) const { + + // Transform to camera coordinates and check cheirality + const Point3 pc = pose_.transform_to(pw); + + // Project to normalized image coordinates + const Point2 pn = project_to_camera(pc); + + if (Dpose || Dpoint) { + const double z = pc.z(), d = 1.0 / z; + + // uncalibration + Matrix2 Dpi_pn; + const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn); + + // chain the Jacobian matrices + if (Dpose) { + calculateDpose(pn, d, Dpi_pn, *Dpose); + } + if (Dpoint) { + calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint); + } + return pi; + } else + return K_.uncalibrate(pn, Dcal, boost::none); + } + + /** project a point from world coordinate to the image + * @param pw is a point in world coordinates + * @param Dpose is the Jacobian w.r.t. pose3 + * @param Dpoint is the Jacobian w.r.t. point3 + * @param Dcal is the Jacobian w.r.t. calibration + */ + inline Point2 project( + const Point3& pw, // + boost::optional Dpose, + boost::optional Dpoint, + boost::optional Dcal) const { // Transform to camera coordinates and check cheirality const Point3 pc = pose_.transform_to(pw);