ROS中生成CameraInfo消息

前言

由于某个第三方代码需要接受CameraInfo消息，我换了一个相机以后，需要自己发布CameraInfo消息。网上搜了半天，很少有介绍CameraInfo这些数据都是怎么来的的资料，可能大部分都是直接生成的吧，像我这样需要自己计算的可能不多。折腾了半天，也算是找到了方法。特做记录总结。转载请注明出处。

分析

首先看一下现有的CameraInfo有哪些数据：

rostopic echo /camera_info

可以看到，除了header外有 D, K, R, P等参数。这几个都是什么？
这就需要看一下在ros官网的说明：
http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/CameraInfo.html

一篇关于这个官方说明的资料在这里：https://github.com/dimatura/ros_vimdoc/blob/master/doc/ros-camera-info.txt

可以看出，D是畸变参数，K是相机内参矩阵，R是旋转，P是（校正后的）投影矩阵。

计算

进过相机标定后，一般都能够获得K和D，但R和P怎么设置？

一般我们不需要图像进行旋转，所以R给单位阵就好。P可以看到，P和K并不是完全相同。可以说，如果是没有畸变的图像，那么P左半部分和K是相同的。那么P该如何计算？找到了这个帖子：https://answers.ros.org/question/119506/what-does-projection-matrix-provided-by-the-calibration-represent/（可能访问不了，贴图到这里）

所以可以用OpenCV的函数getOptimalNewCameraMatrix()计算。函数API说明
只需要设置相机内参矩阵、畸变参数、长宽，alpha是自行设定，但根据ROS中cameraInfo，应该设置为0。

代码

测试代码如下：

void ex(void){
    cout << "camera info parameter..." << endl;

    Mat dst;
    Mat cameraMatrix = Mat::zeros(Size(3,3), CV_64FC1);
    Mat distCoeffs(Size(1,4), CV_64FC1);
    cameraMatrix.at<double>(0,0) = 199.0923665423;
    cameraMatrix.at<double>(0,2) = 132.1920713777;
    cameraMatrix.at<double>(1,1) = 198.8288204701;
    cameraMatrix.at<double>(1,2) = 110.7126600011;
    cameraMatrix.at<double>(2,2) = 1;

    distCoeffs.at<double>(0) = -0.36843631179;
    distCoeffs.at<double>(1) = 0.150947243556;
    distCoeffs.at<double>(2) = -0.00029613053;
    distCoeffs.at<double>(3) = -0.00075943173;
    // distCoeffs.at(4) = 0.0;
    dst = getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, Size(240, 180), 1);
    cout << dst << endl;
}

测试结果看，自己计算的P和原始数据的P基本相同吧。

完整代码

由于ROS的cameraInfo下D/R/P/K并不是opencv的Mat，所以需要进行转换。完整代码如下：

sensor_msgs::CameraInfo getCameraInfo(void){        // extract cameraInfo.
    sensor_msgs::CameraInfo cam;
    string camera_model_file = "/home/larrydong/thesis_ws/src/thesis/process/settings/parameters.xml";
    FileStorage fs(camera_model_file, FileStorage::READ);
    if(!fs.isOpened()){
        ROS_ERROR_STREAM("Cannot load camera info file. From: " << camera_model_file);
    }
    Mat k, d;
    fs["camera_matrix"] >> k;
    fs["distortion_coefficient"] >> d;

    vector<double> D{d.at<double>(0), d.at<double>(1), d.at<double>(2), d.at<double>(3), d.at<double>(4)};
    boost::array<double, 9> K = {
        k.at<double>(0),        k.at<double>(1),        k.at<double>(2),
        k.at<double>(3),        k.at<double>(4),        k.at<double>(5),
        k.at<double>(6),        k.at<double>(7),        k.at<double>(8),
    };
    
    Mat p = getOptimalNewCameraMatrix(k, d, Size(1280, 800), 0);        // get rectified projection.
    boost::array<double, 12> P = {
        p.at<double>(0,0),        p.at<double>(0,1),        p.at<double>(0,2),        p.at<double>(0,3),
        p.at<double>(1,0),        p.at<double>(1,1),        p.at<double>(1,2),        p.at<double>(1,3),
        p.at<double>(2,0),        p.at<double>(2,1),        p.at<double>(2,2),        p.at<double>(2,3)
    };
    boost::array<double, 9> r = {1, 0, 0, 0, 1, 0, 0, 0, 1};

    cam.height = 800;
    cam.width = 1280;
    cam.distortion_model = "plumb_bob";
    cam.D = D;
    cam.K = K;
    cam.P = P;
    cam.R = r;
    cam.binning_x = 0;
    cam.binning_y = 0;
    return cam;
}

int main(int argc, char **argv){ 
	ros::init(argc, argv, "image_info_pub");
	ros::NodeHandle n;
	const string camurl = "file:///home/yjy/calibrationdata/ost.yaml";
	camera_info_manager::CameraInfoManager caminfo(n, "camera_info", camurl);
	ros::Publisher pub_cam_info = n.advertise<sensor_msgs::CameraInfo>("camera_info", 1);
	ros::Rate r(10);
	while(ros::ok()){
		pub_cam_info.publish(caminfo.getCameraInfo());
		ros::spinOnce();
		r.sleep();
	}
	return 0;
}