diff --git a/gtsam/geometry/CalibratedCamera.cpp b/gtsam/geometry/CalibratedCamera.cpp index 89ca6ba8c..2876da579 100644 --- a/gtsam/geometry/CalibratedCamera.cpp +++ b/gtsam/geometry/CalibratedCamera.cpp @@ -96,6 +96,24 @@ Point2 PinholeBase::project_to_camera(const Point3& P, return Point2(u, v); } +/* ************************************************************************* */ +Point2 PinholeBase::project2(const Point3& point, OptionalJacobian<2, 6> Dpose, + OptionalJacobian<2, 3> Dpoint) const { + + Matrix3 Rt; // calculated by transform_to if needed + const Point3 q = pose().transform_to(point, boost::none, Dpoint ? &Rt : 0); + Point2 pn = project_to_camera(q); + + if (Dpose || Dpoint) { + const double z = q.z(), d = 1.0 / z; + if (Dpose) + *Dpose = PinholeBase::Dpose(pn, d); + if (Dpoint) + *Dpoint = PinholeBase::Dpoint(pn, d, Rt.transpose()); // TODO transpose + } + return pn; +} + /* ************************************************************************* */ Point3 PinholeBase::backproject_from_camera(const Point2& p, const double depth) { @@ -116,19 +134,7 @@ CalibratedCamera CalibratedCamera::Lookat(const Point3& eye, /* ************************************************************************* */ Point2 CalibratedCamera::project(const Point3& point, OptionalJacobian<2, 6> Dcamera, OptionalJacobian<2, 3> Dpoint) const { - - Matrix3 Rt; // calculated by transform_to if needed - const Point3 q = pose().transform_to(point, boost::none, Dpoint ? &Rt : 0); - Point2 pn = project_to_camera(q); - - if (Dcamera || Dpoint) { - const double z = q.z(), d = 1.0 / z; - if (Dcamera) - *Dcamera = PinholeBase::Dpose(pn, d); - if (Dpoint) - *Dpoint = PinholeBase::Dpoint(pn, d, Rt.transpose()); // TODO transpose - } - return pn; + return project2(point, Dcamera, Dpoint); } /* ************************************************************************* */ diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index 457ae2819..9f0ba5ea0 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -147,10 +147,20 @@ public: OptionalJacobian<2, 3> Dpoint = boost::none); /** - * backproject a 2-dimensional point to a 3-dimension point + * backproject a 2-dimensional point to a 3-dimensional point at given depth */ static Point3 backproject_from_camera(const Point2& p, const double depth); + /** + * Project point into the image + * @param point 3D point in world coordinates + * @param Dpose the optionally computed Jacobian with respect to camera + * @param Dpoint the optionally computed Jacobian with respect to the 3D point + * @return the intrinsic coordinates of the projected point + */ + Point2 project2(const Point3& point, OptionalJacobian<2, 6> Dpose = + boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const; + private: /** Serialization function */ @@ -253,16 +263,13 @@ public: /// @} /// @name Transformations and mesaurement functions /// @{ + /** - * This function receives the camera pose and the landmark location and - * returns the location the point is supposed to appear in the image - * @param point a 3D point to be projected - * @param Dpose the optionally computed Jacobian with respect to pose - * @param Dpoint the optionally computed Jacobian with respect to the 3D point - * @return the intrinsic coordinates of the projected point + * @deprecated + * Use project2, which is more consistently named across Pinhole cameras */ Point2 project(const Point3& point, - OptionalJacobian<2, 6> Dpose = boost::none, + OptionalJacobian<2, 6> Dcamera = boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const; /**