Once you have the distance to the object, figuring out the XYZ coordinates is just a bit of fairly simple trigonometry. There may be a coordinate mapper class in the new SDK; I haven't played around with it much yet (and Orbbec is kind of light on the API documentation). But here's how to do it manually**...
First off, we need two constants: the vertical and horizontal fields of view of the sensor. Conveniently these are given on the product pages:
- 60° horizontal
- 49.5° vertical
From that and the image size we can get the angular size of each pixel:
- 60° / 640 = 0.09375 degrees per pixel horizontally = phiX
- 49.5° / 480 = 0.103125 degrees per pixel vertically = phiY
(If you don't need super-fine accuracy, you could probably round both of those to 0.1 degree per pixel, and it wouldn't throw things off too badly.)
Now is where the math comes in. I'm going to assume that (0,0,0) is the camera's position itself, and that the pixel we're measuring to is at position (i,j) in the image with a value of d (in known units, whether that's mm, cm, m, inches, furlongs, or whatever). I'm also going to assume that the image is 640x480 pixels. Finally, we define X and the horizontal distance to right of the camera, Y is the vertical distance above the camera, and Z is the distance measured straight out from the front of the camera. As Jackson points out below in this thread, the 16-bit value of the pixel at (i,j) is the distance Z in mm.
Calculating X:
First, we need to get the angle to the pixel. That's simply theta = phiX * (i-320); the degrees per pixel multiplied by the number of pixels from centre. This forms a right-angle triangle (drawn in crude ASCII art as a top-down view)
X
.---------. <-- the object we're measuring to in the real world
| /
|Z /
| / d
|**/ <-- theta is measured here
|/
. <-- the camera position
We know that tan(theta) = X/Z. Therefore we can calculate X by simply re-arranging the equation:
X = tan(theta) * Z
Calculating Y:
Exactly the same as above, only using phiY, and (j-240) instead:
theta = phiY * (j-240)
Y = tan(theta) * Z
EDIT, May 4:Updated the math to use Z in the calculations instead of d; I had previously assume the sensor reported the straight-line distance to the object d, not the perpendicular distance from the camera plane Z. This has been corrected.
