How to assign the sensor_msgs/CameraInfo to cv::Mat

Question

I would like to know what is the smartest way to assign const sensor_msgs::CameraInfoConstPtr& camInfo_msg, to cv::Mat.

Let's say that I wish use the parameters Intrinsic camera matrix float64[9] K into cv::Mat intrK.

I wrote the following code, it works fine, however I have no idea if it is the best way to do it. I would like to know how others solve this problem.

void Class::Callback(const sensor_msgs::CameraInfoConstPtr& camInfo_msg){
  // receive the parameters
 sensor_msgs::CameraInfoPtr cameraInfoMsg = boost::make_shared<sensor_msgs::CameraInfo>(*camInfo_msg);
  // copy parameters
 cv::Mat intrK = cv::Mat::zeros(3, 3, CV_64FC1);
 std::vector<double> v_intrK;
  // initialize cv::Mat of correct size and vector
 v_intrK.assign((cameraInfoLMsg->K).begin(), (cameraInfoLMsg->K).end());
  // assign parameters to vector
 for (std::size_t i = 0; i < intrK.rows; i++)
   for (std::size_t j = 0; j < intrK.cols; j++)
     intrK.at<double>(i, j) = v_intrK[i*3+j];
  // assign vector to cv::Mat

I ask this question because when I assign the image from const sensor_msgs::ImageConstPtr& img_msg to cv::Mat I use the cv_bridge, so there is smart way to do it. Then maybe in camera info exists also something that I have never seen. Thanks for your time.

Answer 1

The easiest and most compact way I found is to create a cv::Mat that points to the same data as sensor_msgs::CameraInfo::K. You can achieve this with the following code:

void Class::Callback(const sensor_msgs::CameraInfoConstPtr& camInfo_msg) {
  cv::Mat intrK(3, 3, CV_64FC1, (void *) camInfo_msg->K.data());
}