视觉融合-相机校准与激光点云投影

Github: 添加链接描述

像平面坐标系到像素坐标系的转换
3D空间中的点在图像平面上的投影与我们在实际的数字图像中看到的并不直接对应, 实际的数字图像由数千个图片像素组成. 因此我们需要实现从图像平面到数字图像上的投影.

在下图中相机中心是O点, 伴随着坐标系i,j,k. 其中k指向图像平面. 与k轴位置相交的C点被称为主点, 代表着图像平面的中心. 以主点为原点的右手平面坐标系o-xy为像平面坐标系.

因此, 在空间上将点P投影到图像平面上之后的第一步是减去主点坐标, 以使离散图像具有其自身的坐标系, 例如该坐标系的中心为图像平面的左下角.

转换过程的第二步是从公制坐标(m)转换为像素坐标. 为此, 我们可以使用校准程序提供的参数k和l, 这些参数将将米转换为像素, 并且可以轻松地将其集成到投影方程式中, 如下所示. 请注意, 在像素坐标系中, y轴的原点位于左上角, 并指向下方.

fk一般在转换矩阵中称为alpha, fl称为beta.

相机校准(camera calibration)
使用镜头虽然可以像针孔相机一样计算空间中的3D点通过镜头后在图像平面上的2D位置, 但是大部分镜头会将失真引入图像. 最常见的是变形称为“radial distortion”, 这是由于镜头的焦距在其直径上不一致. 一般情况镜头的放大效果根据相机中心(光轴)和通过镜头的光线之间的距离而变化. 如果放大倍数增加, 则产生的失真效果称为“pin cushion distortion”. 如果减小, 则称为“barrel distortion”. 使用广角镜如GOPRO时, 通常会发生” Barrel distortions“.

请记住, 如果我们需要通过图像得出关于目标物体（例如车辆）的空间位置, 一定要对相机进行畸变校准(calibration). 通常, 这是通过拍摄一组平面棋盘图案的图片来完成的, 可以从这些对象的已知几何形状中可靠地导出所有镜头和图像传感器参数. 消除相机图像失真的过程称为校正(rectification).
网上有很多关于如何使用OPENCV对相机进行畸变校准的教程
Python
C++
可以尝试下MATLAB的方法:MATLAB

请注意得到相机内参和畸变参数的过程是畸变校准(calibration), 拿着相机内参和畸变参数去消除相机图像失真的才叫校正(rectification). 下文的KITTI数据集已经给出了相机内参和畸变参数, 因此不需要再去拍棋牌图校准了.
在得到相机内参和畸变参数后可以通过OPENCV函数直接对图像进行校正, 你不需要了解复杂的公式.
畸变参数：注意有的文章中还会出现k4,k5,k6, 这三个参数基本用不到, 可以忽略.
[公式]

基于Python-opencv的畸变校准和校正可以遵循如下步骤:

// Converting an image, imported by cv2 or the glob API, to grayscale:
gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
// ret, corners = cv2.findChessboardCorners(gray, (8,6), None)
ret, corners = cv2.findChessboardCorners(gray, (8,6), None)
// Drawing detected corners on an image:
img = cv2.drawChessboardCorners(img, (8,6), corners, ret)
// Camera calibration, given object points, image points, and the shape of the grayscale image
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None)
// Undistorting a test image
dst = cv2.undistort(img, mtx, dist, None, mtx)

而c++代码相对复杂, 在实际校正这一块, 首先调用 cv: : initundistortcorrectfymap 来查找转换矩阵, 然后使用 cv: : : remap 函数执行转换. 详情看上文中的教程链接.
CMOS 相机
CMOS (Complementary Metal-oxide Semiconductor)技术具有几个优点：与CCD(Charge-Coupled Device)不同, CMOS芯片集成了放大器和A / D转换器, 这带来了巨大的成本优势. 对于CCD, 这些组件位于芯片外部. CMOS传感器还具有更快的数据读取速度, 更低的功耗, 更高的抗噪能力以及更小的系统尺寸. 由于这些优点, 在汽车应用中, 几乎所有相机都使用CMOS传感器.

