NaN/Inf handling in head registration

[pgrice]

Occasionally the Pixel23d callback or head registration callback will die due to the presence of "Invalid (NaN, Inf) point coordinates given to radiusSearch!"

[pgrice]

After further investigation, I cannot actually get this to happen with the looking-to-point callback (using Pixel23d), but it appears to occur on the edges of the image for Head Registration. I seem to remember the depth image not covering the whole RGB image, such that in some spots, good depth information is not available for the RGB image, and this is possibly what is causing this issue.

[pgrice]

The issue occurs throughout the image, not only in the margins or in areas without good associated pointcloud data. The issue occurs even when the kinect is launched in the standard mode. Recent developments, then, seem to have produced this bug.