opencv - 如何将sensor_msgs/CameraInfo分配给cv::Mat

Question

我想知道将const sensor_msgs::CameraInfoConstPtr& camInfo_msg分配给cv::Mat的最聪明的方法是什么。

假设我希望将 ntrinsic相机矩阵float64 [9] K 的参数使用到 cv::Mat intrK中。

我写了下面的代码，它工作正常，但是我不知道这是否是最好的方法。我想知道其他人如何解决这个问题。

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

我问这个问题是因为当我将图像从 const sensor_msgs::ImageConstPtr& img_msg分配给cv::Mat时，我使用 cv_bridge，所以有一种聪明的方法可以做到这一点。然后，也许相机信息中也存在我从未见过的东西。
谢谢你的时间。

Answer 1

我发现最简单，最紧凑的方法是创建一个cv::Mat，它指向与sensor_msgs::CameraInfo::K相同的数据。您可以使用以下代码实现此目的:

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