RANSAC 激光雷达地面检测 （2）

上次文章写到了随机采样一致性方法找地面的两个缺点：

一是会把墙壁当作地面，二是对于斜坡会漏掉，导致只检测了一部分的路面；

文章链接：RANSAC 激光雷达地面检测 （1）

---

这次我说下解决这两个问题的思路：

1 第一个问题我们可以引入一个假设，假设雷达和地面是平行的，而这通常是成立的，那么在这个假设的基础上，可以推理出我们找的的平面和激光雷达的Z轴是垂直的，也就是说，平面的法向量和Z轴（0，0，1）的夹角是很小的，那么我们就可以通过这个假设把墙壁这种平面过滤掉；

---

2 第二个问题主要是由于较远地方的斜坡路段的确和近处的路面不是一个平面了，那么我们可以考虑分段拟合，也就是把原本一次拟合的路面分割下，20米（阈值视情况而定）开外的路面我用另外一个平面去拟合；

---

3 另外一个优化是为了加快找平面的速度，以及避免找到低洼处的平面，我用高度值对随机采样点做了一个过滤，因为kitti 数据集的激光雷达高度是1.73米，所以用 {-1.73-0.2 ~ -1.73+0.3 } 范围内的点来采样去拟合平面；

---

代码如下：

def my_ransac_v5(data,
              distance_threshold=0.3,
              P=0.99,
              sample_size=3,
              max_iterations=10000,
              lidar_height=-1.73+0.4,
              lidar_height_down=-1.73-0.2,
              first_line_num=2000,
              alpha_threshold=0.03, # 0.05
              use_all_sample=False,
              y_limit=4,
              ):
    """

    :param data:  N * 4
    :param sample_size:
    :param P :
    :param distance_threshold:
    :param max_iterations:
    :return:
    """
    # np.random.seed(12345)
    random.seed(12345)
    max_point_num = -999
    best_model = None
    best_filt = None
    alpha = 999
    i = 0
    K = max_iterations # 增加夹角判断后，K初始值不能太小，否则可能一直没有进入最优判断语句


    if not use_all_sample:
        # 不采用第一根线做法，单纯只用高度过滤
        z_filter = data[:,2] < lidar_height  # kitti 高度加0.4
        z_filter_down = data[:,2] > lidar_height_down  # kitti 高度过滤
        filt = np.logical_and(z_filter_down, z_filter)  # 必须同时成立

        first_line_filtered = data[filt,:]
        print('first_line_filtered number.' ,first_line_filtered.shape,data.shape)
    else:
        first_line_filtered = data

    if data.shape[0] < 1900 or first_line_filtered.shape[0] < 180:
        print(' RANSAC point number too small.')
        return None, None, None, None

    L_data = data.shape[0]
    R_L = range(first_line_filtered.shape[0])


    while i < K:
        # 随机选3个点  np.random.choice 很耗费时间，改为random模块
        # s3 = np.random.choice(L_data, sample_size, replace=False)
        s3 = random.sample(R_L, sample_size)

        # 计算平面方程系数
        coeffs = estimate_plane(first_line_filtered[s3,:], normalize=False)
        if coeffs is None:
            continue
        # 法向量的模, 如果系数标准化了就不需要除以法向量的模了
        r = np.sqrt(coeffs[0]**2 + coeffs[1]**2 + coeffs[2]**2 )
        # 法向量与Z轴(0,0,1)夹角
        alphaz = math.acos(abs(coeffs[2]) / r)

        # r = math.sqrt(coeffs[0]**2 + coeffs[1]**2 + coeffs[2]**2 )
        # 计算每个点和平面的距离，根据阈值得到距离平面较近的点数量
        # d = np.abs(np.matmul(coeffs[:3], data.T) + coeffs[3]) / r
        d = np.divide(np.abs(np.matmul(coeffs[:3], data[:,:3].T) + coeffs[3]), r)
        d_filt = np.array(d < distance_threshold)
        d_filt_object = ~d_filt

        near_point_num = np.sum(d_filt,axis=0)

        # 为了避免将平直墙面检测为地面，必须将夹角加入判断条件，
        # 如果只用内点数量就会导致某个点数较多的非地面平面占据max_point_num，
        # 而其他地面平面无法做出更换；问题是该阈值的选取多少合适？
        if near_point_num > max_point_num and alphaz < alpha_threshold:
            max_point_num = near_point_num

            best_model = coeffs
            best_filt = d_filt
            best_filt_object = d_filt_object

            # # 与Z轴(0,0,1)夹角
            # alpha = math.acos(abs(coeffs[2]) / r)
            alpha = alphaz

            w = near_point_num / L_data

            wn = math.pow(w, 3)
            p_no_outliers = 1.0 - wn
            # sd_w = np.sqrt(p_no_outliers) / wn
            K = (math.log(1-P) / math.log(p_no_outliers)) #+ sd_w

        i += 1


        if i > max_iterations:
            print(' RANSAC reached the maximum number of trials.')
            return None,None,None,None

    print('took iterations:', i+1, 'best model:', best_model,
          'explains:', max_point_num)
    # 返回地面坐标，障碍物坐标，模型系数，夹角
    return np.argwhere(best_filt).flatten(),np.argwhere(best_filt_object).flatten(), best_model, alpha



# @profile
def my_ransac_segment(data,segment_x=20):
    """
    分段拟合地面

    :param data:  N * 4
    :param sample_size:
    :param P :
    :param distance_threshold:
    :param max_iterations:
    :return:
    """
    data0_20 = data[data[:,0] <= segment_x, :]
    data20plus = data[data[:,0] > segment_x, :]

    indices0_20, indices20, model20, alpha_z0 = my_ransac_v5(data0_20,
                                                           lidar_height=-1.73+0.2,
                                                           )

    indices20plus, indices2, model2, alpha_z = my_ransac_v5(data20plus)



    if (indices20plus is not None) and (indices0_20 is not None):
        return np.vstack((
            data0_20,
            data20plus)), np.hstack(( indices0_20, indices20plus+data0_20.shape[0]))
    elif (indices20plus is not None) and (indices0_20 is None):
        return np.vstack((
            data20plus,
            data0_20)), indices20plus
    elif (indices20plus is None) and (indices0_20 is not None):
        return np.vstack((
            data0_20,
            data20plus)), indices0_20
    else:
        return None, None

同上次一样，测试100张数据，表现的很好，墙面问题解决了，也基本上没有大的路面会被漏掉了；

其中法向量与Z轴夹角的阈值是 0.05弧度，也就是差不多2-3角度的样子，0.03好像差别也不大；

分段的距离阈值是20，也就是说20米距离之后的路面是一个平面去拟合，前面是另外一个平面拟合，我这里只关注x>0的路面，车子后面的雷达数据直接过滤掉了，所以如果你是用的全部数据，

可能也会有点拟合的不是很好。

高度过滤阈值 {-1.73-0.2 ~ -1.73+0.3 }；

效果如下图，这张图之前墙壁会被误检测，地面也会漏掉前面的部分：

偶尔有小块地方漏检：

由于分段两次拟合，总体耗时差不多是之前的两倍，约为38ms;

总体来说，效果已经非常好了，最后再说下我认为的它的优点和缺点：

---

优点：

1 速度比较快；

2 相对高度差和其他栅格高度差的方法来说，它不会把那种平行于地面但是实际在空中的平面误检测出来；

---

缺点：

1 由于找平面是随机抽样的，可能会在某一帧耗费较长时间，而在另外一帧耗费很短的时间，但是目前来看，还没有出现很夸张的情况，基本上都是几十次或几百次就找到了；

2 对于路边的倾斜路面无能为力；