【传感器标定】相机与雷达外参标定（理论与代码）

读书猿

发布于 2024-02-05 15:12:40

1760

发布于 2024-02-05 15:12:40

文章被收录于专栏：无人驾驶感知无人驾驶感知

前言

1、相机外参标定需要提前已知相机内参，相机内参标定这里不细谈, 推荐一篇博客计算机视觉（相机标定；内参；外参；畸变系数

2、外参标定转化数学问题：计算一个三维坐标系到另一个三维坐标系的旋转与平移。

3、标定方案参考论文地址。实际标定对比，此方法精度高、操作方便。

4、标靶：贴一张虚拟的标靶图片：

（实际标靶要根据需求自行制作）

一、相机外参标定

1.1、apritag识别

1、在标靶四个角贴上标签

2、识别四个标签得到在图像坐标系下的坐标

def calibration_point(self):
    img_gray = cv2.cvtColor(self.image, cv2.COLOR_BGR2GRAY)
    detector = apriltag.Detector(searchpath=["./"])
    results = detector.detect(img_gray)
    markerCenter = []
    for i in range(len(results)):
        tmp_obj = results[i]
        markerCenter.append(list(tmp_obj.center))
    return markerCenter

1.2、求相机坐标系下PA，PB，PC方向的单位向量

由像素坐标与相机坐标转换关系

取Zc = 1 。则 x, y, z = (u - cx) / fx, (v - cy) / fy, 1

归一化后可得单位向量PA，PB，PC

1.3、求角APB，角BPC，角CPA

由向量PA，PB，PC容易求出角APB，角BPC，角CPA

例：cos（角APB） = PA .PB

1.4、求PA，PB，PC的长

转化数学问题：四面体PABC，已知长AB、AC、BC，角APB，角BPC，角CPA，求PA、PB、PC的长。

通过已知条件联立方程可以求出解。通过消元法可以解

公式推导参考：消元法

通过余弦定理建立方程，转化后得出一个二元二次方程组。

消元法求解，可能有四组解。

def solve_for_length(self, threePoint):
    solution = []  # 保存的解
    lenAB, lenBC, lenCA = self.w, self.h, np.sqrt(self.w * self.w + self.h * self.h)
    p, q, r = self.get_three_point_r(threePoint)
    p, q, r = 2 * p, 2 * q, 2 * r
    a, b = np.power(lenAB / lenCA, 2), np.power(lenBC / lenCA, 2)
    a2, b2, p2, q2, r2, pr = a * a, b * b, p * p, q * q, r * r, p * r
    pqr = q * pr
    if p2 + q2 + r2 - pqr - 1 == 0:  # 四点共面无解
        return "四点共面，无解"
    ab, a_2, a_4 = a * b, 2 * a, 4 * a
    A = -2 * b + b2 + a2 + 1 + ab * (2 - r2) - a_2
    B = q * (-2 * (ab + a2 + 1 - b) + r2 * ab + a_4) + pr * (b - b2 + ab)
    C = q2 + b2 * (r2 + p2 - 2) - b * (p2 + pqr) - ab * (r2 + pqr) + (a2 - a_2) * (2 + q2) + 2
    D = pr * (ab - b2 + b) + q * ((p2 - 2) * b + 2 * (ab - a2) + a_4 - 2)
    E = 1 + 2 * (b - a - ab) + b2 - b * p2 + a2
    temp = (p2 * (a - 1 + b) + r2 * (a - 1 - b) + pqr - a * pqr)
    b0 = b * temp * temp
    if b0 == 0:  # 当b0=0时求得y=0 不符题意
        return "y=0,无解"

    unknownX = sympy.symbols('x')  # 求解x
    solve = sympy.solve(A * unknownX ** 4 + B * unknownX ** 3 + C * unknownX ** 2 + D * unknownX + E, unknownX)
    n = len(solve)  # 判断解的个数，有重根算一个
    if n == 0:
        return "x无根，无解"

    r3, pr2 = r2 * r, p * r2
    r3q = r3 * q
    inv_b0 = 1.0 / b0

    for i in range(n):
        x = solve[i]
        if abs(np.imag(complex(x))) < 1e-10:
            x = np.real(complex(x))
        else:
            continue
        if x < 0:  # 不符合题意，舍去此解
            continue
        x2 = x * x
        b1 = ((1 - a - b) * x2 + (q * a - q) * x + 1 - a + b) * (((r3 * (a2 + ab * (2 - r2) - a_2 + b2 - 2 * b + 1))
                    * x + (r3q * (2 * (b - a2) + a_4 + ab * (r2 - 2) - 2) + pr2 * (1 + a2 + 2 * (ab - a - b) + r2 * 
                    ( b - b2) + b2))) * x2 + (r3 * (q2 * (1 - 2 * a + a2) + r2 * (b2 - ab) - a_4 + 2 * (a2 - b2) + 2) + 
                    r * p2 * (b2 + 2 * (ab - b - a) + 1 + a2) + pr2 * q * (a_4 + 2 * (b - ab - a2) - 2 - r2 * b)) * x + 
                    2 * r3q * (a_2 - b - a2 + ab - 1) + pr2 * (q2 - a_4 + 2 * (a2 - b2) + r2 * b + q2 * ( a2 - a_2) + 2) 
                    + p2 * (p * (2 * (ab - a - b) + a2 + b2 + 1) + 2 * q * r * (b + a_2 - a2 - ab - 1))))
        if b1 <= 0:  # 求得y<0 舍去此解
            continue
        y = inv_b0 * b1
        v = x * x + y * y - x * y * r
        if v < 0:  # 小于0的解不符合
            continue
        lenPB = lenCA / np.sqrt(v)
        lenPC = x * lenPB
        lenPA = y * lenPB
        solution.append([lenPA, lenPB, lenPC])  # 符合条件的解
    return solution

