Convert CameraInfo K & D to OpenCV format convenience functions?

edit

This is a first pass at adapting the image_geometry code: https://github.com/lucasw/image_manip... - very little testing so far though:

std::vector image_points;
cv::Matx33d intrinsic;
cv::Mat distortion;
image_manip::cameraInfoToCV(msg, intrinsic, distortion);
cv::Mat rvec, tvec = cv::Mat_<double>::zeros(3, 1);
cv::Rodrigues(R_.t(), r_vec);
cv::projectPoints(vector_of_3d_points_in_camera_frame,
    rvec, tvec, intrinsic, distortion, image_points);

Copy of the function code:

// adapted from https://github.com/ros-perception/vision_opencv/blob/noetic/image_geometry/src/pinhole_camera_model.cpp fromCameraInfo
void cameraInfoToCV(const sensor_msgs::CameraInfo::ConstPtr& msg,
    cv::Matx33d& K_,  // Describe current image (includes binning, ROI)
    cv::Mat_<double>& D_)  // Unaffected by binning, ROI - they are in ideal camera coordinates
{
  // TODO(lucasw) this can't be const
  auto cam_info = *msg;

  cv::Matx34d P_;  // Describe current image (includes binning, ROI)

  int d_size = cam_info.D.size();
  D_ = (d_size == 0) ? cv::Mat_<double>() : cv::Mat_<double>(1, d_size, cam_info.D.data());
  auto K_full_ = cv::Matx33d(&cam_info.K[0]);
  // TODO(lucasw) not actually using P_full_
  auto P_full_ = cv::Matx34d(&cam_info.P[0]);

  // Binning = 0 is considered the same as binning = 1 (no binning).
  const uint32_t binning_x = cam_info.binning_x ? cam_info.binning_x : 1;
  const uint32_t binning_y = cam_info.binning_y ? cam_info.binning_y : 1;

  // ROI all zeros is considered the same as full resolution.
  sensor_msgs::RegionOfInterest roi = cam_info.roi;
  if (roi.x_offset == 0 && roi.y_offset == 0 && roi.width == 0 && roi.height == 0) {
    roi.width  = cam_info.width;
    roi.height = cam_info.height;
  }

  // If necessary, create new K_ and P_ adjusted for binning and ROI
  /// @todo Calculate and use rectified ROI
  const bool adjust_binning = (binning_x > 1) || (binning_y > 1);
  const bool adjust_roi = (roi.x_offset != 0) || (roi.y_offset != 0);

  if (!adjust_binning && !adjust_roi) {
    K_ = K_full_;
    P_ = P_full_;
  } else {
    K_ = K_full_;
    P_ = P_full_;

    // ROI is in full image coordinates, so change it first
    if (adjust_roi) {
      // Move principal point by the offset
      /// @todo Adjust P by rectified ROI instead
      K_(0,2) -= roi.x_offset;
      K_(1,2) -= roi.y_offset;
      P_(0,2) -= roi.x_offset;
      P_(1,2) -= roi.y_offset;
    }

    if (binning_x > 1) {
      const double scale_x = 1.0 / binning_x;
      K_(0,0) *= scale_x;
      K_(0,2) *= scale_x;
      P_(0,0) *= scale_x;
      P_(0,2) *= scale_x;
      P_(0,3) *= scale_x;
    }
    if (binning_y > 1) {
      const double scale_y = 1.0 / binning_y;
      K_(1,1) *= scale_y;
      K_(1,2) *= scale_y;
      P_(1,1) *= scale_y;
      P_(1,2) *= scale_y;
      P_(1,3) *= scale_y;
    }
  }
}