├── 000017.bin ├── README.md ├── data_gen.py ├── demo.png ├── download.md ├── image_range_cluster_v2.py └── my_ransac.py /000017.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lonlonago/Lidar-object-clustering-AND-ground-remove/472e66bbb88ebd6f71c7723f0aa7645289402bfa/000017.bin -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | # 激光雷达障碍物检测和聚类 5 | 6 | email:lonlonago@foxmail.com 7 | 8 | wechat: 394467238 9 | 10 | --- 11 | 12 |    13 | ### 参考论文： Efficient Online Segmentation for Sparse 3D Laser Scans ; 14 |    15 | 16 | 17 | 其中包含地面分割代码 my_ransac 18 | 19 |    20 | --- 21 | 22 | ## 依赖安装 install dependence : 23 | numpy 24 |    25 | 26 | 27 | python-pcl（安装参考python-pcl的Github,如果不需要可视化可以不安装，注释掉相应代码即可） 28 |    29 | 30 | 31 | --- 32 | 33 | ## 运行 run 34 | 35 | 直接运行 image_range_cluster_v2.py 即可； 36 |    37 | python image_range_cluster_v2.py 38 | 39 | 40 | --- 41 | 42 | ## 需完善 TODO : 43 | 44 | 45 | 1 运行时间较长（~2s），需优化，论文提到只需要20ms,差距还很大； 46 |    47 | 48 | 49 | 2 目标聚类效果还有待提高，存在把同一个物体分割成几个部分的情况，论文好像也存在这种情况； 50 |    51 | 52 | 53 | 54 | 3 RANSAC 地面分割效果还存在一些问题，针对某些特殊情况会出现漏掉部分地面或者分割出墙面； 55 | 56 | 57 | 4 需要对检测出来的物体做一个立体框，同时计算出目标的尺寸； 58 | 59 | 60 | 61 | 62 | 63 | --- 64 | 65 | ## 效果图 66 | 67 | ![avatar](https://github.com/lonlonago/-/blob/master/demo.png) 68 |    69 | -------------------------------------------------------------------------------- /data_gen.py: -------------------------------------------------------------------------------- 1 | #coding:utf-8 2 | 3 | import numpy as np 4 | 5 | 6 | def _normalize(x): 7 | return (x - x.min()) / (x.max() - x.min()) 8 | 9 | 10 | 11 | def lidar_to_2d_front_view_depth(points, 12 | v_line=65, 13 | h_res=360/2048., 14 | ground_indices=None 15 | ): 16 | """ 17 | TODO 横向的噪点是否还是需要去除？ 18 | 是否需要保留角度信息？ 不需要，那个是雷达的自带分辨率角度 19 | :param points: 20 | :param v_line: 21 | :param h_res: 22 | :return: 23 | """ 24 | x_lidar = points[:, 0] # -71~73 25 | y_lidar = points[:, 1] # -21~53 26 | z_lidar = points[:, 2] # -5~2.6 27 | 28 | # Distance relative to origin 29 | d = np.sqrt(x_lidar ** 2 + y_lidar ** 2 + z_lidar ** 2) 30 | 31 | # set ground depth to zero 32 | d2 = d.copy() 33 | if ground_indices is not None: 34 | d2[ground_indices] = 0 35 | 36 | # Convert res to Radians 37 | h_res_rad = np.radians(h_res) 38 | 39 | # 得到水平角度 40 | x_radians = np.arctan2(-y_lidar, x_lidar) # -1024~1024 -3.14~3.14 ; 41 | 42 | angle_diff = np.abs(np.diff(x_radians)) 43 | threshold_angle = np.radians(250) # 44 | angle_diff = np.hstack((angle_diff, 0.0001)) # 补一个元素，diff少了一个 45 | angle_diff_mask = angle_diff > threshold_angle 46 | 47 | # 把角度转换为image像素坐标 48 | x_img = np.floor((x_radians / h_res_rad)).astype(int) 49 | x_img -= np.min(x_img) # 把坐标为负数的部分做一个转移 50 | 51 | y_img = np.cumsum(angle_diff_mask) 52 | 53 | # x_max = int(360.0 / h_res) #+ 1 # 投影后图片的宽度 54 | x_max = 2049 55 | # x_img[x_img >= x_max] = 2047 56 | 57 | depth_map = np.zeros((v_line, x_max, 5)) #+255 58 | depth_map[y_img, x_img, 0] = x_lidar 59 | depth_map[y_img, x_img, 1] = y_lidar 60 | depth_map[y_img, x_img, 2] = z_lidar 61 | depth_map[y_img, x_img, 3] = d 62 | depth_map[y_img, x_img, 4] = d2 63 | 64 | return depth_map 65 | 66 | -------------------------------------------------------------------------------- /demo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lonlonago/Lidar-object-clustering-AND-ground-remove/472e66bbb88ebd6f71c7723f0aa7645289402bfa/demo.png -------------------------------------------------------------------------------- /download.md: -------------------------------------------------------------------------------- 1 | 2 | 3 | 软件代码下载链接： 4 | 5 | https://pan.baidu.com/s/1WQQ8kaDilaagjoK5IrYZzA 6 | 7 | 8 | 提取码：1111 9 | -------------------------------------------------------------------------------- /image_range_cluster_v2.py: -------------------------------------------------------------------------------- 1 | # coding:utf-8 2 | 3 | from data_gen import lidar_to_2d_front_view_depth, _normalize 4 | from my_ransac import my_ransac_v2 5 | import numpy as np 6 | import time 7 | 8 | 9 | import pcl.pcl_visualization 10 | 11 | 12 | def depth_clustering(projected_image, 13 | angle_tollerance = np.radians(8), 14 | min_cluster_size = 100, 15 | max_cluster_size = 25000, 16 | h_res=np.radians(360/2048.), 17 | v_res=np.radians(26.9/65)): 18 | 19 | depth_shape = depth.shape 20 | 21 | # 初始 label , label matrix 22 | label = 1 23 | label_matrix = np.zeros(depth_shape) 24 | 25 | # 对每个点进行检测，同时聚类 26 | for i in range(depth_shape[0]): 27 | for j in range(depth_shape[1]): 28 | if label_matrix[i, j] == 0: 29 | if depth[i, j] == 0: 30 | continue # 地面点跳过计算 31 | one_label_BFS(i, j, label, label_matrix, 32 | depth, angle_tollerance, 33 | h_res, v_res, projected_image) 34 | label += 1 35 | 36 | 37 | # 过滤掉点数量较多或较少的类别 38 | not_real_obstacle = 0 39 | for lx in range(1,label): 40 | # 最大数量限制会把一些墙体也过滤掉，这是否是需要的？ 41 | # if cluster_num < min_cluster_size : 42 | if cluster_num < min_cluster_size or cluster_num > max_cluster_size: 43 | # 把过滤掉的类别全部设置为地面 44 | label_matrix[label_matrix == lx] = 0 45 | not_real_obstacle += 1 46 | # print('After filtered left ', label - not_real_obstacle, ' real clusters .') 47 | 48 | return label_matrix 49 | 50 | 51 | def one_label_BFS(i, j, label, label_matrix, 52 | depth, angle_tollerance, 53 | hres, vres, projected): 54 | # 不管是广度优先还是深度优先，都是要搜索到所有与它关联的节点； 55 | # 它是怎么保证最多搜索次数只有2N？ 56 | # TODO 用set 更合适，因为会添加进去很多重复的点，可以用set的自动去重； 57 | Q = [(i,j)] 58 | while len(Q) > 0: 59 | Q.pop(0) # 默认是丢弃最后一个元素， 要放在这里，不然会死循环 60 | if label_matrix[r, c] > 0: 61 | # we have already labeled this point.No need to deal it. 62 | continue 63 | # label 赋值 64 | label_matrix[r, c] = label 65 | 66 | # 深度值为0，不纳入计算 67 | if depth[r,c] < 0.0001: 68 | continue 69 | 70 | for rn, cn in neighbourhood(r, c): 71 | if rn >= 65 or rn < 0 or cn < 0 or cn >= 2049: 72 | continue # 超出范围的点，不考虑 73 | if label_matrix[rn, cn] > 0: 74 | # we have already labeled this point.No need to add it. 75 | continue 76 | 77 | # TODO 两种方案，使用固定分辨率，或者实时计算分辨率，好像差别不大 78 | # plan 1 79 | if r != rn: 80 | res = vres 81 | elif c != cn: 82 | res = hres 83 | 84 | # plan 2 85 | # if r != rn: 86 | # # y_img_2 = -np.arcsin(z_lidar/d) # 得到垂直方向角度 87 | # res0 = np.arcsin(projected[r, c, 2] / np.sqrt(projected[r, c, 0] ** 2 + projected[r, c, 1] ** 2 + projected[r, c, 2] ** 2)) 88 | # res1 = np.arcsin(projected[rn, cn, 2] / np.sqrt(projected[rn, cn, 0] ** 2 + projected[rn, cn, 1] ** 2 + projected[rn, cn, 2] ** 2)) 89 | # res00 = np.arcsin(projected[r, c, 2] / depth[r,c]) 90 | # res11 = np.arcsin(projected[rn, cn, 2] / depth[rn, cn]) 91 | # print(res0,res00,res1,res11) 92 | # res = abs(res0 - res1) 93 | # elif c != cn: 94 | # # np.arctan2(-y_lidar, x_lidar) 95 | # res0 = np.arctan2(projected[r, c, 1] , projected[r, c, 0] ) 96 | # res1 = np.arctan2(projected[rn, cn, 1] , projected[rn, cn, 0] ) 97 | # res = abs(res0 - res1) 98 | 99 | 100 | theta = np.arctan2(d2 * np.sin(res), d1 - d2 * np.cos(res)) 101 | if theta > angle_tollerance: 102 | Q.append((rn, cn)) 103 | 104 | 105 | 106 | 107 | def neighbourhood(r, c): 108 | # 某个点的上下左右四个点的坐标 109 | return ( ( r-1, c ), 110 | ( r, c-1 ), 111 | ( r+1, c ), 112 | ( r, c+1 ) ) 113 | 114 | 115 | if __name__ == "__main__": 116 | print('---------'*10) 117 | 118 | path = r'000017.bin' 119 | points = np.fromfile(path, dtype=np.float32).reshape(-1, 4) 120 | 121 | print('-------------------ransac----------------------------') 122 | s = time.time() 123 | indices, model = my_ransac_v2(points[:, :3], 124 | distance_threshold=0.3,) 125 | print('Time take my_ransac_v2:', time.time() - s) 126 | 127 | # points project to a image 2049*65*5 128 | projected_map = lidar_to_2d_front_view_depth(points, ground_indices=indices) 129 | print('Time take lidar_to_2d_front_view_depth:', time.time() - s) 130 | 131 | 132 | # 障碍物聚类，最主要的步骤 133 | labels = depth_clustering(projected_map) 134 | print('Time take depth_clustering:', time.time() - s) 135 | 136 | 137 | # pcl_visualization 可视化 138 | # 先得到一个65*2049*4 --> N*4 139 | color_labels = 10000 + labels*3000 # 区分颜色 140 | color_map = projected_map[:,:,:4] 141 | color_map[:, :, 3] = color_labels 142 | color_map = np.resize(color_map, (-1, 4)) 143 | 144 | # 删除地面显示 delete ground 145 | # color_map = color_map[color_map[:, 3] > 10000, :] 146 | 147 | color_cloud = pcl.PointCloud_PointXYZRGB(color_map.astype(np.float32)) 148 | visual = pcl.pcl_visualization.CloudViewing() 149 | visual.ShowColorCloud(color_cloud, b'cloud') 150 | flag = True 151 | while flag: 152 | flag != visual.WasStopped() 153 | 154 | 155 | 156 | 157 | 158 | -------------------------------------------------------------------------------- /my_ransac.py: -------------------------------------------------------------------------------- 1 | 2 | 3 | import numpy as np 4 | import random 5 | 6 | 7 | 8 | def my_ransac_v2(data, 9 | distance_threshold=0.3, 10 | P=0.99, 11 | sample_size=3, 12 | max_iterations=10000, 13 | ): 14 | """ 15 | 更改迭代条件 16 | 加速计算 17 | :param data: 18 | :param sample_size: 19 | :param P : 20 | :param distance_threshold: 21 | :param max_iterations: 22 | :return: 23 | """ 24 | # np.random.seed(12345) 25 | random.seed(12345) 26 | max_point_num = -999 27 | i = 0 28 | K = 10 29 | L_data = len(data) 30 | R_L = range(L_data) 31 | 32 | # for i in range(max_iterations): 33 | while i < K: 34 | # 随机选3个点 np.random.choice 很耗费时间，改为random模块 35 | # s3 = np.random.choice(L_data, sample_size, replace=False) 36 | # TODO 三个点的高度可以作为第一步初筛的标准; 37 | # TODO 2个点的y 距离可以作为第一步初筛的标准，避免出现把墙作为平面 38 | # 没有效果，一定要迭代固定的次数，才能找到平面，这是为什么，不是随机的吗？ 39 | # if np.sum(data[s3,2] < 0.) != 3: 40 | # print(i,'point up ground. skip', data[s3,2]) 41 | # continue 42 | # print('data[s3,:]',data[s3,2]) 43 | # a = data[s3,2].copy() 44 | # a[a > 0] *= -1 45 | if abs(data[s3[0],1] - data[s3[1],1]) < 3: 46 | continue 47 | 48 | # 三个点的共线性检查; 改变了它的位置 49 | # if not check_in_one_line(data[s3,:]): 50 | # print('error ! 3 points in one line...') 51 | # continue 52 | 53 | # 计算平面方程系数 54 | if coeffs is None: 55 | continue 56 | # 法向量的模, 如果系数标准化了就不需要除以法向量的模了 57 | r = np.sqrt(coeffs[0]**2 + coeffs[1]**2 + coeffs[2]**2 ) 58 | # 计算每个点和平面的距离，根据阈值得到距离平面较近的点数量 59 | # d = np.abs(np.matmul(coeffs[:3], data.T) + coeffs[3]) / r 60 | d = np.divide(np.abs(np.matmul(coeffs[:3], data.T) + coeffs[3]) , r) 61 | # d = abs(np.matmul(coeffs[:3], data.T) + coeffs[3]) / r 62 | d_filt = np.array(d < distance_threshold) 63 | # d_filt = d < distance_threshold 64 | # d_filt = np.where(d < distance_threshold, 1,0) 65 | near_point_num = np.sum(d_filt,axis=0) 66 | # near_point_num = sum(d < distance_threshold) 67 | 68 | # print('coeffs:',coeffs, ) 69 | # print('coeffs2:', coeffs2/r,r ,coeffs2) 70 | # print('# inliers:', near_point_num, near_point_num2) 71 | 72 | if near_point_num > max_point_num: 73 | max_point_num = near_point_num 74 | # TODO 这是是否还有一个重新拟合模型的步骤， 75 | # 有的话怎么用多个点求一个平面 76 | best_model = coeffs 77 | best_filt = d_filt 78 | # if near_point_num > inliers_precent_threshold*len(data): 79 | # break 80 | # 最大迭代次数K，是P的函数, 因为迭代次数足够多，就一定能找到最佳的模型， 81 | # P是模型提供理想需要结果的概率，也可以理解为模型的点都是内点的概率？ 82 | # 83 | # 84 | # P = 0.99 85 | # 当前模型，所有点中随机抽取一个点，它是内点的概率 86 | w = near_point_num / L_data 87 | # np.power(w, 3)是随机抽取三个点，都是内点的概率 88 | # 1-np.power(w, 3)，就是三个点至少有一个是外点的概率， 89 | # 也就是得到一个坏模型的概率; 90 | # 1-P 代表模型永远不会选出一个3个点都是内点的集合的概率， 91 | # 也就是至少有一个外点； 92 | # K 就是得到的需要尝试多少次才会得到当前模型的理论次数 93 | # TODO 完善对该理论最大论迭代次数的理解 94 | wn = np.power(w, 3) 95 | p_no_outliers = 1.0 - wn 96 | # sd_w = np.sqrt(p_no_outliers) / wn 97 | # print('# K:', i, K, near_point_num) 98 | 99 | i += 1 100 | 101 | if i > max_iterations: 102 | print(' RANSAC reached the maximum number of trials.') 103 | break 104 | 105 | print('took iterations:', i+1, 'best model:', best_model, 106 | 'explains:', max_point_num) 107 | return np.argwhere(best_filt).flatten(), best_model 108 | 109 | 110 | 111 | 112 | def estimate_plane(xyz, normalize=True): 113 | """ 114 | 已知三个点，根据法向量求平面方程； 115 | 返回平面方程的一般形式的四个系数 116 | :param xyz: 3*3 array 117 | x1 y1 z1 118 | x2 y2 z2 119 | x3 y3 z3 120 | :return: a b c d 121 | 122 | model_coefficients.resize (4); 123 | model_coefficients[0] = p1p0[1] * p2p0[2] - p1p0[2] * p2p0[1]; 124 | model_coefficients[1] = p1p0[2] * p2p0[0] - p1p0[0] * p2p0[2]; 125 | model_coefficients[2] = p1p0[0] * p2p0[1] - p1p0[1] * p2p0[0]; 126 | model_coefficients[3] = 0; 127 | // Normalize 128 | model_coefficients.normalize (); 129 | // ... + d = 0 130 | model_coefficients[3] = -1 * (model_coefficients.template head<4>().dot (p0.matrix ())); 131 | 132 | """ 133 | vector1 = xyz[1,:] - xyz[0,:] 134 | vector2 = xyz[2,:] - xyz[0,:] 135 | 136 | # 共线性检查 137 | if not np.all(vector1): 138 | print('will divide by zero..') 139 | return None 140 | dy1dy2 = vector2 / vector1 141 | # 2点如果是一条直线，那么必然它的xyz都是同一个比例关系 142 | if not ((dy1dy2[0] != dy1dy2[1]) or (dy1dy2[2] != dy1dy2[1])): 143 | return None 144 | 145 | 146 | b = (vector1[2]*vector2[0]) - (vector1[0]*vector2[2]) 147 | c = (vector1[0]*vector2[1]) - (vector1[1]*vector2[0]) 148 | # normalize 149 | if normalize: 150 | r = np.sqrt(a ** 2 + b ** 2 + c ** 2) 151 | a = a / r 152 | b = b / r 153 | c = c / r 154 | d = -(a*xyz[0,0] + b*xyz[0,1] + c*xyz[0,2]) 155 | # return a,b,c,d 156 | return np.array([a,b,c,d]) 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | --------------------------------------------------------------------------------