1.5、svd分解求解旋转平移矩阵

根据1.4得出四组点A、B、C在相机坐标系下坐标

通过svd分解分别算四组旋转平移矩阵

svd分解数学推导参考：svd分解推导

计算点D在旋转平移后的重投影误差，选取重投影误差最小的作为最终标定结果。

def get_rt(childList, parentList):
    x = get_data(childList)
    y = get_data(parentList)
    zeroMatrix = np.zeros((3, 3))
    for i in range(y.shape[0]):
        newY = y[i].reshape(1, y.shape[1])
        newX = x[i].reshape(1, y.shape[1])
        zeroMatrix = zeroMatrix + newX.T @ newY

    svd = np.linalg.svd(zeroMatrix)
    U, E, V = svd
    midMatrix = np.array([[1, 0, 0],
                          [0, 1, 0],
                          [0, 0, np.linalg.det(V.T @ U.T)]])
    r = V.T @ midMatrix @ U.T
    t = get_avg(parentList).T - r @ get_avg(childList).T
    return r, t

二、雷达外参标定、雷达与相机联合

2.1、通过雷达获取标定板

# 最小二乘法拟合平面 ax + by + cz - 1 = 0
def fit(M):
    np_ones = np.ones((len(M), 1))
    M_T = M.transpose()
    result = np.dot(np.dot(np.linalg.inv(np.dot(M_T, M)), M_T), np_ones)
    return result

def get_plane(self, count):
    pcd = o3d.io.read_point_cloud(self.path[-1])
    cloud_point = np.asarray(pcd.points)
    minX, maxX, minY, maxY, minZ, maxZ = self.fiter
    points, point_x, point_z = [], [], []
    for point in cloud_point:
        if minX < point[0] < maxX and minY < point[1] < maxY and minZ < point[2] < maxZ:
            points.append(list(point))
            point_x.append(point[0])
            point_z.append(point[2])
    min_z, max_z, avg_x = min(point_z), max(point_z), np.average(point_x)
    gap = 0.02
    sum_max, sum_min = 0, 0
    while sum_max < count or sum_min < count:
        if sum_max < count:
            sum_max = 0
            max_z -= gap
        if sum_min < count:
            sum_min = 0
            min_z += gap
        for data in point_z:
            if max_z - gap < data < max_z + gap:
                sum_max += 1
            if min_z - gap < data < min_z + gap:
                sum_min += 1
    result_cloud_point = []
    for path in self.path:
        pcd = o3d.io.read_point_cloud(path)
        cloud_point = np.asarray(pcd.points)
        for point in cloud_point:
            if minX < point[0] < maxX and minY < point[1] < maxY and min_z - gap < point[2] < max_z + gap:
                result_cloud_point.append(point)
    a, b, c = fit(np.array(result_cloud_point)).reshape(-1)
    for i in range(len(result_cloud_point) - 1, 0, -1):
        x, y, z = result_cloud_point[i]
        if np.abs(a * x + b * y + c * z - 1) / np.sqrt(a * a + b * b + c * c) > 0.03:
            del result_cloud_point[i]
    return result_cloud_point

在较为空旷的场地，很容易可以获取到标定板的大致位置

2.2、拟合圆求中心点

def circle_fit(self, point_list, A):
    M, L = [], []
    for i in range(len(point_list) - 1):
        x1, y1, z1 = point_list[i]
        x2, y2, z2 = point_list[i + 1]
        M.append([x2 - x1, y2 - y1, z2 - z1])
        l = (x2 * x2 + y2 * y2 + z2 * z2 - (x1 * x1 + y1 * y1 + z1 * z1)) / 2
        L.append(l)
    B = np.array(M)
    L2 = np.array(L).reshape(len(L), 1)
    B_T = B.transpose()
    A_T = A.transpose()
    left = np.vstack((np.hstack((np.dot(B_T, B), A)), np.hstack((A_T, np.array([0]).reshape(1, 1)))))
    right = np.vstack((np.dot(B_T, L2), np.array([1]).reshape(1, 1)))
    C = np.dot(np.linalg.inv(left), right)
    center_x, center_y, center_z = C[0][0], C[1][0], C[2][0]
    return center_x, center_y, center_z