激光雷达和相机联合标定
温馨提示：这篇文章已超过421天没有更新，请注意相关的内容是否还可用！
所有内容仅供个人学习记录
文章目录
- 一、相机标定原理
- 1.1 相机成像原理
- 1.1.1 世界坐标系到相机坐标系
- 1.1.2 相机坐标系与图像坐标系
- 1.1.3 图像坐标系与像素坐标系
- 1.1.4 总结
- 1.2 畸变
- 1.2.1 径向畸变
- 1.2.2 切向畸变
- 1.2.3 畸变公式
- 1.3 相机标定
- 1.3.1 张正友标定法
- 1.3.2 张正友标定法的整体流程
- 1.3.3 张正友标定法模型
- 1.3.4 模型求解
- 1.3.4.1.求解单应矩阵H
- 二、激光雷达Lidar标定
- 2.1.1 γ γ γ侧偏角的标定
- 2.1.2 α俯仰角的标定
- 2.2 相机与激光雷达联合标定
- 意义
一、相机标定原理
相机内参是相机坐标系转换到图像像素坐标系
相机内参是世界坐标系转换到相机坐标系
1.1 相机成像原理
相机的成像过程涉及到四个坐标系：世界坐标系、相机坐标系、图像坐标系、像素坐标系。
四个坐标系：
1） 世界坐标系(world coordinate system)
现实世界的三维坐标系，是现实世界的物理模型，单位为m。
2） 相机坐标系(camera coordinate system):
以镜头硬件的光电系统为基础，建立的坐标系，为了从相机的角度描述物体位置而定义，首先建立相机的坐标系，作为沟通现实世界和图像/像素坐标系的中间桥梁，单位为m；世界坐标系到相机坐标系只涉及旋转和平移，所以是刚体变换，不涉及形变。
** 3) 图像坐标系(image coordinate system)：**
为了描述成像过程中物体从相机坐标系到图像坐标系的投影透射关系而引入，方便进一步得到像素坐标系下的坐标，单位为m；从相机坐标系到图像坐标系，属于透视投影关系，从3D转换到2D。
** 4) 像素坐标系(pixel coordinate system)：**
为了描述物体成像后的像点在数字图像上（相片）的坐标而引入，是真正从相机内读取到的信息所在的坐标系，单位为个（像素数目）；以图像左上角为原点建立以像素为单位的直接坐标系u-v。像素的横坐标u与纵坐标v分别是在其图像数组中所在的列数与所在行数。（在OpenCV中u对应x，v对应y)
成像过程：
世界坐标系→（刚体变换）→相机坐标系→（透视投影）→理想图像坐标系→（畸变矫正）→真实图像坐标系→（数字化）→数字化图像坐标系
1.1.1 世界坐标系到相机坐标系
绕Z轴旋转的坐标关系如下：
所以从世界坐标系到相机坐标系的转换关系如下所示：
用齐次坐标系表示：
1.1.2 相机坐标系与图像坐标系
从相机坐标系到图像坐标系，属于透视投影关系，从3D转换到2D。
1.1.3 图像坐标系与像素坐标系
像素坐标系和图像坐标系都在成像平面上，只是各自的原点和度量单位不一样。图像坐标系的原点为相机光轴与成像平面的交点，通常情况下是成像平面的中点或者叫principal point。图像坐标系的单位是mm，属于物理单位，而像素坐标系的单位是pixel，我们平常描述一个像素点都是几行几列。所以这二者之间的转换如下：其中dx和dy表示每一列和每一行分别代表多少mm，即1pixel=dx mm
那么通过上面四个坐标系的转换就可以得到一个点从世界坐标系如何转换到像素坐标系的。
1.1.4 总结
相机内参
相机内参分别为：
f：焦距，单位毫米
fx：使用像素来描述x轴方向焦距的长度
fy：使用像素来描述y轴方向焦距的长度
u0，v0：主点坐标（相对于成像平面），单位也是像素
γ：为坐标轴倾斜参数，理想情况下为0
内参矩阵是相机自身的属性，通过标定就可以得到这些参数。
外参矩阵：
相机的外参是世界坐标系在相机坐标系下的描述，R是旋转参数是每个轴的旋转矩阵的乘积，T是平移参数。
参考：https://blog.csdn.net/qq_39607707/article/details/125558954
1.2 畸变
畸变分为径向畸变和切向畸变。
径向畸变发生在相机坐标系转图像物理坐标系的过程中。而切向畸变是发生在相机制作过程，其是由于感光元平面跟透镜不平行。其如下：
1.2.1 径向畸变
1.2.2 切向畸变
2.切向畸变：产生的原因透镜不完全平行于图像平面，这种现象发生于成像仪被粘贴在摄像机的时候。下面图片来自于《学习opencv》p413。
是由于焦平面上不同区域对图像的放大率不同形成的画面扭曲变形的现象， 这种变形的程度从画面中心至画面边缘依次递增， 主要在画面边缘反映比较明显。
相机的畸变可以由非线性模型来表示， 由于公式比较复杂， 对于推导过程， 此处将
不再赘述， 最后给出简化后的畸变模型：
1.2.3 畸变公式
径向畸变公式：
切向畸变公式：
其中， （ x , y ） （x,y） （x,y）， （ x ^ , y ^ ） （\widehat{x},\widehat{y}） （x ,y ）分别为理想的无畸变的归一化的图像坐标、畸变后的归一化图像坐标，r为图像像素点到图像中心点的距离，即 r 2 = x 2 + y 2 r^{2}=x^{2}+y^{2} r2=x2+y2
相机标定的第二个目的就是获得相机的畸变参数，如上式中的 k 1 , k 2 , k 3 , p 1 , p 2 k_{1},k_{2},k_{3},p_{1},p_{2} k1,k2,k3,p1,p2
相机标定：
相机标定是后续计算机视觉工作的前提，其标定的目的是为了确定相机的内参、外参以及畸变参数。标定的方法有：线性标定法、非线性优化标定法、两步标定法。线性标定法：运算速度快但是标定的精度不高，因为相机畸变都是非线性的。非线性优化标定法：标定精度高，但模型复杂。两步标定法有：Tsai的经典两步法和张正友的标定方法。
1.3 相机标定
2、相机的标定方法与意义：
什么是摄像机标定
在图像测量过程以及机器视觉应用中，为确定空间物体表面某点的三维几何位置与其在图像中对应点之间的相互关系，必须建立摄像机成像的几何模型,这些几何模型参数就是摄像机参数。在大多数条件下这些参数必须通过实验与计算才能得到，这个求解参数的过程就称之为摄像机标定。
相机标定的意义
无论是在图像测量或者机器视觉应用中，摄像机参数的标定都是非常关键的环节，其标定结果的精度及算法的稳定性直接影响摄像机工作产生结果的准确性。因此，做好摄像机标定是做好后续工作的前提，是提高标定精度是科研工作的重点所在。其标定的目的就是为了相机内参、外参、畸变参数。
相机标定原理
针对针孔相机模型，只要内参矩阵和外参矩阵就可以唯一的确定相机模型。这个过程就称为相机标定。相机标定的目的是为了获得相机的内参矩阵和外参矩阵。相机标定的内参主要包括焦距、像主点坐标、畸变参数。
通过世界坐标集 X i , Y i , Z i X_{i},Y_{i},Z_{i} Xi,Yi,Zi以及它们在图像平面上的投影坐标集 ( u i , v i ) (u_{i},v_{i}) (ui,vi)，计算相机投影 矩阵M中的 11个未知参数。
相机模型：
1.3.1 张正友标定法
张正友标定法利用如下图所示的棋盘格标定板，在得到一张标定板的图像之后，可以利用相应的图像检测算法得到每一个角点的像素坐标 ( u , v ) ( u , v ) (u,v) 。
张正友标定法将世界坐标系固定于棋盘格上，则棋盘格上任一点的物理坐标W = 0 ，由于标定板的世界坐标系是人为事先定义好的，标定板上每一个格子的大小是已知的，我们可以计算得到每一个角点在世界坐标系下的物理坐标 ( U , V , W = 0 ) ( U , V , W = 0 ) (U,V,W=0) 。
我们将利用这些信息：每一个角点的像素坐标( u , v ) 、每一个角点在世界坐标系下的物理坐标( U , V , W = 0 )，来进行相机的标定，获得相机的内外参矩阵、畸变参数。
1.3.2 张正友标定法的整体流程
1.3.3 张正友标定法模型
2D图像点：
3D图像点：
描述空间坐标到图像坐标的映射:
s s s : 世界坐标系到图像坐标系的尺度因子
K K K : 相机内参矩阵
( u 0 , v 0 ) ( u 0 , v 0 ) (u0,v0): 像主点坐标
α , β α , β α,β: 焦距与像素横纵比的融合
γ γ γ : 径向畸变参数
1.3.4 模型求解
张正友标定法标定相机的内外参数的思路如下：
- 求解内参矩阵与外参矩阵的积（单应矩阵H）；
- 求解内参矩阵；
- 求解外参矩阵。
- 求解畸变系数；
- 利用L-M（Levenberg-Marquardt）算法对上述参数进行优化。
1.3.4.1.求解单应矩阵H
a) 打印一张棋盘格标定图纸，将其贴在平面物体的表面。
b) 拍摄一组不同方向棋盘格的图片，可以通过移动相机来实现，也可以移动标定图片来实现。
c) 对于每张拍摄的棋盘图片，检测图片中所有棋盘格的特征点（角点，也就是下图中黑白棋盘交叉点，中间品红色的圆圈内就是一个角点）。我们定义打印的棋盘图纸位于世界坐标系Zw=0的平面上，世界坐标系的原点位于棋盘图纸的固定一角（比如下图中黄色点）。像素坐标系原点位于图片左上角。
d) 因为棋盘标定图纸中所有角点的空间坐标是已知的，这些角点对应在拍摄的标定图片中的角点的像素坐标也是已知的，如果我们得到这样的N>=4个匹配点对（越多计算结果越鲁棒），就可以根据LM等优化方法得到其单应矩阵H。当然计算单应矩阵一般不需要自己写函数实现，OpenCV中就有现成的函数可以调用，对应的c++函数是:
Mat findHomography(InputArray srcPoints, InputArray dstPoints, int method=0, double ransacReprojThreshold=3, OutputArray mask=noArray() )
从函数定义来看，只要输入匹配点对，指定具体计算方法即可输出结果。
1.3.4.2 求解内参矩阵
我们知道H是内参矩阵和外参矩阵的混合体，而我们想要最终分别获得内参和外参。所以需要想个办法，先把内参求出来（先求内参是因为更容易），得到内参后，外参也就随之解出了。
我们先不考虑镜头畸变，来看看如何求解内参和外参。求解思路是利用旋转向量的约束关系，以下是具体推导，建议自己演算一遍，加深理解。
为了利用旋转向量之间的约束关系，我们先将单应性矩阵H化为3个列向量，即 H = [ h 1 h 2 h 3 ] H=[h1 h2 h3] H=[h1h2h3]，则有
按元素对应关系可得：
懒得抄了，见下面链接
张正友标定法详细推导
opcv代码
import cv2 import glob import numpy as np w = 9 # 内角点个数，内角点是和其他格子连着的点 h = 6 objp = np.zeros((w * h, 3), np.float32)#一个9*6行3列的矩阵 objp[:, :2] = np.mgrid[0:w, 0:h].T.reshape(-1, 2)# 储存棋盘格角点的世界坐标和图像坐标对 reshape(-1, 2)-1表示不确定分几行，2表示分为2列 objp=2.6*objp #棋盘格实际大小 objpoints = [] # 在世界坐标系中的三维点 imgpoints = [] # 在图像平面的二维点 images = glob.glob('photo2/*.jpg') # 取文件夹中所有图片 for fname in images: # 对每张图片，识别出角点，记录世界物体坐标和图像坐标 img = cv2.imread(fname)#获取图像的水平方向和垂直方向的尺寸 img = cv2.resize(img, None, fx=0.5, fy=0.5, interpolation=cv2.INTER_CUBIC)#立方插值法 缩放到原来的二分之一 gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) # 转灰度 # 寻找角点，存入corners，ret是找到角点的flag ret, corners = cv2.findChessboardCorners(gray, (9, 6), None) # criteria:角点精准化迭代过程的终止条件 criteria = (cv2.TERM_CRITERIA_MAX_ITER+ cv2.TERM_CRITERIA_EPS , 30, 0.001)#第一项表示迭代次数达到最大次数时停止迭代，第二项表示角点位置变化的最小值已经达到最小时停止迭代 # 执行亚像素级角点检测 corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria) # 输入图像、角点初始坐标、搜索窗口为2*winsize+1、第四个参数作用类似于winSize，但是总是具有较小的范围，通常忽略（即Size(-1, -1)）, 求角点的迭代终止条件 objpoints.append(objp)# .append()向列表/数组添加元素 imgpoints.append(corners2) # 在棋盘上绘制角点 img = cv2.drawChessboardCorners(img, (9, 6), corners2, ret) cv2.namedWindow('img', cv2.WINDOW_NORMAL) cv2.imshow('img', img) cv2.waitKey(1000) ''' 传入所有图片各自角点的三维、二维坐标，相机标定。 每张图片都有自己的旋转和平移矩阵，但是相机内参和畸变系数只有一组。 mtx，相机内参；dist，畸变系数；rvecs，旋转矩阵；tvecs，平移矩阵。 ''' # 输入：世界坐标系里的位置 像素坐标 图像的像素尺寸大小 3*3矩阵，相机内参数矩阵 畸变矩阵 #使用cv2.calibrateCamera()进行标定，这个函数会返回标定结果、相机的内参数矩阵、畸变系数、旋转矩阵和平移向量 ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None) img = cv2.imread('photo2/5.jpg') img = cv2.resize(img, None, fx=0.5, fy=0.5, interpolation=cv2.INTER_CUBIC) h, w = img.shape[:2] ''' 使用cv.getOptimalNewCameraMatrix()优化内参数和畸变系数 参数1表示保留所有像素点，同时可能引入黑色像素，并返回一个ROI用于将其剪裁掉 设为0表示尽可能裁剪不想要的像素，这是个scale，0-1都可以取。 ''' newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (w, h), 1, (w, h)) # 纠正畸变 dst = cv2.undistort(img, mtx, dist, None, newcameramtx) # 输出纠正畸变以后的图片 x, y, w, h = roi dst = dst[y:y + h, x:x + w] cv2.imwrite('result.png', dst) # 输出：标定结果 相机的内参数矩阵 畸变系数 旋转矩阵 平移向量 mtx_new=mtx.tolist() print("相机内参矩阵:\n", mtx_new)#[fx,s,x0;0,fy,y0;0,0,1],fx,fy为焦距，一般二者相等；x0、y0为主点坐标（相对于成像平面），s为坐标轴倾斜参数，理想情况下为0 print("newcameramtx:\n", newcameramtx) print (("旋转向量rvecs:\n"),rvecs) # 旋转向量 # 外参数，3个旋转参数 print (("平移向量tvecs:\n"),tvecs) # 平移向量 # 外参数，3个平移参数 print("畸变系数dist:\n", dist)#5个畸变参数，径向畸变k1,k2,k3,切向畸变p1、p2 # 计算误差 # 反投影误差越接近0，说明结果越理想。 # 通过之前计算的内参数矩阵、畸变系数、旋转矩阵和平移向量，使用cv2.projectPoints()计算三维点到二维图像的投影， # 然后计算反投影得到的点与图像上检测到的点的误差，最后计算一个对于所有标定图像的平均误差，这个值就是反投影误差。 tot_error = 0 for i in range(len(objpoints)): imgpoints2, _ = cv2.projectPoints(objpoints[i], rvecs[i], tvecs[i], mtx, dist) error = cv2.norm(imgpoints[i], imgpoints2, cv2.NORM_L2) / len(imgpoints2) tot_error += error print("total error: ", tot_error / len(objpoints))二、激光雷达Lidar标定
参考链接
Lidar基础知识
内参是内部激光发射器坐标囍和雷达自身坐标器之间的转换关系。
外参是激光雷达与其他坐标系（例如车辆后轴中心）之间的坐标系转换关系。我们主要标定的是俯仰角（x轴）和侧倾角（y轴）
车体坐标系以车辆后轴中心作为原点， X v X_v Xv指向车辆的前方， Y v Y_v Yv指向车辆的左侧， Z v Z_v Zv指向车辆的上方。由于激光雷达的坐标系与车体坐标系存在一定的夹角， X l X_l Xl与 X v X_v Xv存在 α \alpha α的角度偏差， Y l Y_l Yl与 Y v Y_v Yv存在 γ \gamma γ的角度偏差，所以需要标定。
2.1.1 γ γ γ侧偏角的标定
γ \gamma γ侧偏角的标定需要用到矩形板ABCD。如图所示，矩形板ABCD平行于Z轴，且位于激光雷达的前方。激光束扫描的点构成了EF直线段，EG直线段为AB的平行线。
想要求得 γ \gamma γ侧偏角，可以利用 γ = arccos ( l E G l E F ) = arccos ( l A B l E F ) \gamma = \arccos (\frac{{{l_{EG}}}}{{{l_{EF}}}}) = \arccos (\frac{{{l_{AB}}}}{{{l_{EF}}}}) γ=arccos(lEFlEG)=arccos(lEFlAB)得到。KaTeX parse error: Expected '}', got 'EOF' at end of input: L_{AB可以由矩形板的边长大小得知， L O F L_{OF} LOF已知， L O E L_{OE} LOE也可以通过激光雷达的数据得到，因此可以求得 L E F L_{EF} LEF最后求得 γ \gamma γ侧偏角。
2.1.2 α俯仰角的标定
首先不考虑侧偏角 γ \gamma γ。激光雷达的Y轴垂直纸面向外，X轴呈如图所示方向，Z轴竖直向上。首先在1处放置一块标定板，激光雷达在标定板上扫描的位置为A，接着移动标定板置B处，过B作平行于地面的平行线，得到BC，即可求得 α \alpha α俯仰角。
在实际过程中，需要考虑侧偏角。如图所示，在1处放置一块等腰三角形的标定板，同之前的步骤，将标定板移至2处，由图可知， ∠ F 1 E 1 D 1 \angle {F_1}{E_1}{D_1} ∠F1E1D1与 ∠ F 2 E 2 D 2 \angle {F_2}{E_2}{D_2} ∠F2E2D2为侧倾角。
过 D 2 D_2 D2作地面的平行线交于G点，可知 ∠ G D 2 D 1 \angle GD_2D_1 ∠GD2D1即为要求的 α \alpha α俯仰角。易知
从而可以求得 α \alpha α俯仰角
2.2 相机与激光雷达联合标定
点云图像融合
通过融合的方式，可以弥补单个传感器的不足，可以更好地环境感知。因此，就需要对相机和激光雷达进行联合标定。
标定内容：标定从激光雷达坐标系到相机坐标系的空间变换关系。
意义
精确的环境感知：确保它们的感知数据在相同的坐标系中对齐
障碍物检测和避免：联合标定使激光雷达和相机能够共同工作，提供了多模态的感知数据。
精确的定位和导航：激光雷达和相机联合标定有助于提供高精度的位置信息，支持机器人、自动驾驶汽车和无人机的准确定位和导航。
三维重建和建模：通过联合标定，可以将激光雷达数据和相机图像对齐，以创建准确的三维环境模型。这对于建筑测绘、虚拟现实、三维导航和规划等应用非常有用。
复杂场景的感知：在复杂的城市环境中，激光雷达和相机联合标定使系统能够更好地理解和感知道路、建筑物、交通标志和其他关键元素。
数据融合：联合标定后的激光雷达和相机数据可以进行融合，从而提供了更丰富的感知信息，有助于改善决策和控制系统。
增强可靠性和鲁棒性：通过联合标定，可以减小感知数据之间的不确定性，提高系统的可靠性和鲁棒性，使其在各种复杂场景下都能正常运行。
总的来说，激光雷达相机联合标定的意义在于将不同传感器的数据融合在一起，提供更全面、准确和可靠的环境感知，这对于实现自主导航、环境理解和自动决策是至关重要的。它有助于实现更安全、高效和智能的交通系统、机器人和其他自主系统。
- 意义
