【传感器标定】路侧激光雷达和相机的标定(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::Ptr normaled_pointcloud_ptr(new pcl::PointCloud());
    *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::Ptr image_pointcloud_ptr(new pcl::PointCloud());
    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 undistortedPointVector;
        cv::undistortPoints(std::vector{cv::Point2f(point.x,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