autoware标定工具进行固态激光雷达与相机的联合标定并运用标定结果进行投影(C++)-Toy模板网

这篇具有很好参考价值的文章主要介绍了autoware标定工具进行固态激光雷达与相机的联合标定并运用标定结果进行投影(C++)。希望对大家有所帮助。如果存在错误或未考虑完全的地方，请大家不吝赐教，您也可以点击"举报违法"按钮提交疑问。

本文主要介绍速腾聚创的RS-LIDAR-M1固态雷达激光与小觅相机左眼的联合标定过程，并介绍标定过程中的一些技巧与避雷，加快标定效率。最后给出运用标定结果进行投影的关键代码。

一、安装autoware（为了标定完成后的可视化，可选，本文并未用到）
参考https://blog.csdn.net/qq_41545537/article/details/109312868

二、安装autoware相机和激光雷达联合标定工具
参考https://blog.csdn.net/qq_43509129/article/details/109327157

三、进行标定
1.播放数据
播放事先采集好的标定数据包，命令如下：

rosbag play your_calibration_data.bag /rslidar_points:=/points_raw --loop

其中，参数/rslidar_points:=/points_raw是将原本的固态激光雷达的topic由/rslidar_points改成/points_raw，因为标定工具里面订阅的topic固定是/points_raw。参数–loop是指循环播放bag包。

2.启动步骤二中的标定工具
①　在标定工具所在工作空间打开终端，分别执行：

source devel/setup.bash
rosrun calibration_camera_lidar calibration_toolkit

②　在弹出的界面上选择相机的图像话题，小觅的话题是/mynteye/left/image_color或者/mynteye/left/image_mono，根据自己的情况选择。
③　再在弹出的界面选择 Camera->Velodyne 后出现如下界面： autoware标定工具进行固态激光雷达与相机的联合标定并运用标定结果进行投影(C++)

3.标定过程
①　Pattern Size处是标定板的尺寸，单位是m，根据实际情况填入。
②　Pattern Number处是标定板的格子数，根据实际情况填入。！！！注意：此处务必注意，按照你录制数据时拿标定板的姿态，前面填竖着的数量，后面填横着的数量（即竖✖横），并且填的是格子交点的数量，比如有7个格子，则填6。（经过我的测试，填成横✖竖误差会很大）！！！填完之后重启标定工具才能生效。
③　启动后正常情况下有图像和点云数据显示，右侧点云数据显示区由于视角问题看起来是一团黑色。
④　点击黑色区域，按下大键盘数字键“2”切换视角，可以看到有点云的俯视显示。
autoware标定工具进行固态激光雷达与相机的联合标定并运用标定结果进行投影(C++)
⑤　按下按键“B”，把背景调成白色。
⑥　长按按键“E”，把点云调正，根据该款固态激光雷达只有120度水平视场的特点，当左右各有一半点云的时候为正。
autoware标定工具进行固态激光雷达与相机的联合标定并运用标定结果进行投影(C++)
⑦　长按按键“W”，把点云视角调整为正视。
⑧　长按减号键“-”，把点云放大，直到可以看到标定板为止。现在整体应该是如下效果：

⑨　再次按下数字键“1”，切换视角。由于固态激光雷达分辨率太高，可能无法分辨出周围场景，如果分辨不出标定板，则按按键“P”，增大点云的点尺寸，一次不行，多按几次。最后效果如下（按了4次“P”键）：
autoware标定工具进行固态激光雷达与相机的联合标定并运用标定结果进行投影(C++)
上面几步操作务必完成。 整个显示界面其实分为四个部分，右上部分是实时显示点云的widget，左上部分是实时显示图像的widget；右下是显示截取的点云帧的widget，左下是显示截取的图像帧的widget。在执行后续步骤之前把右上部分的实时点云显示调整至一个非常容易选取的位置，可以让后续截取的点云易于选取，否则每一个截取的点云帧都要重复上述步骤的调整过程，浪费大量时间。

