radial point cloud nodes assume unrectified images

[madsherlock]

The depth_image_proc/point_cloud_xyz_radial and depth_image_proc/point_cloud_xyz_radial nodes apply a projective transformation to depth images to produce point clouds.

This projective transformation is calculated from the camera matrix K from the camera_info topic. In my opinion, this transformation should be calculated from the projection matrix P from the camera_info topic, if available, as well as from the rectification rotation matrix R. These can be provided to the cv::undistortPoints() function from within the initMatrix() method of the nodelet.

It would allow the camera_info topic to be fully utilized; I need a translation contained within the projection matrix to be applied to the point cloud before it is in the right frame.

Is the current behavior (only using K) intended or a bug?

[mikeferguson]

I started to look into this, and it turned into a bit of a rabbit hole:

For starters - let's review which nodes use what (I'm only looking at the rolling branch for now, because any changes are likely to only go there):

• xyz, xyzi, xyzrgb use image_geometry::PinholeCameraModel - which means they actually use the P matrix
• xyz_radial, xyzi_radial, xyzrgb_radial use a transform matrix derived from K and D.

Per the sensor_msgs/CameraInfo message:

• K is the "Intrinsic camera matrix for the raw (distorted) images"
• P "specifies the intrinsic (camera) matrix of the processed (rectified) image"
• R is the "Rectification matrix (stereo cameras only)" and is "A rotation matrix aligning the camera coordinate system to the ideal stereo image plane so that epipolar lines in both stereo images are parallel."

There is quite a bit of good discussion on this answers.ros.org - but basically, it agrees with CameraInfo:

• If you are subscribing to image_raw, use K and D
• If you are subscribing to image_rect, use P and set D to the identity matrix.

Looking back at the earlier history, xyz_radial and xyzi_radial were the first ones implemented - and they subscribe to image_raw - which would be appropriate when using K and D (as currently done in the code). It looks like when xyzrgb_radial was added it subscribes to an image_rect topics but still uses K and D matrix (that is inconsistent). To further complicate things, I just recently renamed image_raw->image_rect in those two nodes (#900 (comment)).

So, I think I should revert the image_raw->image_rect in xyz_radial and xyzi_radial, and instead flip xyzrgb_radial to use image_raw - and add some docs that note these expect raw, rather than rectified images.

This doesn't actually sort out the original issue mentioned here - which I presume to be a somewhat differently configured camera (one that is already rectified).

[mikeferguson]

PR to update topics: #906