点云处理之模型拟合

Model fitting(模型拟合)

课程地址: 三维点云处理 - 深蓝学院 - 专注人工智能与自动驾驶的学习平台 (shenlanxueyuan.com)
Git链接: 计算机视觉信息汇总

1. Least Square（ 最小二乘法)

基本原理

可以参考我之前的链接: 机器学习之回归分析_victor_manches的博客-CSDN博客, 简单的来说就是找到一条令所有点到这条直线的距离和最短。

优点: 简单，快速

缺点: 对噪声不鲁棒，残差的方向的效果一致

解决方法:(使用Huber Loss / Focal Loss / Cauchy Loss)

代码实现

    def least_square(self, points):
        '''自实现的LSE'''
        # 构造系数矩阵
        A = np.c_[points[:,0], points[:,1], np.ones(points.shape[0])]  
         # 构造矩阵b
        b = points[:,2] 
        # 计算参数向量w
        w = np.dot(np.dot(np.linalg.inv(np.dot(A.T, A)), A.T), b)  
        a, b, c = w[0], w[1], -1
        d = w[2]
        n = np.array([a , b ,c])
        return n ,  d

---

2. RANSAC

基本原理

对于三维点云数据，我们更加优选与RANSAC

算法流程:

1. 随机选择(有放回)的抽最小建模数量的点，对于点云来说就是3个点，对于平面来说就是2个点。

2. 对随机抽样的点建模model，得到一个模型

3. 计算所有点到model的距离，对于小于**gap**的记作属于这个model的点

4. 重复**N**次 步骤123 ， 选择包含点最多的model (可以early stop, support data num > inlier ratio)

5. (Trick)对这个model 和其包含的点重新做一次LSE

上面涉及到了两个参数gap 和N次重复实现N， 理论上可以根据卡方分布gap,但是由于实际过程不知道数据点的标准差所以还是自己会去调试。而对于重复实验次数N，可以根据outlier rate(离群点占比)百分比来确定,下表是99%置信度取到至少一个是好的概率:

s 是最少建模数量点

优点:

• 简单
• 对离群点更加鲁棒，实际应用更加广泛

缺点：

• 对于gap需要自定义
• 当inlier ratio比较低的时候需要大量数据

代码实现

    def fit_transform(self , data):
        Dimension = data.shape[1]
        best_points = []
  
        for i in range(self.iter):
            select_points = data[np.random.choice(range(data.shape[0]) , Dimension)]
            n , d = self.modelling(select_points) #求出法向量N 和截距 d
            points_set = []
            for id , p in enumerate(data):
                dis_plane = np.abs(np.dot(n, p) + d) / np.sqrt(np.sum(n ** 2)) #计算所有点到平面的距离
                if dis_plane < self.gap: #小于阈值就加入这个集合
                    points_set.append(id)
            if len(points_set) > len(best_points):
                best_points = points_set
                #early stopping
                if len(best_points) >= data.shape[0] * self.inner_ratio:
                    break
  
        #对最后的RANSAC的点再进行一次LSE，
        fit_data = data[best_points]
        n , d = self.least_square(fit_data)
  
        last_points_set = []
        for id , p in enumerate(data):
            dis_plane = np.abs(np.dot(n, p) + d) / np.sqrt(np.sum(n ** 2)) #计算所有点到平面的距离
            if dis_plane < self.gap:
                last_points_set.append(id)
        print(f'RANSAC data: {len(best_points)} , after LSE data :{len(last_points_set)} and {len(last_points_set) - len(best_points)} has been added', )
        return np.array(last_points_set), data[last_points_set]

    def modelling(self , points):
        '''三维线性拟合
        "{n[0]}x + {n[1]}y + {n[2]}z + {D} = 0
        '''
        point1 , point2 , point3  = points[0] ,  points[1] ,  points[2]
        # 计算向量AB和向量AC
        AB = [point2[i] - point1[i] for i in range(3)]
        AC = [point3[i] - point1[i] for i in range(3)]
  
        # 计算法向量n
        n = [AB[1] * AC[2] - AC[1] * AB[2],
            AB[2] * AC[0] - AC[2] * AB[0],
            AB[0] * AC[1] - AC[0] * AB[1]]
  
        # 代入一个点，求出D
        D = -(n[0] * point1[0] + n[1] * point1[1] + n[2] * point1[2])
  
        # 返回平面方程
        return np.array(n) , D

---

3. Hough Transform

基本原理

其根本思想就是将原始坐标系里的线可以转换成参数坐标系里的点，或者反过来将原始空间里的点转换成参数空间里的线也是可以的。如下图: 参数空间里的交点就是我们求解答案。

但是实际过程中很难直接求得解集，所以我们可以采用投票的方式，将参数空间量化后，固定一个参数比如说a，针对不同a的取值，算出每个直线的b的取值，最后取投票最高的作为解。

Trick:

• 实际过程中，由于只用a和b的话无法表达 x = costant 这条线所以可以使用极坐标系来表明参数x * cosθ + y * sinθ = r 将原始的参数{a , b} 转换成 { θ , r }
• 对于分辨率，是一个精度和速度的tradeoff，可以自己调整
• 对参数空间里的投票做一个Gussion smooth 可以减少local minial
• 对于多个参数，先固定一个参数，然后用引入一个变量θ去表示剩下两个参数，依次类推

算法流程:

1. 设定参数的分辨率和范围，将参数量化
2. 对每个数据点，遍历不同的参数取值，固定一个参数得到另一个参数的值，对其进行投票
3. 得到的票数最多的网格就是对应的参数的取值

优点：

• 对噪声鲁棒
• 泛化能力强
• 对数据缺失的点的拟合能力更加鲁棒

缺点:

• 计算量大，所以一般只用在23维的数据

代码实现(待更新)

---

4. 可视化展示(地面)