The openni driver does this already. It doesn't just produce one depth image. Check [here](http://www.ros.org/wiki/openni_launch/Tutorials/QuickStart) and [here](http://www.ros.org/wiki/openni_launch#openni.launch). `image_raw` contains the raw uint16 depths in mm from the kinect. `image` contains float depths in m. And finally `image_rect` also contains float depths in m, but is rectified like you want. The driver even publishes `points`, a point cloud that puts all the coordinates together for you.
If you really need to know the formula, maybe just look at the code in the driver.
↧