值得注意的是对于自动驾驶汽车的相机, 分辨率实际上并不是那么重要, 目前大部分汽车相机像素在１百万到8百万像素这个区间内, 相对于手机动辄１亿的像素小的不能再小了. 对于汽车相机重要的是感光度, 因为像素量越小, 像素面积越大, 意味着更多的光子落入一个像素, 具有更好的弱光可见性.

LIDAR Cloud Projection

齐次坐标系
在开始激光点云投影前, 首先我们得讨论什么是齐次坐标. 齐次坐标是指一个用于投影几何里的坐标系统, 如同欧几里得坐标系一样. 那我们为什么需要齐次坐标呢?欧几里得坐标系不是很好吗?

上文提过, 在相机世界中, 3D外界点转换到2D图像像素点转换方程是

我们可以通过相机的内在参数 intrinsic camera parameters　实现这一转换. 但是激光雷达的外部坐标系和相机的外部坐标系位置是不一样的. 因此除了构成投影几何形状的固有相机内参数外, 我们还需要有关相机和激光雷达在公共参考坐标系中的位置和对齐方式的其他信息. 几乎每一家自动驾驶厂商的激光雷达和相机的放置位置都是不一样的, 这些信息往往只能厂商给出.
从激光雷达的位置移动到相机的位置涉及平移和旋转操作, 我们需要将其应用于每个3D点. 问题在于我们得到的投影方程的问题是涉及到 z 的除法, 这使得它们是非线性的(3维), 从而使我们无法将它们转化为更方便的矩阵向量形式.
避免此问题的一种方法是同时更改激光雷达和相机的坐标系, 从原始的欧几里得坐标系转换为齐次坐标系的形式. 在两个欧几里得坐标系之间来回移动是一种非线性操作, 但是一旦我们处于齐次坐标系中, 投影变换将变为线性, 因此可以表示为简单的矩阵向量乘法. 两个坐标系之间的转换如下图所示.

欧几里得坐标－>齐次坐标

n维欧氏坐标系中的一个点由具有n个分量的向量表示. 通过简单地将数字1添加为附加分量, 可以实现到(n+1)维同构坐标的转换. 该变换可以应用于图像坐标(左侧)以及场景坐标(右侧).

// 1. Convert current Lidar point into homogeneous coordinates and          store it in the 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;

齐次坐标－>欧几里得坐标

从齐次坐标转换回欧几里得坐标系, 只需要通过删除最后一个坐标并将前n个坐标除以第(n + 1)个坐标.

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);

内参矩阵
有了齐次坐标系的帮助, 我们可以用矩阵向量的形式表示投影方程了:

上述方程从左到右依次是齐次坐标系的相机投影几何转换矩阵/ 相机内参数矩阵K/ 齐次坐标点
相机内参数以矩阵形式排列, 可以方便地以紧凑的方式表示针孔相机模型的属性.
内参矩阵告诉你在外部世界的点在经过外参矩阵变换之后, 是如何继续经过摄像机的镜头、并通过针孔成像和电子转化而成为像素点的.

外参矩阵
现在我们已经实现了在相机坐标系中3D空间中的点P到2D像素平面中的点P’之间的映射.

但是激光雷达和相机的坐标系所在空间位置是不一样的, 它们都需要在车辆坐标系中进行校准.

如下是常见的车辆坐标系
车辆坐标系的原点放置在后轴中点下方的地面上, x轴指向行驶方向.

为了将在激光雷达传感器坐标系中测量的点投影到相机中, 我们需要在投影操作中添加其他转换, 以使我们能够将车辆坐标系中的点关联到相机坐标系, 反之亦然. 通常, 这种投影操作可以分为三个部分：平移, 旋转和缩放. 让我们依次看一下它们：

平移(translation)：
通过添加平移向量t到P, 使得P点线性平移到新位置P’.

在齐次坐标中, 我们可以使用大小为N的单位矩阵I连接平移向量t表示.

缩放(scale):
通过成分乘以尺度向量s实现缩放.

旋转(rotation)：
下图为点P在顺时针方向上的旋转的实现:

