将OpenCV立体声垫转换为sensor_msgs / CameraInfo

Question

我正在尝试将OpenCV的stereoCalibrate函数的输出转换为可发布的sensor_msgs/CameraInfo消息。运行校准代码

stereoRectify(M1, D1, M2, D2, img_size, R, T, R1, R2, P1, P2, Q, CALIB_ZERO_DISPARITY, -1, img_size, &roi1, &roi2);

填充CameraInfo期望的M1，M2，D1，D2，R1，R2，P1和P2矩阵。从我可以从文档中得知的信息：

• M = K
• D = D
• R = R
• P = P

其中1左边，2右边。

但是，该消息希望数据以boost::array的形式保持双精度。我认为它会像下面这样琐碎，但是我错了。有没有一种简单的方法可以将OpenCV Mat转换为boost :: array？

stereoRectify(M1, D1, M2, D2, img_size, R, T, R1, R2, P1, P2, Q, CALIB_ZERO_DISPARITY, -1, img_size, &roi1, &roi2);

leftCameraInfo.width = rightCameraInfo.width = img_size.width;
leftCameraInfo.height = rightCameraInfo.height = img_size.height;
leftCameraInfo.distortion_model = rightCameraInfo.distortion_model = sensor_msgs::distortion_models::PLUMB_BOB;
leftCameraInfo.D = D1;
rightCameraInfo.D = D2;
rightCameraInfo.K = M2;
leftCameraInfo.R = R1;
rightCameraInfo.R = R2;
leftCameraInfo.P = P1;
rightCameraInfo.P = P2;