激光雷达与rgb相机外参标定

1 简介

最近在做livox与rgb相机的外参标定，网上看了很多开源方法大对数是基于ros做的，由于对ros不太熟悉，所以先基于python写一个初始版本，以下是全部代码，后面有有时间再整理，相机的内参是使用matlab工具箱标定的，大致思路是将标定板的点云数据通过左右、上下的坐标替换，深度值转换为灰度值，进而得到灰度图，对灰度图做传统图像处理，找到圆心，然后再逆转回在激光雷达坐标系上的坐标。

2 代码

import pandas as pd
import pcl
import open3d as o3d
import numpy as np
import cv2 as cv

import warnings

import xml

from sympy import false
warnings.filterwarnings("ignore")

rgb_mtx = np.array([[164.9671, 0., 334.1256,
                    0., 167.1601, 219.2284,
                    0., 0., 1.]]).reshape((3, 3))

rgb_dist = np.array([-0.0844, 0.0065, -2.2149e-04, -1.1157e-04, 2.8005e-04]).reshape((1, 5))

# rgb_mtx = np.array([[120, 0., 640,
#                      0., 120, 360,
#                      0., 0., 1.]]).reshape((3, 3))

# rgb_dist = np.zeros((5, 1), dtype=np.float64) 

def find_lidar_blobs(input_img, show=False):
    input_img = 255 - input_img
    params = cv.SimpleBlobDetector_Params()
    params.minThreshold = 5
    # params.maxThreshold = 5
    params.blobColor = 0
    # Filter by Area.
    params.filterByArea = True
    params.minArea = 400
    params.maxArea = 21000
    # Filter by Circularity
    params.filterByCircularity = True
    params.minCircularity = 0.1
    # Filter by Convexity
    params.filterByConvexity = True
    params.minConvexity = 0.87
    # Filter by Inertia
    params.filterByInertia = True
    params.minInertiaRatio = 0.1
    detector = cv.SimpleBlobDetector_create(params)
    # keypoints是一个列表，其中的每个元素都是一个cv2.KeyPoint
    # KeyPoint包含两个属性 圆的直径以及圆心的位置
    keypoints = detector.detect(input_img)
    # keypoints = [kp for kp in keypoints if 72 <= kp.size <= 88]
    img_with_keypoints = cv.drawKeypoints(input_img, keypoints, np.array([]), (0,255,0), cv.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)
    # size_list = [kp.size for kp in keypoints]
    if show:
        cv.namedWindow("Keypoints", 0)
        cv.imshow('Keypoints', img_with_keypoints)
        cv.waitKey(0)
        cv.destroyAllWindows()


    return keypoints

def filter_raw_pcl(data_path, color_change):
    ori_pcl_data = pd.read_csv(data_path, low_memory=False)
    x_condition = (ori_pcl_data['X'] > 0.6) & (ori_pcl_data['X'] <2)  #纵向
    y_condition = (ori_pcl_data['Y'] > -0.6) & (ori_pcl_data['Y'] < 0.5) #横向
    z_condition = (ori_pcl_data['Z'] > 0.3) & (ori_pcl_data['Z'] < 3) #高度
    ref_condition =  (ori_pcl_data['Reflectivity'] > 0) & (ori_pcl_data['Reflectivity'] < 15)
    # ref_condition =  (ori_pcl_data['Reflectivity'] > 10)
    filtered_data = ori_pcl_data[x_condition & y_condition & z_condition & ref_condition]
    # filtered_data = ori_pcl_data[x_condition & y_condition & z_condition]
    if color_change:
        min = filtered_data['Reflectivity'].min()
        max = filtered_data['Reflectivity'].max()
        filtered_data['reflectance_normalized'] = (filtered_data['Reflectivity'] - min) / (max - min)
    return filtered_data

def show_pcl(data):
    point_cloud = o3d.geometry.PointCloud()
    # 根据数据类型显示
    if isinstance(data, pd.DataFrame):
        point_cloud.points = o3d.utility.Vector3dVector(data[['X', 'Y', 'Z']].values)
    else:
        point_cloud.points = o3d.utility.Vector3dVector(data)
    o3d.visualization.draw_geometries([point_cloud])

def sac_plane(valid_data_df):
    valid_data = valid_data_df[['X', 'Y', 'Z']].values.astype('float32')
    cloud = pcl.PointCloud(valid_data)

    seg = cloud.make_segmenter()
    seg.set_optimize_coefficients(True)
    seg.set_model_type(pcl.SACMODEL_PLANE)
    seg.set_method_type(pcl.SAC_RANSAC)
    seg.set_distance_threshold(0.01)

    inliners, coefficients = seg.segment()

    guess = np.expand_dims(np.array(coefficients), axis=1)
    res = np.dot(valid_data, guess[:3, :]) + guess[3, 0]
    
    plane_cloud = cloud.extract(inliners, negative=False)
    plane_cloud_arr = plane_cloud.to_array()


    return plane_cloud_arr, coefficients

