diff --git a/gtsam/geometry/GeneralCameraT.h b/gtsam/geometry/GeneralCameraT.h index c00d58312..5c4dbe85f 100644 --- a/gtsam/geometry/GeneralCameraT.h +++ b/gtsam/geometry/GeneralCameraT.h @@ -72,6 +72,12 @@ class GeneralCameraT { cameraPoint.z() > 0); } + Point3 backproject(const Point2& projection, const double scale) const { + Point2 intrinsic = calibration_.calibrate(projection); + Point3 cameraPoint = CalibratedCamera::backproject_from_camera(intrinsic, scale); + return calibrated_.pose().transform_from(cameraPoint); + } + /** * project a 3d point to the camera * P is point in camera coordinate