⑩　此时点云视角已经调整好，把图像界面放大，把图像完整地显示出来，效果如下：
autoware标定工具进行固态激光雷达与相机的联合标定并运用标定结果进行投影(C++)
⑪　此时开始捕捉标定帧。选取的准则是：当某一帧标定板在图像中和在点云中完整出现才算合格，如上图，图像中的标定板刚好显示完整，点云中的标定板显示完整，所以是合格的一帧。
⑫　对于合格的一帧，点击右上角的“Grab”按键，如果捕捉成功（即程序能检测到图像中的棋盘格），则会在界面下方显示，如下图所示：
autoware标定工具进行固态激光雷达与相机的联合标定并运用标定结果进行投影(C++)
⑬　重复捕捉大概二三十帧，理论上来说：越多帧，标定效果越好。尽量多捕捉不同姿态的标定板帧。
⑭　捕捉完成后，开始手动选择点云。如上图，标定板在图像中被检测到，我们需要手动选择点云中的标定板，以此形成对应关系求解变换矩阵。
⑮　选择的时候可以把右下方的点云界面调大，选择的模型是一个圆面。同理，为了让点云更能被分辨，按之前提到的方法把背景成白色，把点调大，直至标定板清晰可见。
⑯　点击鼠标左键选择标定板，尽量使圆面处于标定板的正中间，选择过后如果不满意，点击鼠标右键取消选择，重新再选，效果如下：
autoware标定工具进行固态激光雷达与相机的联合标定并运用标定结果进行投影(C++)
⑰　对捕捉的每一帧都进行这样的选取，直至所有帧选择完毕。
⑱　点击右上角的“Calibrate”按钮。
⑲　计算完成后，再点击右上角“Project”按钮，应该出现如下效果：
autoware标定工具进行固态激光雷达与相机的联合标定并运用标定结果进行投影(C++)
左侧的红点是用标定后的参数将右侧手动选择的标定板点投影到图像上的结果。完美情况下，红点应该出现在标定板上我们手动选择的位置。但是实际上只要大致对上就ok。
⑳　如果每一帧图像都能基本对上，则说明标定效果还行；如果误差较大，则说明标定失败，需要重新标定。
㉑　如果标定结束，则点击左上角的“Save”按钮，将标定结果保存，取名后存至文件夹。然后对之后弹出的两个界面，都选择“No”。
㉒　至此整个标定过程结束。

四、运用标定结果进行投影
标定完成后会得到一个yml格式的文件，里面有标定得到的外参、相机内参、相机畸变系数、图像尺寸、重投影误差，示例如下：

%YAML:1.0
---
CameraExtrinsicMat: !!opencv-matrix
   rows: 4
   cols: 4
   dt: d
   data: [ -5.8407131946527358e-03, -3.2811216518650155e-02,
       9.9944450078028035e-01, 1.9907201492930962e-01,
       -9.9986339451409767e-01, -1.5262476339699793e-02,
       -6.3442199462111493e-03, 6.9461651636179914e-02,
       1.5462159620299232e-02, -9.9934502594776853e-01,
       -3.2717590579534050e-02, -1.5654594735971714e-01, 0., 0., 0., 1. ]
CameraMat: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [ 1.0143281094389476e+03, 0., 6.3163571518821800e+02, 0.,
       1.0096395620868118e+03, 3.2954732055473158e+02, 0., 0., 1. ]
DistCoeff: !!opencv-matrix
   rows: 1
   cols: 5
   dt: d
   data: [ -6.3944068169403991e-03, -1.7957073252917993e-02,
       -1.3865038466759662e-02, 1.5781011631053978e-03,
       1.5292969053996039e-01 ]
ImageSize: [ 1280, 720 ]
ReprojectionError: 9.3588671201531770e-01

运用opencv里面的cv::projectPoints函数可以进行点云到图像之间的投影，下面是具体的代码。

	    // 下列代码段从标定结果文件中读取外参矩阵、内参矩阵、畸变矩阵，
	    // 其中外参矩阵中：前三行前三列是旋转矩阵、第四列是平移向量
	    cv::Mat extrinsic_mat, camera_mat,dist_coeff; //外参矩阵，内参矩阵，畸变矩阵
        cv::Mat rotate_mat,transform_vec; //旋转矩阵，平移向量
	    cv::FileStorage fs_read("your_calibration_file_path", cv::FileStorage::READ); //打开标定结果文件
	    fs_read["CameraExtrinsicMat"] >> extrinsic_mat; //从文件里读取4x4外参矩阵
	    fs_read["CameraMat"] >> camera_mat; //从文件里读取3x3相机内参矩阵
	    fs_read["DistCoeff"] >> dist_coeff; //从文件里读取5x1畸变矩阵
	    fs_read.release(); //关闭文件

	// 下列代码段从外参矩阵中读取旋转矩阵、平移向量
	rotate_mat=cv::Mat(3, 3, cv::DataType<double>::type); // 将旋转矩阵赋值成3x3矩阵
    for(int i=0;i<3;i++)
    {
        for(int j=0;j<3;j++)
        {
            rotate_mat.at<double>(i,j)=extrinsic_mat.at<double>(j,i); // 取前三行前三列
        }
    }
    transform_vec=cv::Mat(3, 1, cv::DataType<double>::type); //将平移向量赋值成3x1矩阵
    transform_vec.at<double>(0)=extrinsic_mat.at<double>(1,3);
    transform_vec.at<double>(1)=extrinsic_mat.at<double>(2,3);
    transform_vec.at<double>(2)=-extrinsic_mat.at<double>(0,3);

