激光雷达点云投影到图像

 

本文旨在记录一些关于激光雷达点云投影到图像的心得。

一、数据准备

1.相机内参（intrinsic matrix），获得方法见ROS功能包camera_calibration标定相机内参_ros camera calibration-CSDN博客

2.激光雷达与相机的外参（extrinsic matrix），获取方法见cam_lidar_calibration

3.同时采集的点云和图像

二、相关代码

1.定义内参矩阵 Camera_inMat

        cv::Mat Camera_inMat = cv::Mat::zeros(3, 3, CV_64F);
        Camera_inMat.at<double>(0, 0) = 1452.65;
        Camera_inMat.at<double>(0, 2) = 969.05;
        Camera_inMat.at<double>(1, 1) = 1453.85;
        Camera_inMat.at<double>(1, 2) = 595.991;
        Camera_inMat.at<double>(2, 2) = 1;

2.定义外参矩阵 transformMatrix_l2c 

        double roll = -1.550963;
        double pitch = 0.013;
        double yaw = -1.560963;
        double tran_x = 0.15;
        double tran_y = -0.10;
        double tran_z = -0.22;

        cv::Mat R_x = (Mat_<double>(3, 3) << 1, 0, 0, 0, cos(roll), -sin(roll), 0, sin(roll), cos(roll));
        cv::Mat R_y = (Mat_<double>(3, 3) << cos(pitch), 0, sin(pitch), 0, 1, 0, -sin(pitch), 0, cos(pitch));
        cv::Mat R_z = (Mat_<double>(3, 3) << cos(yaw), -sin(yaw), 0, sin(yaw), cos(yaw), 0, 0, 0, 1);
        cv::Mat Rotation1 = R_z * R_y * R_x;
        Eigen::Matrix3d RotationMatrix;
        cv::cv2eigen(Rotation1, RotationMatrix);

        Eigen::Vector3d t(tran_x , tran_y , tran_z );
        Eigen::Matrix4d transformMatrix_c2l;
        transformMatrix_c2l.setIdentity();
        transformMatrix_c2l.block<3, 3>(0, 0) = RotationMatrix;
        transformMatrix_c2l.topRightCorner(3, 1) = t;
        Eigen::Matrix4d transformMatrix_l2c = transformMatrix_c2l.inverse();

3.计算投影点并绘图 pt

cv::Mat X(3,1,cv::DataType<double>::type);
cv::Mat Y(3,1,cv::DataType<double>::type);
for (pcl::PointCloud<MyPointType>::const_iterator it = cloud_roi->begin(); it != cloud_roi->end(); it++)
{
                        
        X.at<double>(0,0)=it->x;
        X.at<double>(1,0)=it->y;
        X.at<double>(2,0)=it->z;
          
        Y = Camera_inMat * X;

        cv::Point pt;
        pt.x = Y.at<double>(0, 0) / Y.at<double>(2, 0); 
        pt.y = Y.at<double>(1, 0) / Y.at<double>(2, 0);
            
        cv::circle(image, pt, 2, CV_RGB(255, 0 , 255 ), -1);  
}

另外，cam_lidar_calibration包中投影点的过程好像有问题，建议使用以上代码。

参考：

​​​​​自动驾驶视觉融合-相机校准与激光点云投影 - 知乎 (zhihu.com)

激光雷达和相机的联合标定的实现（源代码，干货满满） - 知乎 (zhihu.com)