def projecto2D(filter_data):
    projected_points = filter_data*1000
    x_min, x_max = projected_points[:, 0].min(), projected_points[:, 0].max()  
    z_min, z_max = projected_points[:, 2].min(), projected_points[:, 2].max()  
    y_min, y_max = projected_points[:, 1].min(), projected_points[:, 1].max()
    x_range = x_max -x_min
    
    # 创建RGB图像  
    image_width = int(y_max - y_min)  
    image_height = int(z_max - z_min)  
    # TODO 当前是用int8保存深度值 后面使用float16格式
    rgb_image = np.zeros((image_height, image_width, 1), dtype=np.uint8)
    # 映射颜色到RGB图像  
    for point in projected_points:  
        y, z = int(y_max -point[1]), int(z_max - point[2])
        if 0 <= y < image_width and 0 <= z < image_height:  
            rgb_image[z, y] = int(255*(point[0]- x_min)/x_range)
    cv.imwrite("demo.jpg", rgb_image)
    return[x_min, x_range, y_max, z_max], rgb_image
    

def cal_rgbd(img_path, xyz, show=False):
    if isinstance(img_path, str):
        ori_img = cv.imread(img_path, cv.IMREAD_UNCHANGED)
    else:
        ori_img = img_path
    img_show = cv.cvtColor(ori_img, cv.COLOR_GRAY2BGR)
    # 图片二值化
    ret, smoothed_img = cv.threshold(ori_img, 15, 255, cv.THRESH_BINARY)
    kernel = np.ones((5,5),np.uint8)
    smoothed_img = cv.dilate(smoothed_img,kernel,iterations = 1)
    if show:
        cv.namedWindow('threshold', 0)
        cv.imshow("threshold", smoothed_img)
        key = 0
        while True:
            key = cv.waitKey()
            if key == 27:
                break
    # 高斯滤波
    smoothed_img = cv.GaussianBlur(smoothed_img, (5, 5), 0)
    keypoints = find_lidar_blobs(smoothed_img, show= show)
    gridcell_list  = []
    for keypoint in keypoints:
        x, z = int(keypoint.pt[0]), int(keypoint.pt[1])
        valid_point_num = 0
        value_sum  = 0
        for i in range(10):
            for j in range(10):
                # 加入距离筛选
                # TODO 现在深度值有问题 深度值从左到右递减
                if(ori_img[z+j, x+i]) > 30:
                    valid_point_num += 1
                    value_sum += ori_img[z+j, x+i]
        if valid_point_num == 0:
            continue
        mean_value = value_sum / valid_point_num
        # 还原为之前的深度值 
        mean_value_trans = mean_value*xyz[1]/255 + xyz[0]
        # print("mean_value_trans", mean_value_trans)
        if isinstance(mean_value_trans, np.ndarray):
            mean_value_trans = mean_value_trans.item()
    # return[x_min, x_range, y_max, z_max], rgb_image
        x_trans= xyz[2] - x
        z_trans = xyz[3] -z
        if z_trans > 400: #在纵向上加入高度筛选
            gridcell_list.append([mean_value_trans,x_trans,z_trans])
            cv.circle(img_show,(x, z), 1, (0, 0, 255))
    if show:
        cv.namedWindow("img_show", 0)
        cv.imshow('img_show', img_show)
        key = 0
        while True:
            key = cv.waitKey()
            if key == 27:
                break
    return np.array(gridcell_list, dtype=np.float64)

def add_world_point(circle_dist=8,boardh=12, boardw=4):
    world_points = []
    for i in range(boardh):
        for j in range(boardw):        
            if i%2 == 0:
                x = circle_dist * (2 * j)
            else:
                x = circle_dist * (2 * j + 1)
            y = circle_dist * i
            world_points.append([x, y, 0])
    return np.array(world_points, dtype=np.float32)

def find_rgb_blobs(input_img, show=False):
    params = cv.SimpleBlobDetector_Params()
    params.minThreshold = 5
    # params.maxThreshold = 5
    # params.blobColor = 0
    # Filter by Area.
    params.filterByArea = True
    params.minArea = 20
    params.maxArea = 500
    # Filter by Circularity
    params.filterByCircularity = True
    params.minCircularity = 0.1
    # Filter by Convexity
    params.filterByConvexity = True
    params.minConvexity = 0.87
    # Filter by Inertia
    params.filterByInertia = True
    params.minInertiaRatio = 0.1
    detector = cv.SimpleBlobDetector_create(params)
    keypoints = detector.detect(input_img)
    img_with_keypoints = cv.drawKeypoints(input_img, keypoints, np.array([]), (0,255,0), cv.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)
    # size_list = [kp.size for kp in keypoints]
    if show:
        cv.namedWindow("Keypoints", 0)
        cv.imshow('Keypoints', img_with_keypoints)
        cv.waitKey(0)
        cv.destroyAllWindows()
    return keypoints

