diff --git a/image_geometry/include/image_geometry/stereo_camera_model.h b/image_geometry/include/image_geometry/stereo_camera_model.h
index a4f5880cd..7c0ab689d 100644
--- a/image_geometry/include/image_geometry/stereo_camera_model.h
+++ b/image_geometry/include/image_geometry/stereo_camera_model.h
@@ -61,6 +61,12 @@ class StereoCameraModel
    */
   void projectDisparityImageTo3d(const cv::Mat& disparity, cv::Mat& point_cloud,
                                  bool handleMissingValues = false) const;
+
+  /**
+   * \brief Project a 3d point to a rectified pixel 
+   */
+  void reproject3dToImage(const cv::Point3d & xyz, float disparity, cv::Point2i & left_uv_rect) const;
+
   static const double MISSING_Z;
   
   /**
diff --git a/image_geometry/src/stereo_camera_model.cpp b/image_geometry/src/stereo_camera_model.cpp
index 062edb746..9b557722e 100644
--- a/image_geometry/src/stereo_camera_model.cpp
+++ b/image_geometry/src/stereo_camera_model.cpp
@@ -137,4 +137,21 @@ void StereoCameraModel::projectDisparityImageTo3d(const cv::Mat& disparity, cv::
   cv::reprojectImageTo3D(disparity, point_cloud, Q_, handleMissingValues);
 }
 
+
+void StereoCameraModel::reproject3dToImage(const cv::Point3d & xyz, float disparity, 
+                                              cv::Point2i & left_uv_rect) const
+{
+  assert(initialized());
+
+  // [X Y Z W]^T = Q * [u v d 1]^T
+  // where [x, y, z] = (X/W, Y/W, Z/W)
+
+  double d = getDisparity(xyz.z);
+  double W = Q_(3,2)*d + Q_(3,3);
+
+  int u = (int) round((xyz.x * W - Q_(0, 3)) / Q_(0, 0)); 
+  int v = (int) round((xyz.y * W - Q_(1, 3)) / Q_(1, 1)); 
+
+  left_uv_rect = cv::Point2i(u, v);
+}
 } //namespace image_geometry