其中R被称为旋转矩阵. 在3D空间中, 点P的旋转是围绕x,y,z三个轴实现的, 因此可以表述为下面的旋转公式.

合在一起就是3D旋转公式.
齐次坐标的优点之一是, 它们可以通过级联几个矩阵-矢量乘法来轻松组合多个变换.
平移矩阵T和旋转矩阵R一起被称为外参矩阵. 它们共同描述了如何把点从世界坐标系转换到相机坐标系. 需要注意的是缩放成分S已集成到内矩阵中K, 因此不再是外参矩阵的一部分.

3D 投影方程
通过将各个外参矩阵和内参矩阵进行级联, 实现了3D激光雷达到2D图像平面上的投影.
实现3D投影需要从完成这些坐标系的变换:
激光雷达世界坐标系->相机坐标系(激光雷达和相机都在车辆坐标系中进行位置校准, 从而互相关联)->像平面坐标系->像素坐标系.
欧几里得坐标系->齐次坐标系->欧几里得坐标系.

KITTI数据集介绍
我们高速公路相机图片，点云数据来自KITTI数据集，下面大概介绍下要用的KITTI传感器配置、标定参数说明：

KITTI的传感器的布置

我们可以从KITTI的网站下载对应数据包的带有内参和外参的校准文件calib.zip.

calib.zip校准文件夹中包括三个子文件：calib_velo_to_cam.txt, calib_imu_to_velo.txt和calib_cam_to_cam.txt. calib_velo_to_cam.txt中的内容与Velodyne激光雷达和左灰色相机(编号为0)有关.
Velodyne激光雷达和左灰色相机(编号为0)有关.

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

其中矩阵R和T为我们提供了传感器设置的外参数.
R：3x3旋转矩阵
T：3x1平移向量

在文件 "calib_velo_to_cam.txt“ 中给了激光雷达到左边相机的平移、旋转参数：

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
…

P_rect_00：就是上面的投影K值。
R_rect_00： is the 3x3 rectifying rotation to make image planes co-planar, i.e. to align both cameras of the stereo rig 。

calib_time: 09-Jan-2012 13:57:47
corner_dist: 9.950000e-02
S_00: 1.392000e+03 5.120000e+02
K_00: 9.842439e+02 0.000000e+00 6.900000e+02 0.000000e+00 9.808141e+02 2.331966e+02 0.000000e+00 0.000000e+00 1.000000e+00
D_00: -3.728755e-01 2.037299e-01 2.219027e-03 1.383707e-03 -7.233722e-02
R_00: 1.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00
T_00: 2.573699e-16 -1.059758e-16 1.614870e-16
S_rect_00: 1.242000e+03 3.750000e+02
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
....
....
S_03: 1.392000e+03 5.120000e+02
K_03: 9.037596e+02 0.000000e+00 6.957519e+02 0.000000e+00 9.019653e+02 2.242509e+02 0.000000e+00 0.000000e+00 1.000000e+00
D_03: -3.639558e-01 1.788651e-01 6.029694e-04 -3.922424e-04 -5.382460e-02
R_03: 9.995599e-01 1.699522e-02 -2.431313e-02 -1.704422e-02 9.998531e-01 -1.809756e-03 2.427880e-02 2.223358e-03 9.997028e-01
T_03: -4.731050e-01 5.551470e-03 -5.250882e-03
S_rect_03: 1.242000e+03 3.750000e+02
R_rect_03: 9.998321e-01 -7.193136e-03 1.685599e-02 7.232804e-03 9.999712e-01 -2.293585e-03 -1.683901e-02 2.415116e-03 9.998553e-01
P_rect_03: 7.215377e+02 0.000000e+00 6.095593e+02 -3.395242e+02 0.000000e+00 7.215377e+02 1.728540e+02 2.199936e+00 0.000000e+00 0.000000e+00 1.000000e+00 2.729905e-03