def cal_rgb(rgb_img_path, show=False):
    rgb_cell_list = []
    rgb_img = cv.imread(rgb_img_path)
    
    gray = cv.cvtColor(rgb_img,cv.COLOR_BGR2GRAY)
    
    rgb_img[0:100, :] = 0
    rgb_img[:, 0:250] = 0
    rgb_img[250:480, :] = 0
    rgb_img[:, 400:] = 0
    keypoints = find_rgb_blobs(rgb_img, show=show)
    for keypoint in keypoints:
        x, y = int(keypoint.pt[0]), int(keypoint.pt[1])
        rgb_cell_list.append([x, y])
    rgb_cell_list  = np.array(rgb_cell_list)
    ori = np.copy(rgb_cell_list)
    # 当前的rgb已经严格按照从上到下，从左到右排列
    ori[:, 1] = np.round(ori[:, 1] / 8) * 8
    indices = np.lexsort((ori[:, 0], ori[:, 1]))
    rgb_cell_list = rgb_cell_list[indices]
    # word_points = add_world_point()
    # if show:
    #     i = 0
    #     for point in rgb_cell_list:
    #         cv.putText(rgb_img, "{}".format(i), (point[0], point[1]), cv.FONT_HERSHEY_TRIPLEX,thickness = 1,fontScale= 0.5,color=(0,255,0))
    #         i = i + 1
    #     cv.namedWindow("img_show_whole", 0)
    #     cv.imshow('img_show_whole', rgb_img)
    #     key = 0
    #     while True:
    #         key = cv.waitKey()
    #         if key == 27:
    #             break
    # rgb_cell_list = np.array(rgb_cell_list, dtype=np.float32).reshape(1, -1, 2)
    # word_points = np.array(word_points).reshape(1, -1, 3)
    # ret, mtx, dist, rvecs, tvecs = cv.calibrateCamera(word_points, rgb_cell_list, gray.shape[::-1], None, None)
    # if ret:
    #     print(mtx, dist)
    rgb_cell_list = rgb_cell_list.tolist()[:12]
    return np.array(rgb_cell_list, dtype=np.float64)

def getRT(world_points,gridcell_list, mtx, dist):
    _, rvec, tvec = cv.solvePnP(world_points, 
                            gridcell_list, mtx, dist,cv.SOLVEPNP_ITERATIVE)
    rotation_m, _ = cv.Rodrigues(rvec)  # 罗德里格斯变换
    RT = np.transpose(rotation_m)
    shouldBeIdentity = np.dot(RT, rotation_m)
    I = np.identity(3, dtype=rotation_m.dtype)
    n = np.linalg.norm(I - shouldBeIdentity)
    assert (n < 1e-6)
    return rotation_m, tvec


def main():
    show = True
    data_path = '~/cxx_project/lidar_rgb_calib/data/0221/2024-02-21_18-06-03.Csv'
    filter_data = filter_raw_pcl(data_path, False)
    # show_pcl(filter_data)
    calib_board, conf = sac_plane(filter_data)
    xyz, depth_img = projecto2D(calib_board)
    depth_cell_list = cal_rgbd(depth_img, xyz, show=show)
    depth_ori = np.copy(depth_cell_list)
    # # 当前的rgb已经严格按照从上到下，从左到右排列
    depth_ori[:, 2] = np.round(depth_ori[:, 2] / 60) * 60
    indices = np.lexsort((-depth_ori[:, 1], -depth_ori[:, 2]))
    depth_cell_list = depth_cell_list[indices]/1000
    # depth_cell_list = depth_cell_list[:, [0, 2, 1]]
    print("depth_cell_list:", depth_cell_list)
    rgb_img_path = '~/cxx_project/lidar_rgb_calib/data/0221/BIAODINGBAN/undistort_img/52.jpg'
    img = cv.imread(rgb_img_path)
    rgb_cell_list = cal_rgb(rgb_img_path, show=show)
    print("rgb_cell_list:", rgb_cell_list)
    R, T= getRT(depth_cell_list, rgb_cell_list, rgb_mtx, rgb_dist)
    print("R:{}, T{}".format(R, T))
    image_points, _ = cv.projectPoints(calib_board, R, T, rgb_mtx, rgb_dist)
    for point in image_points:
        x, y = point[0]
        cv.circle(img, (int(x), int(y)), radius=1, color=(0, 255, 0), thickness=-1)
    # if show:
    cv.namedWindow("res", 0)
    cv.imshow('res', img)
    cv.waitKey(0)
    # cv.destroyAllWindows()


if __name__ == "__main__":
    main()
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311