diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index 18df33f40..9cc910f1d 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -149,9 +149,6 @@ public: /// Project a point into the image and check depth std::pair projectSafe(const Point3& pw) const; - /// 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 * Throws a CheiralityException if point behind image plane iff GTSAM_THROW_CHEIRALITY_EXCEPTION @@ -163,6 +160,9 @@ public: Point2 project2(const Point3& point, OptionalJacobian<2, 6> Dpose = boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const; + /// backproject a 2-dimensional point to a 3-dimensional point at given depth + static Point3 backproject_from_camera(const Point2& p, const double depth); + private: /** Serialization function */ @@ -274,6 +274,11 @@ public: OptionalJacobian<2, 6> Dcamera = boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const; + /// backproject a 2-dimensional point to a 3-dimensional point at given depth + Point3 backproject(const Point2& pn, double depth) const { + return pose().transform_from(backproject_from_camera(pn, depth)); + } + /** * Calculate range to a landmark * @param point 3D location of landmark