毕设中有一个小模块是要在点云中做平面的拟合，翻阅了一些资料后觉得用最简单的最小二乘法加上AX+BY+CZ=D的平面方程就可以实现（使用SVD进行最小二乘法拟合），但是当实现过后发现效果并不是很好，考虑原因应该是因为最小二乘法受误差值当影响比较大(代码也放在文末)。继续查阅资料决定选择RANSAC实现平面拟合。
原理解释
这里参考维基百科对于RANSAC算法原理进行简要解释：
这里要先解释两个名词
内点：拟合模型用到的点
外点：误差点
（个人的理解感觉很像随机森林，投票制）(直线拟合为例)
1 随机选出两个点。（选择出可以估计出模型的最小数据集，如果是平面就选择3个点）
2 两点确定一条直线，拟合出这条直线
3 将所有数据带入这个模型，计算出“内点”的数目；(累加在一定误差范围内的适合当前迭代推出模型的数据)
4 比较当前模型和之前推出的最好的模型的“内点“的数量，记录最大“内点”数的模型参数和“内点”数；
5 重复1-4步，直到迭代结束或者当前模型已经足够好了(“内点数目大于一定数量”)。
(PS:这一部分没有提及对于迭代次数设置的推导，感兴趣的可以去查一查)
python代码实现
废话不多说，上代码
import numpy as np
from ransac import *
import random
def augment(xyzs):
axyz = np.ones((len(xyzs), 4))
axyz[:, :3] = xyzs
return axyz
def estimate(xyzs):
axyz = augment(xyzs[:3])
return np.linalg.svd(axyz)[-1][-1, :]
def is_inlier(coeffs, xyz, threshold):
return np.abs(coeffs.dot(augment([xyz]).T)) < threshold
def run_ransac(data, estimate, is_inlier, sample_size, goal_inliers, max_iterations, stop_at_goal=True, random_seed=None):
best_ic = 0
best_model = None
random.seed(random_seed)
# random.sample cannot deal with "data" being a numpy array
data = list(data)
for i in range(max_iterations):
s = random.sample(data, int(sample_size))
m = estimate(s)
ic = 0
for j in range(len(data)):
if is_inlier(m, data[j]):
ic += 1
print(s)
print('estimate:', m,)
print('# inliers:', ic)
if ic > best_ic:
best_ic = ic
best_model = m
if ic > goal_inliers and stop_at_goal:
break
print('took iterations:', i+1, 'best model:', best_model, 'explains:', best_ic)
return best_model, best_ic
if __name__ == '__main__':
import matplotlib
import matplotlib.pyplot as plt
from mpl_toolkits import mplot3d
fig = plt.figure()
ax = mplot3d.Axes3D(fig)
def plot_plane(a, b, c, d):
xx, yy = np.mgrid[:10, :10]
return xx, yy, (-d - a * xx - b * yy) / c
n = 100
max_iterations = 100
goal_inliers = n * 0.3
# test data
xyzs = np.random.random((n, 3)) * 10
xyzs[:50, 2:] = xyzs[:50, :1]
ax.scatter3D(xyzs.T[0], xyzs.T[1], xyzs.T[2])
# RANSAC
m, b = run_ransac(xyzs, estimate, lambda x, y: is_inlier(x, y, 0.01), 3, goal_inliers, max_iterations)
a, b, c, d = m
xx, yy, zz = plot_plane(a, b, c, d)
ax.plot_surface(xx, yy, zz, color=(0, 1, 0, 0.5))
plt.show()
image.png
最小二乘法平面拟合
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import numpy as np
N_POINTS = 10
TARGET_X_SLOPE = 2
TARGET_y_SLOPE = 3
TARGET_OFFSET = 5
EXTENTS = 5
NOISE = 5
# create random data
xs = [np.random.uniform(2*EXTENTS)-EXTENTS for i in range(N_POINTS)]
ys = [np.random.uniform(2*EXTENTS)-EXTENTS for i in range(N_POINTS)]
zs = []
for i in range(N_POINTS):
zs.append(xs[i]*TARGET_X_SLOPE + \
ys[i]*TARGET_y_SLOPE + \
TARGET_OFFSET + np.random.normal(scale=NOISE))
# plot raw data
plt.figure()
ax = plt.subplot(111, projection='3d')
ax.scatter(xs, ys, zs, color='b')
# do fit
tmp_A = []
tmp_b = []
for i in range(len(XS)):
tmp_A.append([xs[i], ys[i], 1])
tmp_b.append(zs[i])
b = np.matrix(tmp_b).T
A = np.matrix(tmp_A)
# Manual solution
fit = (A.T * A).I * A.T * b
errors = b - A * fit
residual = np.linalg.norm(errors)
# Or use Scipy
# from scipy.linalg import lstsq
# fit, residual, rnk, s = lstsq(A, b)
print("solution:")
print("%f x + %f y + %f = z" % (fit[0], fit[1], fit[2]))
print("errors:")
print(errors)
print("residual:")
print(residual)
# plot plane
xlim = ax.get_xlim()
ylim = ax.get_ylim()
X,Y = np.meshgrid(np.arange(xlim[0], xlim[1]),
np.arange(ylim[0], ylim[1]))
Z = np.zeros(X.shape)
for r in range(X.shape[0]):
for c in range(X.shape[1]):
Z[r,c] = fit[0] * X[r,c] + fit[1] * Y[r,c] + fit[2]
ax.plot_wireframe(X,Y,Z, color='k')
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_zlabel('z')
plt.show()
image.png
