【机器学习】数据拟合线性&平面

【Ransac算法】

官方demo：https://scipy.github.io/old-wiki/pages/Cookbook/RANSAC803e.html?action=AttachFile&do=view&target=ransac.py

线性与平面拟合代码：https://github.com/falcondai/py-ransac/blob/master/line_fitting.py

算法基本思想和流程

RANSAC是通过反复选择数据集去估计出模型，一直迭代到估计出认为比较好的模型。
具体的实现步骤可以分为以下几步：

1. 选择出可以估计出模型的最小数据集；(对于直线拟合来说就是两个点，对于计算Homography矩阵就是4个点)
2. 使用这个数据集来计算出数据模型；
3. 将所有数据带入这个模型，计算出“内点”的数目；(累加在一定误差范围内的适合当前迭代推出模型的数据)
4. 比较当前模型和之前推出的最好的模型的“内点“的数量，记录最大“内点”数的模型参数和“内点”数；
5. 重复1-4步，直到迭代结束或者当前模型已经足够好了(“内点数目大于一定数量”)。

迭代次数推导

这里有一点就是迭代的次数我们应该选择多大呢？这个值是否可以事先知道应该设为多少呢？还是只能凭经验决定呢？
这个值其实是可以估算出来的。下面我们就来推算一下。

                                 

“内点”的概率t通常是一个先验值。然后P是我们希望RANSAC得到正确模型的概率。如果事先不知道tt的值，可以使用自适应迭代次数的方法。也就是一开始设定一个无穷大的迭代次数，然后每次更新模型参数估计的时候，用当前的“内点”比值来当成是tt估算出迭代次数。

用Python实现直线拟合

import numpy as np
import matplotlib.pyplot as plt
import random
import math

# 数据量。
SIZE = 50
# 产生数据。np.linspace 返回一个一维数组，SIZE指定数组长度。
# 数组最小值是0，最大值是10。所有元素间隔相等。
X = np.linspace(0, 10, SIZE)
Y = 3 * X + 10

fig = plt.figure()
# 画图区域分成1行1列。选择第一块区域。
ax1 = fig.add_subplot(1,1, 1)
# 标题
ax1.set_title("RANSAC")


# 让散点图的数据更加随机并且添加一些噪声。
random_x = []
random_y = []
# 添加直线随机噪声
for i in range(SIZE):
    random_x.append(X[i] + random.uniform(-0.5, 0.5)) 
    random_y.append(Y[i] + random.uniform(-0.5, 0.5)) 
# 添加随机噪声
for i in range(SIZE):
    random_x.append(random.uniform(0,10))
    random_y.append(random.uniform(10,40))
RANDOM_X = np.array(random_x) # 散点图的横轴。
RANDOM_Y = np.array(random_y) # 散点图的纵轴。

# 画散点图。
ax1.scatter(RANDOM_X, RANDOM_Y)
# 横轴名称。
ax1.set_xlabel("x")
# 纵轴名称。
ax1.set_ylabel("y")

# 使用RANSAC算法估算模型
# 迭代最大次数，每次得到更好的估计会优化iters的数值
iters = 100000
# 数据和模型之间可接受的差值
sigma = 0.25
# 最好模型的参数估计和内点数目
best_a = 0
best_b = 0
pretotal = 0
# 希望的得到正确模型的概率
P = 0.99
for i in range(iters):
    # 随机在数据中红选出两个点去求解模型
    sample_index = random.sample(range(SIZE * 2),2)
    x_1 = RANDOM_X[sample_index[0]]
    x_2 = RANDOM_X[sample_index[1]]
    y_1 = RANDOM_Y[sample_index[0]]
    y_2 = RANDOM_Y[sample_index[1]]

    # y = ax + b 求解出a，b
    a = (y_2 - y_1) / (x_2 - x_1)
    b = y_1 - a * x_1

    # 算出内点数目
    total_inlier = 0
    for index in range(SIZE * 2):
        y_estimate = a * RANDOM_X[index] + b
        if abs(y_estimate - RANDOM_Y[index]) < sigma:
            total_inlier = total_inlier + 1

    # 判断当前的模型是否比之前估算的模型好
    if total_inlier > pretotal:
        iters = math.log(1 - P) / math.log(1 - pow(total_inlier / (SIZE * 2), 2))
        pretotal = total_inlier
        best_a = a
        best_b = b

    # 判断是否当前模型已经符合超过一半的点
    if total_inlier > SIZE:
        break

