点群データの平面推定_part3

やること

• 平面の導出
• RANSACの勉強

前回の記事

techsho.hatenablog.com

RANSACの勉強

前回は理想的な点群に対して、最小二乗平面を推定した。 今回は、ノイズのあるデータを考える。

ノイズデータの平面推定

前回のコードに対してノイズ成分を付加する

import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
%matplotlib inline
A = 0; B = 0; C = -1 ; D = 1

points = []
cnt = 0
for x in np.arange(0, 10, 0.5):
  for y in np.arange(0, 10, 0.5):
    for z in np.arange(0, 10, 0.5):
      if(abs(A*x + B*y + C*z + D) < 1e-8):
        points.append([x, y, z])
        if((x+y+z) == 15): points.append([x, y, z*1000])
        cnt+=1
points = np.asarray(points).T

fig = plt.figure()
ax = Axes3D(fig)
ax.set_xlabel("X")
ax.set_ylabel("Y")
ax.set_zlabel("Z")

ax.scatter3D(points[0],points[1],points[2])

plt.show()

prePoints = np.copy(points)
points = points.T
vecB = -points[:,2]
points[:,2] = 1
dim = np.dot(points.T, points)
vec = np.dot(vecB, points)
ans = np.dot(np.linalg.inv(dim), vec)


A = ans[0]; B = ans[1]; C = ans[2]
xmesh, ymesh = np.meshgrid(np.linspace(0, 10, 20),
                            np.linspace(0, 10, 20))
print(xmesh.shape, ymesh.shape)
zmesh = (C + A * xmesh.ravel() +
        B * ymesh.ravel()).reshape(xmesh.shape)

fig = plt.figure()
ax = Axes3D(fig)
ax.set_xlabel("X")
ax.set_ylabel("Y")
ax.set_zlabel("Z")
from matplotlib.ticker import ScalarFormatter
ax.zaxis.set_major_formatter(ScalarFormatter(useMathText=True))
ax.ticklabel_format(style="sci",  axis="z",scilimits=(0,0))
ax.scatter(prePoints[0],prePoints[1],prePoints[2])
ax.plot_wireframe(xmesh, ymesh, zmesh, color='r')

このように、ノイズ成分の影響で平面推定にずれが生じる このノイズ成分を除去するためにRandom Sample Consensus(RANSAC)を使う

RANSAC

前回の記事では、Open3dを使った平面推定を行ったが、その際にRANSACによるノイズ除去が実行されていた。
RANSACとは、簡単に言うと以下の処理をする

1. すべての点群データから、モデル導出に必要な数だけランダムに点群を選ぶ
   • 直線なら2点
   • 平面なら3点
2. 最小二乗法などで、モデル推定(モデルパラメータ導出)
   • 直線なら直線の方程式がモデルになる
   • 平面なら平面の方程式がモデルになる
3. 推定したモデルに対して、点群情報を入力し、実値とモデルを使った推定値の差を取る
4. 差が閾値以下ならモデルにフィットする点、閾値以上なら外れ値として定義
5. フィットする点が閾値以上集まったら、推定モデルは正しいモデルの候補の一つとする
6. 1.~5.を任意の数だけ繰り返し、モデルの候補を作る
7. モデルの候補から最も良いモデルを選ぶ

以上が、RANSACのイメージである

実行したコードを以下に示す

def fit_plane(samples):
  if(len(samples) < 3):
    print("error: The number of samples is small.")
    return 0
  vecB = -samples[:,2]
  samples[:,2] = 1
  dim = np.dot(samples.T, samples)
  print("dim")
  print(dim)
  vec = np.dot(vecB, samples)
  print("vec")
  print(vec)
  print("inv")
  print(np.linalg.inv(dim))
  ans = np.dot(np.linalg.inv(dim), vec)
  return ans

def check_outlier(coeff, p):
  return np.abs((coeff[0]*p[0]+coeff[1]*p[1]+coeff[2]) - (-p[2]))

def ransac(data, n=3, k=100, t=2.0, d=300):
    good_models = []
    good_model_errors = []
    iterations = 0
    while iterations < k:
        sample = data[np.random.choice(len(data), n, False)]
        coeff = fit_plane(sample)
        # print(coeff)

        inliers = []
        for points in data:
            if (points == sample).all(1).any(): continue
            if check_outlier(coeff, points) > t:
                continue
            else:
                inliers.append(points)
        # print(len(inliers))
        if len(inliers) > d:
            error_array = []
            for points in data:
                error_array.append(check_outlier(coeff, points))
            error_array = np.asarray(error_array)
            ave_error = np.mean(error_array)
            good_models.append(coeff)
            good_model_errors.append(ave_error)
        iterations += 1

    best_model_index = np.argmin(good_model_errors)
    return good_models[best_model_index]

a, b, c = ransac(prePoints.T)
print(a, b, c)

実行結果(ax+by+c = -z)は、

a = -1.1102230246251565e-16
b =  0.0
c = -1.0

となる。

今回の方程式の解き方では、逆行列が生成できないケースが出てくる(singular matrix)