上述代码中，rotate_mat实际是取的外参矩阵前三行前三列的转置，transform_vec也不是按照外参矩阵第四列的顺序赋值。具体原因参考https://blog.csdn.net/qq_22059843/article/details/103022451

// 该函数实现点云到图像的投影，将点云上的点投影到图像上并在图像上显示，并将能够投影到图像的点按图像对应像素的颜色对点进行染色
// 输入参数分别为点云和图像
void projection(const pcl::PointCloud<pcl::PointXYZI>::Ptr&ccloud,cv::Mat&img)
{
    vector<cv::Point3f> points3d; //存储点云点的vcector，必须存储成cv::Point3f格式
    points3d.reserve(ccloud->size()+1); //先给points3d分配足够大的内存空间，避免push_back时频繁复制转移
    cv::Point3f point;
    for(int i=0;i<ccloud->size();i++)
    {
        point.x=ccloud->points[i].x;
        point.y=ccloud->points[i].y;
        point.z=ccloud->points[i].z;
        points3d.push_back(point); //逐个插入
    }
    vector<cv::Point2f> projectedPoints; //该vector用来存储投影过后的二维点，三维点投影后变二维
    
    // 投影核心函数
    // 第一个参数为原始3d点
    // 第二个参数为旋转矩阵
    // 第三个参数为平移向量
    // 第四个参数为相机内参矩阵
    // 第五个参数为投影结果
    cv::projectPoints(points3d,rotate_mat,transform_vec,camera_mat,dist_coeff,projectedPoints);
    
    pcl::PointXYZRGB point_rgb;
	//遍历投影结果
    for (int i = 0; i<projectedPoints.size(); i++)
    {
        cv::Point2f p = projectedPoints[i];
        point_rgb.x=ccloud->points[i].x;
        point_rgb.y=ccloud->points[i].y;
        point_rgb.z=ccloud->points[i].z;
        point_rgb.r=0;
        point_rgb.g=0;
        point_rgb.b=0;

		// 由于图像尺寸为720x1280，所以投影后坐标不在图像范围内的点不进行染色（默认黑色）
        if (p.y<720&&p.y>=0&&p.x<1280&&p.x>=0) 
        {
            point_rgb.r=int(img.at<cv::Vec3b>(p.y,p.x)[2]); //读取像素点的rgb值
            point_rgb.g=int(img.at<cv::Vec3b>(p.y,p.x)[1]);
            point_rgb.b=int(img.at<cv::Vec3b>(p.y,p.x)[0]);
        }
        rgb_cloud->push_back(point_rgb); //对于投影后在图像中的点进行染色后加入点云rgb_cloud
    }
    sensor_msgs::PointCloud2 ros_cloud; // 申明ros类型点云
    pcl::toROSMsg(*rgb_cloud,ros_cloud); // pcl点云转ros点云
    ros_cloud.header.frame_id="rslidar"; //
    cloud_pub.publish(ros_cloud); //cloud_pub是在函数外定义的一个ros点云发布器，将染色后的点云发布
    
    //再次遍历投影结果
    for (int i = 0; i<projectedPoints.size(); i++)
    {
        cv::Point2f p = projectedPoints[i];
        // 由于图像尺寸为720x1280，所以投影后坐标处于图像范围内的点才在图像中进行标示
        if (p.y<720&&p.y>=0&&p.x<1280&&p.x>=0)
        {
        	//标示的方式是在对应位置画圆圈
            cv::circle(img, p, 1, cv::Scalar(0, 0,255 ), 1, 8, 0);
        }
    }
    sensor_msgs::ImagePtr msg=cv_bridge::CvImage(std_msgs::Header(),"bgr8",img).toImageMsg(); //利用cv_bridge将opencv图像转为ros图像
    image_pub.publish(msg); //image_pub是在函数外定义的一个ros图像发布器，将标示后的图像发布

}

投影后的效果如下所示，其中，左边是图像，右边是染色后的点云（点云看起来像图像，因为点调大了）：
autoware标定工具进行固态激光雷达与相机的联合标定并运用标定结果进行投影(C++)
关于投影的大致数学细节，可以参考我之前的文章https://blog.csdn.net/qq_38222947/article/details/118220876文章来源地址https://www.toymoban.com/news/detail-475471.html