😎
Sampson距離で曲線と点との近似距離を求める
はじめに
サンプソン距離（Sampson distance）は、「制約式からの残差を、その勾配で正規化」した幾何距離の一次近似。例えば2次曲線のような高次元の制約式でも、曲線からの距離の近似値を求めることができる。
定義はシンプルで：
OpenCVにも、cv::sampsonDistance 関数があるが、これはエピポーラ幾何やFundatamental行列の文脈で使われるもので、一般的な関数のサンプソン距離計算には使えない。
https://amroamroamro.github.io/mexopencv/matlab/cv.sampsonDistance.html
定義
ある関数の理想では解が0となるとする。
しかし、現実世界の観測データはノイズなどで
この残差が理想的な関数からどれだけ離れているかを、サンプソン距離を使用して表すと、観測点
- 分子：誤差（残差）の二乗
- 分母：勾配ノルムの二乗（2変数の場合は、偏微分のノルムの合計）
上記の式は二乗したスケールなので、元の距離のスケールに戻したい場合は平方根を取る。
本章のplotやpython codeはこちら
Sampson距離計算の例
一次関数
シンプルな例として、直線の例を考える。
一次関数の偏微分：
サンプソン距離：
一次関数のサンプソン距離は点から直線までの垂線距離の二乗と厳密に一致する。
例：y=x
点 (1,\,1.6)
- 残差
f(1,1.6)= 1-1.6 =-0.6 - 偏微分（勾配値）：
\frac{\partial f}{\partial x} = 1
\frac{\partial f}{\partial y} = -1 - Sampson距離:
\mathrm{sd(1,1.6)}= \frac{(-0.6)^2}{1^2+(-1)^2}=\dfrac{0.36}{2}=0.18
Sampson距離の平方根を取ると
円
円の公式：
-
x_0, y_0 -
r
偏微分：
サンプソン距離：
例）中心が原点の単位円（半径1）
点 (1.4,-0.8)
- 残差
f(1.4,-0.8)=1.4^2+(-0.8)^2-1=1.6 - 勾配値：
\frac{\partial f}{\partial x}=2x=2*1.4=2.8
\frac{\partial f}{\partial y}=2*(-0.8)=-1.6 - Sampson距離:
\mathrm{sd}=\frac{(1.6)^2}{(2.8)^2+(-1.6)^2}=\frac{2.56}{7.84+2.56}=\frac{2.56}{10.4}\approx 0.246
Sampson距離の平方根を取ると
円の真の距離は中心座標からのユークリッド距離から半径を引いたものなので、
例）中心が (1,2)
点 (4,2)
-
残差：
f(4,2)=(4-1)^2+(2-2)^2-4=9-4=5 -
勾配値:
\frac{\partial f}{\partial x}=2(x-1)=2(4-1)=6
\frac{\partial f}{\partial y}=2(y-2)=2(2-2)=0 -
Sampson距離:
\mathrm{sd}=\frac{(5)^2}{6^2+0^2}=0.6944
平方根を取ると
真の距離は
楕円
楕円は、中心
楕円の公式：
-
a -
b -
(x_0,y_0) -
\theta \theta=0
この式を展開すると、一般二次曲線の形
に変換できる。ここで
偏微分：
サンプソン距離は：
例）中心が原点の楕円 a=2, b=1, \cos\theta=1, \sin\theta=0
この楕円は以下の関数で表される：
これを一般二次曲線の形（
点 (2.5,0)
- 残差：
f(2.5,0)=\tfrac{1}{4}\cdot (2.5)^2 + 0 - 1 = 1.5625 - 1 = 0.5625 - 偏微分：
\frac{\partial f}{\partial x}=2Ax + By + D = 2*\tfrac{1}{4}*2.5 + 0 + 0 = 1.25
\frac{\partial f}{\partial y}=Bx + 2Cy + E = 0*2.5 + 2*1*0 + 0 = 0 - Sampson距離：
\mathrm{sd}=\dfrac{(0.5625)^2}{(1.25)^2+0^2}=\dfrac{0.3164}{1.5625}=0.2025
Sampson距離の平方根を取ると
真の距離は
点 (1,0)
- 残差：
f(1,0)=\tfrac{1}{4}\cdot 1^2 + 0 - 1= -0.75 - 偏微分：
\frac{\partial f}{\partial x}=2Ax + By + D=2*\tfrac{1}{4}+0*0+0 = 0.5
\frac{\partial f}{\partial y}=Bx + 2Cy + E=0*1+2*1*0+0=0 - Sampson距離：
\mathrm{sd}=\dfrac{(-0.75)^2}{(0.5)^2+0^2}=\dfrac{0.5625}{0.25}=2.25
Sampson距離の平方根を取ると
真の距離は1なので、0.5の誤差が生じている。
例）中心が(1,2) a=3,\ b=2,\ (x_0,y_0)=(1,2),\ \theta=30^\circ
長軸
（
これを一般二次曲線（
点 (4.5,3.5)
- 残差
f(x,y)
- 偏微分（勾配）
- サンプソン距離（二乗）
- サンプソン距離（平方根）
真の距離は約
楕円と点の距離計測
# Re-run computations (environment was reset). Compute true distance for the rotated ellipse and point (4.5,3.5).
import numpy as np
def to_local(p, center, theta):
c, s = np.cos(theta), np.sin(theta)
x, y = p[0] - center[0], p[1] - center[1]
return np.array([ c*x + s*y, -s*x + c*y ])
def to_world(pl, center, theta):
c, s = np.cos(theta), np.sin(theta)
x, y = pl
X = c*x - s*y + center[0]
Y = s*x + c*y + center[1]
return np.array([X, Y])
def closest_point_on_axis_aligned_ellipse(u, v, a, b, tol=1e-12, max_iter=200):
su, sv = (1.0 if u>=0 else -1.0), (1.0 if v>=0 else -1.0)
U, V = abs(u), abs(v)
if U==0 and V==0:
return np.array([a, 0.0])
inside = (U**2)/(a**2) + (V**2)/(b**2) < 1.0
def g(lmbd):
return (a*a*U*U)/((lmbd + a*a)**2) + (b*b*V*V)/((lmbd + b*b)**2) - 1.0
if inside:
lo = -min(a*a, b*b) + 1e-15
hi = 0.0
else:
lo = 0.0
hi = 1.0
while g(hi) > 0.0 and hi < 1e12:
hi *= 2.0
lmbd = None
for _ in range(max_iter):
mid = 0.5*(lo + hi)
val = g(mid)
if abs(val) < tol:
lmbd = mid; break
# sign handling
if val > 0:
lo = mid
else:
hi = mid
if lmbd is None:
lmbd = mid
ux = (a*a * U) / (lmbd + a*a)
vy = (b*b * V) / (lmbd + b*b)
return np.array([su*ux, sv*vy])
def true_ellipse_distance(point, center, a, b, theta):
uv = to_local(point, center, theta)
q_local = closest_point_on_axis_aligned_ellipse(uv[0], uv[1], a, b)
q_world = to_world(q_local, center, theta)
return np.linalg.norm(point - q_world), q_world, uv, q_local
center = np.array([1.0, 2.0])
a, b = 3.0, 2.0
theta = np.deg2rad(30.0)
P = np.array([4.5, 3.5])
d_true, Q_world, uv, q_local = true_ellipse_distance(P, center, a, b, theta)
print("true distance:", d_true)
print("closest point (world):", Q_world)
print("point local uv:", uv)
print("closest local uv:", q_local)
true distance: 0.8284657791528559
closest point (world): [3.71230637 3.24330115]
point local uv: [ 3.78108891 -0.45096189]
closest local uv: [ 2.9705768 -0.27942281]
真の楕円距離との比較
サンプソン距離の利点は、高次関数（楕円など）に対しても非常に高速に近似距離を計算できることである。ということで、真の距離計算と比較してみる。
真の楕円距離計算とサンプソン距離計算の比較コード
import numpy as np
import matplotlib.pyplot as plt
from time import perf_counter
# ---------- Geometry helpers ----------
def to_local(p, center, theta):
c, s = np.cos(theta), np.sin(theta)
x, y = p[0] - center[0], p[1] - center[1]
# rotate world->local
u = c*x + s*y
v = -s*x + c*y
return np.array([u, v])
def to_world(pl, center, theta):
c, s = np.cos(theta), np.sin(theta)
x, y = pl[0], pl[1]
X = c*x - s*y + center[0]
Y = s*x + c*y + center[1]
return np.array([X, Y])
def closest_point_on_axis_aligned_ellipse_scalar(u, v, a, b, tol=1e-12, max_iter=200):
# Eberly 法のスカラー版（二分法）
su = 1.0 if u >= 0 else -1.0
sv = 1.0 if v >= 0 else -1.0
U, V = abs(u), abs(v)
if U == 0 and V == 0:
return np.array([a, 0.0]) # 原点にいる場合の最近点を便宜的に (a,0)
inside = (U*U)/(a*a) + (V*V)/(b*b) < 1.0
def g(lmbd):
return (a*a*U*U)/((lmbd + a*a)**2) + (b*b*V*V)/((lmbd + b*b)**2) - 1.0
if inside:
lo = -min(a*a, b*b) + 1e-15
hi = 0.0
else:
lo = 0.0
hi = 1.0
# 外部点は g(hi) <= 0 になるまで上限を拡張
tries = 0
while g(hi) > 0.0 and tries < 60:
hi = 2.0*hi if hi > 0 else 1.0
tries += 1
lmbd = 0.5*(lo + hi)
for _ in range(max_iter):
val = g(lmbd)
if abs(val) < tol:
break
if val > 0: # 右へ
lo = lmbd
else: # 左へ
hi = lmbd
lmbd = 0.5*(lo + hi)
ux = (a*a * U) / (lmbd + a*a)
vy = (b*b * V) / (lmbd + b*b)
return np.array([su*ux, sv*vy])
def true_ellipse_distance_loop(points, center, a, b, theta):
# 各点ごとに最近接点を求め、距離を返す（ループ版）
d = np.empty(points.shape[0])
for i, (x, y) in enumerate(points):
uv = to_local(np.array([x, y]), center, theta)
q_local = closest_point_on_axis_aligned_ellipse_scalar(uv[0], uv[1], a, b)
q_world = to_world(q_local, center, theta)
d[i] = np.hypot(x - q_world[0], y - q_world[1])
return d
# ---------- Implicit form & Sampson ----------
def implicit_coeffs_from_ellipse(center, a, b, theta):
cx, cy = center
ct, st = np.cos(theta), np.sin(theta)
A = (ct*ct)/(a*a) + (st*st)/(b*b)
B = 2*ct*st*(1/(a*a) - 1/(b*b))
C = (st*st)/(a*a) + (ct*ct)/(b*b)
D = -2*A*cx - B*cy
E = -B*cx - 2*C*cy
F = A*cx*cx + B*cx*cy + C*cy*cy - 1.0
return A, B, C, D, E, F
def sampson_distance_loop(points, coeffs):
# 各点ごとに Sampson 距離を計算（ループ版）
A, B, C, D, E, F = coeffs
out = np.empty(points.shape[0])
for i, (x, y) in enumerate(points):
f = A*x*x + B*x*y + C*y*y + D*x + E*y + F
fx = 2*A*x + B*y + D
fy = B*x + 2*C*y + E
denom = np.hypot(fx, fy)
if denom == 0:
denom = np.finfo(float).eps
out[i] = abs(f) / denom
return out
Benchmark setup
# 中心座標 1,2
center = np.array([1.0, 2.0])
# 長軸、短軸
a, b = 3.0, 2.0
# 回転角 30度
theta = np.deg2rad(30.0)
# 2次曲線の係数
coeffs = implicit_coeffs_from_ellipse(center, a, b, theta)
# ランダム点（同一集合）
rng = np.random.default_rng(42)
N = 20000
pts = np.column_stack([rng.uniform(-5, 7, size=N), rng.uniform(-5, 7, size=N)])
# warm-up（JITではないがキャッシュ効果や最初のオーバーヘッド回避用）
_ = sampson_distance_loop(pts[:50], coeffs)
_ = true_ellipse_distance_loop(pts[:50], center, a, b, theta)
新の楕円距離計算 計測（True
# true distance loop
t2 = perf_counter()
d_true_loop = true_ellipse_distance_loop(pts, center, a, b, theta)
t3 = perf_counter()
t_true_loop_ms = (t3 - t2) * 1e3
サンプソン距離 計測（Sampson
t0 = perf_counter()
d_s_loop = sampson_distance_loop(pts, coeffs)
t1 = perf_counter()
t_sampson_loop_ms = (t1 - t0) * 1e3
# 真の距離とSampson距離の誤差
abs_err = np.abs(d_true_loop - d_s_loop)
可視化
# ---------- Plot 1: runtime bar (ms) ----------
print("Times (ms): Sampson=", t_sampson_loop_ms, " True=", t_true_loop_ms)
plt.figure()
plt.bar(["Sampson (loop)","True (loop)"], [t_sampson_loop_ms, t_true_loop_ms])
plt.ylabel("Total time (ms)")
plt.title(f"Runtime (loop) for N={N} points")
plt.tight_layout()
plt.show()
# ---------- Plot 2: scatter plot of absolute error ----------
print("Mean abs error:", abs_err.mean(), "Median:", np.median(abs_err), "p95:", np.percentile(abs_err,95))
plt.figure(figsize=(6,5))
plt.scatter(d_true_loop, abs_err, s=6, alpha=0.4)
plt.xlabel("True distance to ellipse")
plt.ylabel("|True - Sampson| (absolute error)")
plt.title("Error vs. True Distance (Loop, N=5000)")
plt.grid(True, alpha=0.3)
plt.show()
中心が
明らかにサンプソン距離計算の方が速い。
計算速度
次に、真の距離とサンプソン距離の誤差をプロットすると、明らかに距離2辺りで大きな誤差が生じることが確認できた。それを無視すると、全体的には、点と楕円との距離が近いほど誤差が小さく、遠ざかるほど誤差が大きくなる傾向がある。
真の距離とサンプソン距離の誤差
誤差が大きい上位1%をマークしてみると、楕円の中心部に集中していることがわかる。これは、楕円の中心部では勾配が小さくなるため、サンプソン距離の分母が小さくなり、誤差が大きくなるためであると考えらえる。
Sampson距離が大きかった点をマーク
ちなみに、Sampson距離の平方根の値で色付けしてみる。円の外側で離れたところよりも、円の中心部が最も距離が大きくなっているのがわかる。
Sampson距離の平方根
【 RANSACによる曲線フィッティング 】
RANSACはRandom Sample Consensusの略で、外れ値に頑健なモデルフィッティング手法の一つ。ランダムにサンプリングした点群からモデルを推定し、他の点がそのモデルにどれだけ適合するかを評価する。
ランダムサンプリング ➔ 関数・モデル作成 ➔ 全点に対する距離計算 ➔ 距離閾値に収まった点の割合評価 を繰り返すため、全点に対する距離計算がボトルネックになる。
ここにサンプソン距離を用いることで、計算コストを削減できる。
以下にノイズの多い点群に対して、RANSACで目的関数にフィッティングを行う場合を考える。
共通関数
import numpy as np
import matplotlib.pyplot as plt
def ransac(points, fit_func, sd_func, min_samples, thresh, trials=1000, valid_check=None):
"""
points: 座標 (N,2) XY順
fit_func: フィッティングしたい関数
sd_func: その関数のSampson距離計算関数
min_samples: 最小サンプル数
thresh: サンプソン距離のしきい値
trials: 試行回数
valid_check: パラメータの有効性チェック関数
"""
rng = np.random.default_rng(0)
best_coef, best_inliers, best_inlier_num = None, None, -1
for i in range(trials):
idx = rng.choice(len(points), size=min_samples, replace=False)
coef = fit_func(points[idx])
if valid_check and not valid_check(coef):
continue
d = sd_func(points, coef)
inlier_mask = d < thresh
if inlier_mask.sum() > best_inlier_num:
best_inlier_num = inlier_mask.sum()
best_inliers = inlier_mask
# インライアだけで関数パラメーターを再計算
best_coef = fit_func(points[inlier_mask])
best_trial = i
return best_coef, best_inlier_num, best_inliers, best_trial
1. 放物線
- 方程式:
y=ax^2+bx+c - パラメーター決定に最低限必要な点の数：3点
デモデータ生成
def make_parabola_points(a=0.4, b=-0.8, c=1.2, n_in=200, n_out=200, noise=1, seed=1):
rng = np.random.default_rng(seed)
xs = rng.uniform(-4, 4, size=n_in)
ys = a*xs**2 + b*xs + c + rng.normal(0, noise, size=n_in)
inliers = np.column_stack([xs, ys])
outliers = np.column_stack([rng.uniform(-6,6,n_out), rng.uniform(-3,15,n_out)])
return np.vstack([inliers, outliers])
# 点群生成
pts = make_parabola_points()
# 可視化
plt.figure(figsize=(4,4))
xs = np.linspace(-6,6,400)
ys = coef[0]*xs**2 + coef[1]*xs + coef[2]
plt.scatter(pts[:,0], pts[:,1], s=8, alpha=0.3, label="all")
plt.legend(); plt.title("Before")
何となく放物線に沿った点群が見えるが、ノイズや外れ値が多いデータとなっている。
デモデータの点群
この点群に対して、RANSAC + Sampson距離で放物線をフィッティングするための関数を定義する。
二次関数用のパラメータ探索関数とSampson距離関数
def fit_parabola(points):
"""
与えられた点群から二次関数の係数 (a,b,c) を最小二乗で求める
"""
x,y = points[:,0], points[:,1]
X = np.stack([x**2, x, np.ones_like(x)], axis=1)
coef, *_ = np.linalg.lstsq(X, y, rcond=None)
return coef # (a,b,c)
def sd_parabola(points, coef):
"""
与えられた点群と二次関数の係数 (a,b,c) から Sampson 距離を計算する
"""
a,b,c = coef
x,y = points[:,0], points[:,1]
f = y - (a*x**2+b*x+c)
fx = -(2*a*x+b)
fy = 1
return np.abs(f)/np.hypot(fx,fy)
ではRANSACを実行してみる。
min_samples=3（放物線のパラメータ決定に必要な点の数）、thresh=0.2（Sampson距離の閾値）とする。
RANSAC実行
# RANSAC実行
coef, inlier_num, mask, trial = ransac(
pts, fit_parabola, sd_parabola, min_samples=3, thresh=0.2,
)
# フィッティング後の関数で再度Sampson距離計測
sd = sd_parabola(pts, coef)
# 可視化用にインライアとされた点を記録
inlier_mask = sd < 0.2
# フィット関数の式を文字列化
a, b, c = coef
equation = f"y = {a:.3f}x² + {b:.3f}x + {c:.3f}"
# 可視化
fig, ax = plt.subplots(figsize=(4,4))
xs = np.linspace(-6,6,400)
ys = a*xs**2 + b*xs + c
ax.scatter(pts[:,0], pts[:,1], s=8, alpha=0.3, label="all")
ax.scatter(pts[inlier_mask,0], pts[inlier_mask,1], s=8, label="inliers")
ax.plot(xs, ys, "r", lw=1, alpha=0.5)
ax.legend()
ax.set_title(f"Trial:{trial}, Inlier rate:{inlier_num/len(pts):.2f}")
# 下に式を追加（中央寄せ）
fig.text(0.5, -0.05, equation, ha='center', fontsize=12)
plt.tight_layout()
plt.show()
760回目の施行で点群の23%をインライアとする放物線が得られた。
フィッティングした放物線
2. 楕円
- 方程式:
f(x,y)=Ax^2+Bxy+Cy^2+Dx+Ey+F=0 - 条件:
B^2-4AC<0 - パラメーター決定に最低限必要な点の数：5点
デモデータ生成
def make_ellipse_points(center=(1,2), a=3, b=2, theta=np.deg2rad(25), n_in=200, n_out=200, noise=0.2, seed=2):
rng = np.random.default_rng(seed)
t = rng.uniform(0,2*np.pi,size=n_in)
ct, st = np.cos(theta), np.sin(theta)
X = center[0] + a*np.cos(t)*ct - b*np.sin(t)*st
Y = center[1] + a*np.cos(t)*st + b*np.sin(t)*ct
X += rng.normal(0,noise,n_in); Y += rng.normal(0,noise,n_in)
inliers = np.column_stack([X,Y])
outs = np.column_stack([rng.uniform(center[0]-5,center[0]+5,n_out),
rng.uniform(center[1]-5,center[1]+5,n_out)])
return np.vstack([inliers, outs])
# 点群生成
pts = make_ellipse_points()
# 可視化
plt.figure(figsize=(4,4))
xs = np.linspace(-6,8,500); ys = np.linspace(-6,8,500)
X,Y = np.meshgrid(xs,ys)
plt.scatter(pts[:,0],pts[:,1],s=6,alpha=0.3)
plt.legend()
plt.title("Before")
plt.show()
デモデータの点群
楕円用のパラメータ探索関数とSampson距離関数
# フィット（SVD）
def fit_conic(points):
x,y = points[:,0], points[:,1]
D = np.stack([x**2,x*y,y**2,x,y,np.ones_like(x)],axis=1)
_,_,Vt = np.linalg.svd(D)
p = Vt[-1,:]
return p/np.linalg.norm(p[:3])
# サンプソン距離
def sd_conic(points, p):
A,B,C,D,E,F = p
x,y = points[:,0], points[:,1]
f = A*x**2+B*x*y+C*y**2+D*x+E*y+F
fx = 2*A*x+B*y+D
fy = B*x+2*C*y+E
return np.abs(f)/np.hypot(fx,fy)
# 楕円判定
def is_ellipse(p):
A,B,C,_,_,_ = p
return (B*B - 4*A*C)<0
ではRANSACを実行してみる。
min_samples=5（楕円関数のパラメータ決定に必要な点の数）、thresh=0.2（Sampson距離の閾値）とする。
RANSAC実行
# 実行
coef, inlier_num, mask, trial = ransac(
pts, fit_conic, sd_conic, min_samples=5, thresh=0.2, valid_check=is_ellipse,
)
# フィッティング後の関数で再度Sampson距離計測
sd = sd_conic(pts, coef)
# 可視化用にインライアとされた点を記録
inlier_mask = sd < 0.2
def _fmt_term(coeff, symbol, is_first=False, power=""):
"""符号と0扱い込みで綺麗に整形。symbol='x^2' などを渡す"""
if abs(coeff) < 1e-12:
return "" # ほぼ0は省略
s = f"{abs(coeff):.4g}·{symbol}"
if is_first:
return ("" if coeff >= 0 else "−") + s
return (" + " if coeff >= 0 else " − ") + s
def conic_equation_string(p):
A, B, C, D, E, F = p
# x^2, xy, y^2, x, y, 1 の順に並べる
parts = []
first_added = False
t = _fmt_term(A, "x²", is_first=not first_added); parts.append(t); first_added |= bool(t)
t = _fmt_term(B, "xy", is_first=not first_added); parts.append(t); first_added |= bool(t)
t = _fmt_term(C, "y²", is_first=not first_added); parts.append(t); first_added |= bool(t)
t = _fmt_term(D, "x", is_first=not first_added); parts.append(t); first_added |= bool(t)
t = _fmt_term(E, "y", is_first=not first_added); parts.append(t); first_added |= bool(t)
# 定数項
if abs(F) >= 1e-12:
t = f"{' + ' if F>=0 else ' − '}{abs(F):.4g}"
if not first_added: # これが最初の項なら先頭の±を正規化
t = ("" if F>=0 else "−") + f"{abs(F):.4g}"
parts.append(t)
lhs = "".join(parts) if parts else "0"
return lhs + " = 0"
# ===== 可視化（等高線 + 係数の式を下部に表示） =====
fig, ax = plt.subplots(figsize=(4, 4))
xs = np.linspace(-6, 8, 500)
ys = np.linspace(-6, 8, 500)
X, Y = np.meshgrid(xs, ys)
A, B, C, D_, E, F = coef # Dと変数名が被るのを避けるため D_ に
Z = A*X**2 + B*X*Y + C*Y**2 + D_*X + E*Y + F
ax.scatter(pts[:, 0], pts[:, 1], s=6, alpha=0.3, label="points")
ax.scatter(pts[inlier_mask, 0], pts[inlier_mask, 1], s=12, label="inliers")
ax.contour(X, Y, Z, levels=[0], colors="r", linewidths=2)
ax.legend(loc="upper right")
ax.set_aspect("equal", adjustable="box")
ax.grid(True, alpha=0.3)
ax.set_title(f"Trial: {trial}, Inlier rate: {inlier_num/len(pts):.2f}")
# ここで式を下に表示
eq_text = conic_equation_string(coef)
# 図の下部に中央寄せで表示（等幅フォントで読みやすく）
fig.text(0.5, 0.02, eq_text, ha="center", va="bottom", fontsize=10, family="monospace")
plt.tight_layout(rect=[0, 0.06, 1, 1]) # 下のマージンを少し広げる
plt.show()
185回目の施行で点群の11%をインライアとする楕円が得られた。
フィッティングした楕円
3. 双曲線
- 方程式:
f(x,y)=Ax^2+Bxy+Cy^2+Dx+Ey+F=0 - 条件:
B^2−4AC>0 - パラメーター決定に最低限必要な点の数：5点
デモデータ生成
# 双曲線データ（近似的に作る）
def make_hyperbola_points(n_in=200, n_out=200, noise=0.2, seed=3):
rng = np.random.default_rng(seed)
t = rng.uniform(-2,2,n_in)
X = np.cosh(t) + rng.normal(0,noise,n_in)
Y = np.sinh(t) + rng.normal(0,noise,n_in)
inliers = np.column_stack([X,Y])
outs = np.column_stack([rng.uniform(-4,4,n_out), rng.uniform(-4,4,n_out)])
return np.vstack([inliers, outs])
def is_hyperbola(p):
A,B,C,_,_,_ = p
return (B*B - 4*A*C)>0
# 点群生成
pts = make_hyperbola_points()
# 可視化
plt.figure(figsize=(4,4))
xs = np.linspace(-5,5,500); ys = np.linspace(-5,5,500)
X,Y = np.meshgrid(xs,ys)
plt.scatter(pts[:,0],pts[:,1],s=6,alpha=0.3)
plt.legend()
plt.title("Before")
plt.show()
デモデータの点群
双曲線パラメータとSampson距離計算関数は楕円と同じ。再掲する。
双曲線のパラメータ探索関数とSampson距離関数
# フィット（SVD）
def fit_conic(points):
x,y = points[:,0], points[:,1]
D = np.stack([x**2,x*y,y**2,x,y,np.ones_like(x)],axis=1)
_,_,Vt = np.linalg.svd(D)
p = Vt[-1,:]
return p/np.linalg.norm(p[:3])
# サンプソン距離
def sd_conic(points, p):
A,B,C,D,E,F = p
x,y = points[:,0], points[:,1]
f = A*x**2+B*x*y+C*y**2+D*x+E*y+F
fx = 2*A*x+B*y+D
fy = B*x+2*C*y+E
return np.abs(f)/np.hypot(fx,fy)
# 双曲線判定
def is_hyperbola(p):
A,B,C,_,_,_ = p
return (B*B - 4*A*C)>0
RANSAC実行
# ===== RANSAC 実行（双曲線条件: B^2-4AC>0） =====
coef, inlier_num, mask, trial = ransac(
pts, fit_conic, sd_conic,
min_samples=5, thresh=0.2, valid_check=is_hyperbola
)
# フィッティング後の関数で再度Sampson距離計測
sd = sd_conic(pts, coef)
# 可視化用にインライアとされた点を記録
inlier_mask = sd < 0.2
# ===== 可視化（等高線 f=0 ＋ 二次式を図下に表示） =====
fig, ax = plt.subplots(figsize=(4,4))
xs = np.linspace(-5, 5, 500)
ys = np.linspace(-5, 5, 500)
X, Y = np.meshgrid(xs, ys)
A, B, C, D_, E, F = coef # D と被らないよう一時変数 D_ に
Z = A*X**2 + B*X*Y + C*Y**2 + D_*X + E*Y + F
ax.scatter(pts[:,0], pts[:,1], s=6, alpha=0.3, label="points")
ax.scatter(pts[inlier_mask,0], pts[inlier_mask,1], s=12, label="inliers")
ax.contour(X, Y, Z, levels=[0], colors="r", linewidths=2)
ax.set_aspect("equal", adjustable="box")
ax.grid(True, alpha=0.3)
ax.legend(loc="upper right")
ax.set_title(f"RANSAC + Sampson (Hyperbola)\nTrial: {trial}, Inlier rate: {inlier_num/len(pts):.2f}")
# 二次式を図の下に表示（等幅フォントで崩れにくく）
eq_text = conic_equation_string(coef)
fig.text(0.5, 0.02, eq_text, ha="center", va="bottom", fontsize=10, family="monospace")
plt.tight_layout(rect=[0, 0.06, 1, 1]) # 下マージンを広げる
plt.show()
試行回数421回目で点群の40%をインライアとする双曲線が得られた。
フィッティングした双曲線
4. サインカーブ
- 方程式:
y=A\sin(Bx+C)+D - パラメーター決定に最低限必要な点の数：4点
デモデータ生成
import numpy as np
import matplotlib.pyplot as plt
# ---- データ生成 ----
def make_wave_points(n_in=200, n_out=200, noise=0.2, seed=0):
rng = np.random.default_rng(seed)
xs = np.linspace(-2*np.pi, 2*np.pi, n_in)
ys = np.sin(xs) + rng.normal(0, noise, size=n_in)
inliers = np.stack([xs, ys], axis=1)
outliers = np.column_stack([
rng.uniform(-2*np.pi, 2*np.pi, size=n_out),
rng.uniform(-1.5, 1.5, size=n_out)
])
pts = np.vstack([inliers, outliers])
rng.shuffle(pts)
return pts
# 点群生成
pts = make_wave_points()
# ---- 可視化 ----
fig, ax = plt.subplots(figsize=(4,4))
ax.scatter(pts[:,0], pts[:,1], s=6, alpha=0.25, label="points")
xx = np.linspace(-2*np.pi, 2*np.pi, 800)
ax.legend()
ax.grid(True, alpha=0.3)
ax.set_title(f"Before")
plt.show()
# ---- サイン波モデル ----
# f(x,y) = y - sin(x) = 0
def fit_sine(points):
# サイン波にパラメータ a*sin(bx + c) + d をフィット
x, y = points[:,0], points[:,1]
from scipy.optimize import curve_fit
def model(x, a, b, c, d):
return a*np.sin(b*x + c) + d
p0 = [1, 1, 0, 0]
try:
popt, _ = curve_fit(model, x, y, p0=p0, maxfev=10000)
except:
popt = np.array([np.nan]*4)
return popt
def sd_sine(points, params):
a,b,c,d = params
x, y = points[:,0], points[:,1]
f = y - (a*np.sin(b*x + c) + d)
fx = -a*b*np.cos(b*x + c)
fy = np.ones_like(y)
denom = np.hypot(fx, fy)
denom = np.where(denom==0, np.finfo(float).eps, denom)
return np.abs(f) / denom
# ---- 実行 ----
coef, inlier_num, mask, trial = ransac(
pts, fit_sine, sd_sine, min_samples=4, thresh=0.2,
)
# ---- 可視化 ----
# フィッティング後の関数で再度Sampson距離計測
sd = sd_sine(pts, coef)
# 可視化用にインライアとされた点を記録
inlier_mask = sd < 0.2
fig, ax = plt.subplots(figsize=(4,4))
ax.scatter(pts[:,0], pts[:,1], s=6, alpha=0.25, label="points")
ax.scatter(pts[inlier_mask,0], pts[inlier_mask,1], s=12, alpha=0.9, label="inliers")
xx = np.linspace(-2*np.pi, 2*np.pi, 800)
yy = coef[0]*np.sin(coef[1]*xx + coef[2]) + coef[3]
ax.plot(xx, yy, lw=2, label="RANSAC+Sampson fit")
ax.legend()
ax.grid(True, alpha=0.3)
ax.set_title(f"Trial: {trial}, Inlier rate: {inlier_num/len(pts):.2f}")
plt.show()
print(f"推定パラメータ: a={coef[0]:.3f}, b={coef[1]:.3f}, c={coef[2]:.3f}, d={coef[3]:.3f}")
インライア率66%のサインカーブが得られてはいるのだが、期待してた関数より振幅が低くなってしまった。
これはサインカーブのパラメータ決定に用いるのに必要な最低限の点の数を指定したことで、ランダムに選ばれた点では正しい関数パラメーターを導出するのに十分な分布ではなかったことが原因と考えられる。
実際、以下のようにmin_samples=5とするだけで期待通りの振幅の関数がRANSACで得られた。
min_samples=5
# ---- 実行 ----
coef, inlier_num, mask, trial = ransac(
pts, fit_sine, sd_sine, min_samples=5, thresh=0.2,
)
# ---- 可視化 ----
# フィッティング後の関数で再度Sampson距離計測
sd = sd_sine(pts, coef)
# 可視化用にインライアとされた点を記録
inlier_mask = sd < 0.2
fig, ax = plt.subplots(figsize=(4,4))
ax.scatter(pts[:,0], pts[:,1], s=6, alpha=0.25, label="points")
ax.scatter(pts[inlier_mask,0], pts[inlier_mask,1], s=12, alpha=0.9, label="inliers")
xx = np.linspace(-2*np.pi, 2*np.pi, 800)
yy = coef[0]*np.sin(coef[1]*xx + coef[2]) + coef[3]
ax.plot(xx, yy, lw=2, label="RANSAC+Sampson fit")
ax.legend()
ax.grid(True, alpha=0.3)
ax.set_title(f"Trial: {trial}, Inlier rate: {inlier_num/len(pts):.2f}")
plt.show()
print(f"推定パラメータ: a={coef[0]:.3f}, b={coef[1]:.3f}, c={coef[2]:.3f}, d={coef[3]:.3f}")
Discussion