其中00,01,02,03 代表相机的编号, 0表示左边灰度相机, 1右边灰度相机, 2左边彩色相机, 3右边彩色相机.
S_xx：1x2 矫正前的图像xx的大小
K_xx：3x3 矫正前相机xx的校准矩阵
D_xx：1x5 矫正前相机xx的失真向量形式是[k1, k2, p1, p2, k3] . k1, k2 和k3 是径向畸变系数, 而p1 和p2 是切向畸变系数.
R_xx：3x3 （外部）的旋转矩阵(从相机0到相机xx)
T_xx：3x1 （外部）的平移矢量(从相机0到相机xx)
S_rect_xx：1x2 矫正后的图像xx的大小
R_rect_xx：3x3 纠正旋转矩阵（使图像平面共面）即以立体方式对齐两个立体相机的摄像头（KITTI车辆中有两个Point Grey摄像头）
P_rect_xx：3x4 矫正后的投影矩阵, 包含上述的相机内参矩阵K.

fu/fv 为相机焦距, 一般相同, cu/cv为主点坐标（相对于成像平面）, s为坐标轴倾斜参数, 理想情况下为0. b(i) 代表其他相机相对于cam 0的偏移. 在从相机0投影到其他相机时需要.

以下等式说明了如何使用齐次坐标在相机0的图像平面上将空间中的3D激光雷达点X投影到2D像素点Y（使用Kitti自述文件中的表示法）：

RT_velo_to_cam * x ：是将Velodyne坐标中的点x投影到编号为0的相机（参考相机）坐标系中

R_rect00 *RT_velo_to_cam * x ：是将Velodyne坐标中的点x投影到编号为0的相机（参考相机）坐标系中, 再以参考相机0为基础进行图像共面对齐修正(这是使用KITTI数据集的进行3D投影的必要操作)

P_rect_00 * R_rect00 *RT_velo_to_cam * x ：是将Velodyne坐标中的点x投影到编号为0的相机（参考相机）坐标系中, 再进行图像共面对齐修正, 然后投影到相机0的像素坐标系中. 如果将P_rect_00改成P_rect_2, 也就是从参考相机0投影到相机2的像素坐标系中(其他相机相对与相机0有偏移b(i)).

以下为实现激光雷达3D点云投影到2D图像平面的步骤：

// 1. Convert current Lidar point into homogeneous coordinates and store it in the 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;

// 2. Then, apply the projection equation as detailed in lesson 5.1 to map X onto the image plane of the camera. 
// Store the result in Y.
Y = P_rect_00 * R_rect_00 * RT * X;

// 3. Once this is done, 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);

投影前处理
点云开始向像平面投影之前需要对其进行处理，需要把下面这些点云过滤掉：

… 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.
for(auto it=lidarPoints.begin(); it!=lidarPoints.end(); ++it) {
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
}
最后得到的投影结果如下：

点云投影之后，需要对点云进行聚类，表明那些点云属于属于一个目标。联合 YOLO目标检测 讲过的框图，可以将投影到框图内的点云聚类成一类，得到下面的结果：
关于框图的所包含的数据如下：
struct BoundingBox { // bounding box around a classified object (contains both 2D and 3D data)

int boxID; // unique identifier for this bounding box
int trackID; // unique identifier for the track to which this bounding box belongs

cv::Rect roi; // 2D region-of-interest in image coordinates
int classID; // ID based on class file provided to YOLO framework
double confidence; // classification trust

std::vector<LidarPoint> lidarPoints; // Lidar 3D points which project into 2D image roi
std::vector<cv::KeyPoint> keypoints; // keypoints enclosed by 2D roi
std::vector<cv::DMatch> kptMatches; // keypoint matches enclosed by 2D roi

};

一些问题：
发现框图往往会比实际物体大一些，这样的话，会把一些不属于目标物体的点，聚类到该物体上了。所以往往会在原来框图的基础上缩小一定比例，然后将投影在缩小之后的小框图上的点保留，下面分别是缩小10%，25%的结果。

实际运用过程中往往会缩小5-10%，会达到比较好的聚类效果。
框图交叉时，有些点云会同时属于多个框图，造成聚类错误，比如下面的情景：

常规的做法是，将同时属于多个框图的点云去除掉，避免的它们的干扰。

算法运用
算法的应用，详见github，SFND_3D_Object_Tracking 。
https://github.com/godloveliang/SFND_3D_Object_Tracking