【传感器标定】路侧激光雷达和相机的标定(2)

接下来，路边激光雷达和相机的校准（1）。在上面，我提到我手里有几个相机和一个激光雷达。现在我需要对激光雷达和相机进行联合校准，并将激光雷达校准到相机。从数学原理上讲，激光雷达坐标系相机坐标系的外参矩阵包括旋转和平移。旋转以欧拉角表示，分别是滚动(roll),俯仰(pitch)和偏航(yaw)。位移为tx,ty和tz。在手动校准代码的过程中，通过按钮控制激光雷达的旋转欧拉角和平移，将其转化为外参矩阵。点云通过外参矩阵和相机内参投射到图像上。在可视化界面上不断调整雷达坐标系的位置，以匹配图像和雷达的参考目标，如人或车。

首先，通过以下代码初始化外参矩阵。这里使用Eigen库，是一个C 开源线性代数库为矩阵提供了快速的线性代数操作，包括解方程等功能。translate_matrix外参矩阵即将校准。

void CreateTranslateMatrix(const double delt_x,                            const double delt_y,                            const double delt_z,                            const double azimuth_degree,                            const double pitching_degree,                            const double roll_degree,                            Eigen::Matrix4f& translate_matrix) {     translate_matrix = Eigen::Matrix4f::Identity(4,4);     //matrix multiply     translate_matrix.block<3,3>(0,0) =             Eigen::AngleAxisf(roll_degree * M_PI / 180.0,Eigen::Vector3f::UnitX()).matrix()             *Eigen::AngleAxisf(pitching_degree * M_PI / 180.0,Eigen::Vector3f::UnitY()).matrix()             *Eigen::AngleAxisf(azimuth_degree * M_PI / 180.0,Eigen::Vector3f::UnitZ()).matrix();     translate_matrix(0,3) = delt_x;     translate_matrix(1,3) = delt_y;     translate_matrix(2,3) = delt_z;     //translate_matrix = coord_convert * translate_matrix; }

另外，在上一篇文章中，我们通过了"张正友标定法"将相机的内参矩阵和畸变参数保存在相机中xml文件中。这里暂时不考虑畸变的影响，先从xml相机内参加载在文件中。intrinsic和distCoffs分别是已校准的相机内参矩阵和畸变系数。

void LoadCameraParams(std::string file_name, cv::Mat& distCoeffs ,cv::Mat& intrinsic) {     cv::FileStorage file_storage(file_name,cv::FileStorage::READ | cv::FileStorage::FORMAT_XML);     std::cout << file_name << "打开" << file_storage.isOpened() << std::endl;      file_storage["distCoeffs"] >> distCoeffs;     file_storage["intrinsic"] >> intrinsic;     std::cout << "distCoeffs:\n" << distCoeffs << std::endl;     std::cout << "intrinsic:\n" << intrinsic << std::endl; }

通过激光雷达到相机的外参和相机本身的内参，根据我们已知的激光雷达到图像的坐标转换关系：

编写代码将点云投射到图片中，我们主要使用它pcl中提供好的pcl::transformPointCloud函数，简单分三步走：

void pcl::transformPointCloud(                      const pcl::PointCloud< PointT > &  cloud_in, //源点云                      pcl::PointCloud< PointT > &  cloud_out, ///转换后的点云                      const Eigen::Matrix4f  & transform ) //转换矩阵

Step1:首先调整激光雷达坐标轴的方向。以下图为参考。激光雷达和相机对坐标轴有不同的定义。例如，雷达前面是x轴，相机前面是z轴。我们需要交换坐标轴。

    {         xyz_swap << 0,-1,0,0,                 0,0,-1,0,                 1,0,0,0,                 0,0,0,1;     }     Eigen::Matrix4f translate_matrix = xyz_swap * translate;

Step2:将点云从激光雷达坐标系转换为相机坐标系。

pcl::transformPointCloud(*origin_pointcloud_ptr_, //input                          *transformed_pointcloud, //output                          translate_matrix);

Step3:将相机坐标系转换为归一化的相平面坐标系，并投影到像素平面。

    pcl::PointCloud<PointType>::Ptr normaled_pointcloud_ptr(new pcl::PointCloud<PointType>());     *normaled_pointcloud_ptr = *transformed_pointcloud;     for(int i = 0;i < normaled_pointcloud_ptr->size();  i) {         auto& pcl_point =  normaled_pointcloud_ptr->points.at(i);         //std::cout << "point:" << pcl_point << "\t";         pcl_point.x /= pcl_point.z;         pcl_point.y /= pcl_point.z;         pcl_point.z = 1;         //std::cout << "point:" << pcl_point << std::endl;     }      pcl::PointCloud<PointType>::Ptr image_pointcloud_ptr(new pcl::PointCloud<PointType>());     pcl::transformPointCloud(*normaled_pointcloud_ptr,                              *image_pointcloud_ptr,                              intrinsic_matrix );

在映射后的帮助下，忽略相机畸变的影响image_pointcloud_ptr激光点云可以画在图像上。

    for(int i = 0;i < image_pointcloud_ptr->size();  i) {         const auto& pcl_point =  image_pointcloud_ptr->points.at(i);         cv::Point point(pcl_point.x,pcl_point.y);         std::vector<cv::Point2f> undistortedPointVector;         cv::undistortPoints(std::vector<cv::Point2f>{cv::Point2f(pointx,point.y)},
                            undistortedPointVector,
                            camera_calib_param_.intrinsic,
                            camera_calib_param_.distCoeffs,
                            cv::noArray(),
                            camera_calib_param_.intrinsic);
        if(origin_pointcloud_ptr_->at(i).z > calib_info.pointcloud_height_threshold) {
            cv::circle(frame_,point,2,cv::Scalar(0, 255, 0),2);
        }
    }

由于外参参数初始设置的为0，一开始点云和图像肯定是对不准的。此时就需要手动调整"rool","pitch","yaw","tx","ty","tz"这六个参数。点云投影后失去了深度信息，因此无法在图像上区分不同深度的点云投影点。实际情况下，建议多放一些参照物，便于点云和图像的对齐。

        上面是自己手写的一个激光雷达到相机的联合标定的程序(涵主题思想)，主要是为了学习。实际上ROS的dynamic_reconfigure功能包提供了同样的功能，使用更加友好，包含带界面的参数调整，可以自己实验一下。

 

【参考文献】

1. https://blog.csdn.net/Yong_Qi2015/article/details/108898577

2.https://blog.csdn.net/zt1091574181/article/details/114838741

3.https://blog.csdn.net/weixin_45377028/article/details/109194773