畢設(shè)中有一個小模塊是要在點(diǎn)云中做平面的擬合乎串，翻閱了一些資料后覺得用最簡單的最小二乘法加上AX+BY+CZ=D的平面方程就可以實現(xiàn)（使用SVD進(jìn)行最小二乘法擬合）绎谦，但是當(dāng)實現(xiàn)過后發(fā)現(xiàn)效果并不是很好，考慮原因應(yīng)該是因為最小二乘法受誤差值當(dāng)影響比較大(代碼也放在文末)。繼續(xù)查閱資料決定選擇RANSAC實現(xiàn)平面擬合。
原理解釋
這里參考維基百科對于RANSAC算法原理進(jìn)行簡要解釋：
這里要先解釋兩個名詞
內(nèi)點(diǎn)：擬合模型用到的點(diǎn)
外點(diǎn)：誤差點(diǎn)
（個人的理解感覺很像隨機(jī)森林幼东，投票制）(直線擬合為例)
1 隨機(jī)選出兩個點(diǎn)。（選擇出可以估計出模型的最小數(shù)據(jù)集，如果是平面就選擇3個點(diǎn)）
2 兩點(diǎn)確定一條直線根蟹，擬合出這條直線
3 將所有數(shù)據(jù)帶入這個模型脓杉，計算出“內(nèi)點(diǎn)”的數(shù)目；(累加在一定誤差范圍內(nèi)的適合當(dāng)前迭代推出模型的數(shù)據(jù))
4 比較當(dāng)前模型和之前推出的最好的模型的“內(nèi)點(diǎn)“的數(shù)量简逮，記錄最大“內(nèi)點(diǎn)”數(shù)的模型參數(shù)和“內(nèi)點(diǎn)”數(shù)球散；
5 重復(fù)1-4步，直到迭代結(jié)束或者當(dāng)前模型已經(jīng)足夠好了(“內(nèi)點(diǎn)數(shù)目大于一定數(shù)量”)散庶。
(PS:這一部分沒有提及對于迭代次數(shù)設(shè)置的推導(dǎo)蕉堰，感興趣的可以去查一查)
python代碼實現(xiàn)
廢話不多說，上代碼
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()
最小二乘法平面擬合
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()