# 用我们得到的最佳估计画图
Y = best_a * RANDOM_X + best_b

# 直线图
ax1.plot(RANDOM_X, Y)
text = "best_a = " + str(best_a) + "\nbest_b = " + str(best_b)
plt.text(5,10, text,
         fontdict={'size': 8, 'color': 'r'})
plt.show()

最后得到结果如下图：

【SVD分解】

空间平面方程的一般表达式为：

Ax+By+Cz+D=0;则有：平面法向量为n=（A,B,C）.

第一种方法： 对于空间中n个点（n=3），空间中的离散点得到拟合平面，其实这就是一个最优化的过程。即求这些点到某个平面距离最小和的问题。由此，我们知道一个先验消息，那就是该平面一定会过众散点的平均值。接着我们需要做的工作就是求这个平面的法向量。根据协方差矩阵的SVD变换，最小奇异值对应的奇异向量就是平面的方向。

第二种方法：使用法线方法， 对于空间中n个点（n=3），若已获得点云法线使用合适的方法剔除离群点，计算点云的形心P；

若在已经获得法线的点云中，可以对法线进行剔除离散点之后，求取最小方差的均值，直接求得法线方向N( alpha, beta, theta )；

使用点法式描述三维平面；或者根据形心P和法线方向，计算出平面方程的一般式。

使用法线多次聚类：完成场景平面提取

使用法线两次聚类：第一次根据法线方向进行聚类，使用一个欧式距离约束，找出方向接近的簇S(1)，这样得到的S(1)内的集合，每一类指向了大致相同的方向，但距离上并不一定接近；第二次，再次根据点云的空间位置进行聚类，对S(1)的每一簇内再次进行基于距离的聚类，找出每一簇内位置接近的类别，这样再次对集合进行划分，得到的每一类方向大致相同，而位置较近，可以假设为一个平面的点。

此外，若考虑到平面密度要求，还可以再根据密度进行一次聚类，把密度较低的平面从集合中踢出去。

【 点云匹配：ICP算法（Iterative Closest Point迭代最近点）】

（Iterative Closest Point迭代最近点）算法是一种点集对点集配准方法，

ICP算法能够使不同的坐标下的点云数据合并到同一个坐标系统中，首先是找到一个可用的变换，配准操作实际是要找到从坐标系1到坐标系2的一个刚性变换。ICP算法本质上是基于最小二乘法的最优配准方法。该算法重复进行选择对应关系点对， 计算最优刚体变换，直到满足正确配准的收敛精度要求。

                                 

假设给两个三维点集 X1 和 X2，ICP方法的配准步骤如下：

第一步，计算X2中的每一个点在X1 点集中的对应近点；

第二步，求得使上述对应点对平均距离最小的刚体变换，求得平移参数和旋转参数；

第三步，对X2使用上一步求得的平移和旋转参数，得到新的变换点集；

第四步， 如果新的变换点集与参考点集满足两点集的平均距离小于某一给定阈值，则停止迭代计算，否则新的变换点集作为新的X2继续迭代，直到达到目标函数的要求。

 最近点对查找：对应点的计算是整个配准过程中耗费时间最长的步骤，查找最近点，利用k－d tree提高查找速度 K－d tree 法建立点的拓扑关系是基于二叉树的坐标轴分割，构造 k－d tree的过程就是按照二叉树法则生成，首先按 X轴寻找分割线，即计算所有点的x值的平均值，以最接近这个平均值的点的x值将空间分成两部分，然后在分成的子空间中按 Y轴寻找分割线，将其各分成两部分，分割好的子空间在按X轴分割……依此类推，最后直到分割的区域内只有一个点。这样的分割过程就对应于一个二叉树，二叉树的分节点就对应一条分割线，而二叉树的每个叶子节点就对应一个点。这样点的拓扑关系就建立了。