三维激光点云到二维图像的投影

Camera系列文章

传感器融合是将多个传感器采集的数据进行融合处理,以更好感知周围环境;这里首先介绍Camera的相关内容,包括摄像头及图像知识基本介绍,OpenCV图像识别(特征提取,目标分类等),融合激光点云和图像进行TTC估计。

系列文章目录
1. 摄像头基础及校准
2. Lidar TTC估计
3. Camera TTC估计
4. OpenCV图像特征提取
5. 常见特征检测算法介绍
6. 常见特征描述符介绍
7. YOLOv3物体检测
8. 三维激光点云到二维图像的投影


文章目录


前言

我们可以使用摄像头或激光雷达跟踪物体。单一传感器的主要问题是,每个传感器都有其弱点,某些场景可能会可靠性降低,因此需要恰当的融合摄像头和激光雷达以提升跟踪过程效果。
本章中将介绍如何融合3D激光雷达点云数据和摄像头跟踪的特征点——将激光点云投影到Camera图像上。

齐次坐标系

相机坐标系下,3D空间点 P ⃗ \vec{P} P 到图像坐标系中 P ′ ⃗ \vec{P'} P′ 的映射公式如下:
三维激光点云到二维图像的投影
除了上述针孔相机模型几何投影对应的摄像头内参,也需要摄像头和雷达在同一坐标系下的相对位置和方位关系。激光雷达到摄像头的转换包含对每个点进行平移和旋转操作。使用线性变换(映射),3D点使用向量表示,旋转、平移、缩放操作、透视变换使用对向量的矩阵乘表示。3D空间点到2D图像的变换会丢掉Z轴(距离)信息,导致非线性,通常解决办法是引入齐次坐标系(Homogenous coordinate system)替代原有的欧氏坐标系(Euclidean coordinate system),如下所示,通过简答的矩阵-向量乘即可实现,具体如下:
三维激光点云到二维图像的投影

内外参(Intrinsic and Extrinsic Parameters)

内参

以下是针孔摄像头模型的投影方程。
三维激光点云到二维图像的投影
内参矩阵K的参数对成像的影响可参考如下网站:Dissecting the Camera Matrix, Part 3: The Intrinsic Matrix

外参

目前我们讨论了摄像头坐标系中的3D空间点P到2D图像平面的映射。但通常有多个传感器,每个传感器都有各自的坐标系,同时还有车辆坐标系。如下图所示,车辆坐标系的坐标原点位于后轴中心点的地面上,X轴指向行车方向,Y轴指向车辆左侧,Z轴向上,符合右手定则。围绕X、Y、Z的旋转为roll, pitch,yaw。
三维激光点云到二维图像的投影
当车辆同时配备激光雷达和摄像头时,就需要进行坐标系变换(映射),通常包括:①平移(如下图所示, P ⃗ \vec{P} P → P ′ ⃗ \vec{P'} P′ ),使用平移向量 t ⃗ \vec{t} t 。齐次坐标系中,可以使用N*N的单位矩阵I和平移向量 t ⃗ \vec{t} t 拼接得到矩阵T。其中N为 P ⃗ \vec{P} P 元素个数。
三维激光点云到二维图像的投影
②缩放因子,在其次坐标系中为 s ⃗ \vec{s} s
三维激光点云到二维图像的投影
③旋转矩阵R: P ′ ⃗ \vec{P'} P′ 逆时针方向旋转为正。
三维激光点云到二维图像的投影
三维激光点云到二维图像的投影
绕X、Y、Z轴的旋转可以用旋转矩阵R表示, R = R z ⋅ R y ⋅ R x R =R_z \cdot R_y \cdot R_x R=Rz​⋅Ry​⋅Rx​。
齐次坐标系的一大好处是可以通过拼接矩阵-向量乘实现多个矩阵乘的组合矩阵。
旋转矩阵R和平移向量 t ⃗ \vec{t} t 也称外参,指示点在不同坐标系的转换。实现从激光雷达坐标系到摄像头坐标系的转换后,还需要映射到图像平面上,因此还需要合并内参矩阵,齐次坐标系中变换如下。线性因子已合并只内参矩阵K中(焦距为相关参数)
三维激光点云到二维图像的投影
外参对物体成像的影响可参考如下网站:Perspective Camera Toy

KITTI传感器配置

本章中我们将使用KITTI数据集生成数据流,因此下面介绍KITTI车辆传感器配置及其坐标。KITTI采集车如下,配置2个前视摄像头,1个车顶Velodyne激光雷达(带IMU)。
三维激光点云到二维图像的投影
对于所有数据集,内外参校准文件都可以从KITTI网站下载。如下为“calib_velo_to_cam.txt”文件,Velodyne到左前摄像头的坐标变换矩阵,即外参。

calib_time: 15-Mar-2012 11:37:16
R: 7.533745e-03 -9.999714e-01 -6.166020e-04 1.480249e-02 7.280733e-04 -9.998902e-01 9.998621e-01 7.523790e-03 1.480755e-02
T: -4.069766e-03 -7.631618e-02 -2.717806e-01
…

为实现激光点云到摄像头图像的投影,也需要内参矩阵,存储在"calib_cam_to_cam.txt",如下:

calib_time: 09-Jan-2012 13:57:47
…
R_rect_00: 9.999239e-01 9.837760e-03 -7.445048e-03 -9.869795e-03 9.999421e-01 -4.278459e-03 7.402527e-03 4.351614e-03 9.999631e-01
P_rect_00: 7.215377e+02 0.000000e+00 6.095593e+02 0.000000e+00 0.000000e+00 7.215377e+02 1.728540e+02 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00 0.000000e+00
…

R_rect_00是3*3旋转校准,使图像平面共面,即对齐两个摄像头,左摄像头的一行像素对齐右侧摄像头的一行像素。P_rect_00包含摄像头内参(K),以下是3D激光点云的空间点 X到左摄像头2D图像点Y的坐标变换。
Y=P_rect_XX * R_rect_00 * (R|T)_velo_to_cam * X

激光点云到摄像头图像的投影

void projectLidarToCamera2()
{
  //load image from file
  cv::Mat img = cv::imread("./images/00000000.png");
  
  //load lidar points from file
  std::vector<LidarPoint> lidarPoints;
  readLidarPts("./dat/C51_LidarPts_0000.dat",lidarPoints);
  
  //store calibration data in Opencv matrices
  cv::Mat P_rect_00(3,4,cv::DataType<double>::type);//3×4 projection matrix after rectification
  cv::Mat R_rect_00(4,4,cv::DataType<double>::type);//3×3 rectifying rotation to make image planes co-planar
  cv::Mat RT(4,4,cv::DataType<double>::type);//rotation matrix and translation vector
  
  RT.at<double>(0,0) = 7.533745e-03;RT.at<double>(0,1) = -9.999714e-01;RT.at<double>(0,2) = -6.166020e-04;RT.at<double>(0,2) = -4.069766e-03;
  RT.at<double>(1,0) = 1.480249e-02;RT.at<double>(1,1) = 7.280733e-04;RT.at<double>(1,2) = -9.998902e-01;RT.at<double>(1,3) = -7.631618e-02;
  RT.at<double>(2,0) = 9.998621e-01;RT.at<double>(2,1) = 7.523790e-03;RT.at<double>(2,2) = 1.480755e-02;RT.at<double>(2,3) = -2.717806e-01;
  RT.at<double>(3,0) = 0.0;RT.at<double>(3,1) = 0.0;RT.at<double>(3,2) = 0.0;RT.at<double>(3,3) = 1.0;
  
  R_rect_00.at<double>(0,0) = 9.999239e-01;R_rect_00.at<double>(0,1) = 9.837760e-03;R_rect_00.at<double>(0,2) = -7.445048e-03;R_rect_00.at<double>(0,3) = 0.0;
  R_rect_00.at<double>(1,0) = -9.869795e-03;R_rect_00.at<double>(1,1) = 9.999421e-01;R_rect_00.at<double>(1,2) = -4.278459e-03;R_rect_00.at<double>(1,3) = 0.0;
  R_rect_00.at<double>(2,0) = 7.402527e-03;R_rect_00.at<double>(2,1) = 4.351614e-03;R_rect_00.at<double>(2,2) = 9.999631e-01;R_rect_00.at<double>(2,3) = 0.0;
  R_rect_00.at<double>(3,0) = 0.0;R_rect_00.at<double>(3,1) = 0.0;R_rect_00.at<double>(3,2) = 0.0;R_rect_00.at<double>(3,3) = 1.0;

  P_rect_00.at<double>(0,0) = 7.215377e+02;P_rect_00.at<double>(0,1) = 0.000000e+00;P_rect_00.at<double>(0,2) = 6.095593e+02;P_rect_00.at<double>(0,3) = 0.000000e+00;
  P_rect_00.at<double>(1,0) = 0.000000e+00;P_rect_00.at<double>(1,1) = 7.215377e+02;rect_00.at<double>(1,2) = 1.728540e+02;P_rect_00.at<double>(1,3) = 0.000000e+00;
  P_rect_00.at<double>(2,0) = 0.000000e+00;P_rect_00.at<double>(2,1) = 0.000000e+00;P_rect_00.at<double>(2,2) = 1.000000e+00;P_rect_00.at<double>(2,3) = 0.000000e+00;

  //project lidar points
  cv::Mat visImg=img.clone();
  cv::Mat overlay = visImg.clone();

  cv::Mat X(4,1,cv::DataType<double>::type);
  cv::Mat Y(4,1,cv::DataType<double>::type);
  for(auto it=lidarPoints.begin();it!=lidarPoints.end();++it)
  {
    // Filtering Lidar Points
    // Filter Criterion
    // positioned behind the Lidar sensor and thus have a negative x coordinate.
    //too far away in x-direction and thus exceeding an upper distance limit.
    //too far off to the sides in y-direction and thus not relevant for collision detection
    //too close to the road surface in negative z-direction.
    //showing a reflectivity close to zero, which might indicate low reliability.
    float maxX = 25.0, maxY = 6.0, minZ = -1.4; 
    if(it->x > maxX || it->x < 0.0 || abs(it->y) > maxY || it->z < minZ || it->r<0.01 )
    {
       continue; // skip to next point
    }
    
    // convert each 3D point into homogeneous coordinates and store it in a 4D variable X
    X.at<double>(0,0)=it->x;
    X.at<double>(1,0)=it->y;
    X.at<double>(2,0)=it->z;
    X.at<double>(3,0)=1;
 
    //apply the projection equation to map X onto the image plane of the camera. Store the result in Y
    Y=P_rect_00*R_rect_00*RT*X;
    // transform Y back into Euclidean coordinates and store the result in the variable pt
    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);

    float val=it->x;
    float maxVal=20.0;
    int red=min(255,(int)(255*abs((val-maxVal)/maxVal)));
    int green=min(255,(int)(255*(1-abs((val-maxVal)/maxVal))));
    cv::circle(overlay, pt,5,cv::Scalar(0,green,red),-1);    
  }

  float opacity=0.6;
  cv::addWeighted(overlay,opacity,visImg,1-opacity,0,visImg);

  string windowName = "Lidar data on image overlay";
  cv::namedWindow(windowName,3);
  cv::imshow(windowName,visImg);
  cv::waitKey(0);  //wait for key to be pressed
}

三维激光点云到二维图像的投影

上一篇:PyQt5 *绘画,支持导入可变大小的图片和保存图片、插入可变矩形、椭圆、箭头,支持文本输入和再编辑,支持图层变动等


下一篇:Qt编写自定义控件30-颜色多态按钮