image rectify: add camera info publisher

[cedricpradalier]

Context: I have been using the rectify node to to undistort images and save them in a bag along with the TF and the camera info of the source image. For space considerations, the original image is not stored in the bag. Later, I have an offline process that read the bag and needs to back project 3D points to the rectified images (to add colour to a point cloud).

I initially assumed that I could use the initial camera info, with zero distortion, but this led to very inaccurate reprojections. I ultimately found that my reprojection was correct if I used the intrinsics computed by getOptimalNewCameraMatrix, with alpha = 0. The fact that there is this free parameter alpha, which is not explicitly mentioned in the rectify node, would justify publishing a rectified CameraInfo in my opinion.

   sensor_msgs::CameraInfo ci; // original CameraInfo
   cv::Mat_<float> intrinsics(3,3,0.0f), distortion(1,ci.D.size(), 0.0f);
   intrinsics(0,0)=ci.K[0]; intrinsics(0,2)=ci.K[2];
   intrinsics(1,1)=ci.K[4]; intrinsics(1,2)=ci.K[5];
   intrinsics(2,2)=ci.K[8];
   for (size_t i=0;i<ci.D.size();i++) {
     distortion(0,i) = ci.D[i];
   }
   cv::Rect new_roi;
   cv::Size S(ci.width,ci.height);
   cv::Mat_<float> new_intrinsics = cv::getOptimalNewCameraMatrix(intrinsics,distortion,S,0,S,&new_roi);
   ci.K[0]=new_intrinsics(0,0); ci.K[2]=new_intrinsics(0,2);
   ci.K[4]=new_intrinsics(1,1); ci.K[5]=new_intrinsics(1,2);
   ci.K[8]=new_intrinsics(2,2);
   ci.D.clear();
   ci.D.assign(5,0.0);

I can implement it if this is deemed relevant but I only have access to a machine running humble right now.