def func(p, X, Y, Z, normals, fig, epsilon=0.7, alpha=np.pi/12):
#print(points.shape, normals.shape, fig, epsilon, alpha)
E = 0
if fig == 0:
figure = F.sphere(p)
elif fig == 1:
figure = F.plane(p)
elif fig==2:
figure = F.cylinder(p)
else:
figure = F.cone(p)
#dist[i] = i番目の点からの垂直距離
dist = figure.f_rep(X,Y,Z) / epsilon
#theta[i] = i番目の点の法線とnormalとの偏差(角度の差)
#np.sumは各点の法線同士の内積を取っている
#[nf_1*ni_1, nf_2*ni_2, ...]みたいな感じ
theta = np.arccos(np.abs(np.sum(figure.normal(X,Y,Z) * normals, axis=1))) / alpha
E = np.sum(np.exp(-dist**2) + np.exp(-theta**2))
#最小化なのでマイナスを返す
global E_list
E_list.append(E)
return -E
def func(p, X, Y, Z, normals, fig, epsilon=0.7, alpha=np.pi / 12):
if fig == 0:
figure = F.sphere(p)
elif fig == 1:
figure = F.plane(p)
elif fig == 2:
figure = F.cylinder(p)
else:
figure = F.cone(p)
# dist[i] = i番目の点からの垂直距離
dist = figure.f_rep(X, Y, Z) / epsilon
# theta[i] = i番目の点の法線とnormalとの偏差(角度の差)
# np.sumは各点の法線同士の内積を取っている
# [nf_1*ni_1, nf_2*ni_2, ...]みたいな感じ
theta = np.arccos(np.abs(np.sum(figure.normal(X, Y, Z) * normals,
axis=1))) / alpha
# E = Σ (1-exp(-d^2))^2 + (1-exp(-θ^2))^2
E = np.sum((1 - np.exp(-dist**2))**2 + (1 - np.exp(-theta**2))**2)
global E_list
E_list.append(E)
return E
def OptiViewer2(path, fig_type):
#グラフの枠を作っていく
fig = plt.figure()
ax = Axes3D(fig)
#軸にラベルを付けたいときは書く
ax.set_xlabel("X")
ax.set_ylabel("Y")
ax.set_zlabel("Z")
#点群,法線,OBBの対角線の長さ 取得
#points, X, Y, Z, normals, length = PreProcess(path)
#自作の点群を扱いたいときはこちら
#points, X, Y, Z, normals, length = PreProcess2()
# PLYデータを扱いたいときはこちら
points, X, Y, Z, normals, length = ViewPLY(path)
print("points:{}".format(points.shape[0]))
#点群を描画
#ax.plot(X,Y,Z,marker="o",linestyle='None',color="white")
U, V, W = Disassemble(normals)
#法線を描画
#ax.quiver(X, Y, Z, U, V, W, length=0.1, normalize=True)
#OBBを描画
OBBViewer(ax, points)
###最適化###
#result = figOptimize(points, normals, length, fig_type)
#result = figOptimize2(X, Y, Z, normals, length, fig_type)
result, label_list, max_label, num = RANSAC2(fig_type, points, normals, X, Y, Z, length)
print(result)
#fig_typeに応じた図形を選択
if fig_type==0:
figure = F.sphere(result)
elif fig_type==1:
figure = F.plane(result)
elif fig_type==2:
figure = F.cylinder(result)
else:
figure = F.cone(result)
#最適化された図形を描画
#plot_implicit(ax, figure.f_rep, points, AABB_size=1, contourNum=30)
print("num:{}".format(num))
# ラベルに色分けして点群プロット
LabelViewer(ax, points, label_list, max_label)
#最後に.show()を書いてグラフ表示
plt.show()
def RANSAC2(fig, points, normals, X, Y, Z, length):
# 図形に応じてRANSAC
if fig==0:
res1, figure1 = SphereDict(points, normals, X, Y, Z, length)
epsilon, alpha = 0.01*length, np.pi/12
elif fig==1:
res1, figure1 = PlaneDict(points, normals, X, Y, Z, length)
epsilon, alpha = 0.08*length, np.pi/9
elif fig==2:
res1, figure1 = CylinderDict(points, normals, X, Y, Z, length)
epsilon, alpha = 0.01*length, np.pi/12
elif fig==3:
res1, figure1 = ConeDict(points, normals, X, Y, Z, length)
epsilon, alpha = 0.03*length, np.pi/9
# フィット点を抽出
MX1, MY1, MZ1, num1, index1 = CountPoints(figure1, points, X, Y, Z, normals, epsilon=epsilon, alpha=alpha)
print("BEFORE_num:{}".format(num1))
if num1!=0:
# フィット点を入力にフィッティング処理
res2 = Fitting(MX1, MY1, MZ1, normals[index1], length, fig, figure1.p, epsilon=epsilon, alpha=alpha)
print(res2.x)
if fig==0:
figure2 = F.sphere(res2.x)
elif fig==1:
figure2 = F.plane(res2.x)
elif fig==2:
figure2 = F.cylinder(res2.x)
elif fig==3:
figure2 = F.cone(res2.x)
# フィッティング後のスコア出力
_, _, _, num2, _ = CountPoints(figure2, points, X, Y, Z, normals, epsilon=epsilon, alpha=alpha, plotFlag=True)
print("AFTER_num:{}".format(num2))
# フィッティング後の方が良ければres2を出力
if num2 >= num1:
label_list, max_label, max_label_num = CountPoints(figure2, points, X, Y, Z, normals, epsilon=epsilon, alpha=alpha, printFlag=True, labelFlag=True, plotFlag=True)
return res2.x, label_list, max_label, max_label_num
#X, Y, Z, num, index = CountPoints(figure2, points, X, Y, Z, normals, epsilon=epsilon, alpha=alpha)
# res1のスコア0 OR res2よりスコアが多い => res1を出力
label_list, max_label, max_label_num = CountPoints(figure1, points, X, Y, Z, normals, epsilon=epsilon, alpha=alpha, printFlag=True, labelFlag=True, plotFlag=True)
#X, Y, Z, num, index = CountPoints(figure2, points, X, Y, Z, normals, epsilon=epsilon, alpha=alpha)
return res1, label_list, max_label, max_label_num
def SphereDict(points, normals, X, Y, Z, length):
n = points.shape[0]
N = 5000
# ランダムに2点ずつN組抽出
#index = np.array([np.random.choice(n, 2, replace=False) for i in range(N)])
index = np.random.choice(n, size=(int((n-n%2)/2), 2), replace=False)
points_set = points[index, :]
normals_set = normals[index, :]
num = points_set.shape[0]
# c = p1 - r*n1
# c = p2 - r*n2 より
# r = (p1-p2)*(n1-n2)/|n1-n2|^2, c = p1 - r*n1となる
radius = lambda p1, p2, n1, n2 : np.dot(p1-p2, n1-n2) / np.linalg.norm(n1-n2)**2
center = lambda p1, n1, r : p1 - r * n1
# 二点の組[p1, p2], [n1, n2]をradius, centerに代入
r = [radius(points_set[i][0], points_set[i][1], normals_set[i][0], normals_set[i][1]) for i in range(num)]
### r < lengthの条件を満たさないものを除去 ###
r = [i for i in r if abs(i) <= length]
print(num)
num = len(r)
print(num)
c = [center(points_set[i][0], normals_set[i][0], r[i]) for i in range(num)]
# rはあとで絶対値をつける
r = list(map(abs, r))
#print(np.array(r).shape, np.array(c).shape)
# パラメータ
# p = [x0, y0, z0, r]
r = np.reshape(r, (num,1))
p = np.concatenate([c, r], axis=1)
# 球面生成
Spheres = [F.sphere(p[i]) for i in range(num)]
# フィットしている点の数を数える
Scores = [CountPoints(Spheres[i], points, X, Y, Z, normals, epsilon=0.01*length, alpha=np.pi/12)[3] for i in range(num)]
print(p[Scores.index(max(Scores))])
return p[Scores.index(max(Scores))], Spheres[Scores.index(max(Scores))]
#マークされた点 and ラベルを付けてない点をデキューした場合
if x in marked_index and label_list[x] == 0:
#ラベルを付ける
label_list[x] = label
#K近傍をエンキュー
for p in K_neighbor(points, points[x], k):
q.put(p)
#ラベルを変える
label = label + 1
return np.array(label_list)
figure = F.sphere([0.75, 0.75, 0.75, 0.75])
points, X, Y, Z, normals, length = PreProcess2()
label_list , MX, MY, MZ = CountPoints(figure, points, X, Y, Z, normals, epsilon=0.04*length, alpha=np.pi/10)
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
#軸にラベルを付けたいときは書く
ax.set_xlabel("X")
ax.set_ylabel("Y")
ax.set_zlabel("Z")
#points
ax.plot(X, Y, Z, marker="o",linestyle="None",color="white")
#マーク
ax.plot(MX, MY, MZ, marker=".",linestyle="None",color="orange")
def DetectViewer2(path):
#点群,法線,OBBの対角線の長さ 取得
#points, X, Y, Z, normals, length = PreProcess(path)
#自作の点群を扱いたいときはこちら
#points, X, Y, Z, normals, length = PreProcess2()
# PLYデータを扱いたいときはこちら
points, X, Y, Z, normals, length = ViewPLY(path)
#元の点群データを保存しておく
ori_points = points[:, :]
#ori_normals = normals[:, :]
# 検知した図形のリスト
fitting_figures = []
print("points:{}".format(points.shape[0]))
###グラフ初期化###
ax = ViewerInit(points, X, Y, Z, normals)
while points.shape[0] >= ori_points.shape[0] * 0.05:
print("points:{}".format(points.shape[0]))
scores = []
paras = []
indices = []
###最適化###
for fig_type in [0,1,2,3]:
###グラフ初期化##
#ax = ViewerInit(points, X, Y, Z, normals)
#図形フィッティング
#result = figOptimize(points, normals, length, fig_type)
#result = figOptimize2(X, Y, Z, normals, length, fig_type)
result, MX, MY, MZ, num, index = RANSAC(fig_type, points, normals, X, Y, Z, length)
print(result)
#fig_typeに応じた図形を選択
if fig_type==0:
figure = F.sphere(result)
elif fig_type==1:
figure = F.plane(result)
elif fig_type==2:
figure = F.cylinder(result)
elif fig_type==3:
figure = F.cone(result)
#図形描画
#plot_implicit(ax, figure.f_rep, points, AABB_size=1, contourNum=50)
#図形に対して"条件"を満たす点群を数える、これをスコアとする
#MX, MY, MZ, num, index = CountPoints(figure, points, X, Y, Z, normals, epsilon=0.08*length, alpha=np.pi/9)
#print("AFTER_num:{}".format(num))
#条件を満たす点群, 最適化された図形描画
#ax.plot(MX,MY,MZ,marker=".",linestyle='None',color="orange")
#最後に.show()を書いてグラフ表示
#plt.show()
#スコアとパラメータ,インデックスを保存
scores.append(num)
paras.append(result)
indices.append(index)
print("="*100)
if max(scores) <= ori_points.shape[0] * 0.05:
print("おわり！")
break
###グラフ初期化###
ax = ViewerInit(points, X, Y, Z, normals)
# スコアが最大の図形を描画
best_fig = scores.index(max(scores))
# スコアが最大の図形を保存
fitting_figures.append([best_fig, paras[best_fig]])
if best_fig==0:
figure = F.sphere(paras[best_fig])
print("球の勝ち")
elif best_fig==1:
figure = F.plane(paras[best_fig])
print("平面の勝ち")
elif best_fig==2:
figure = F.cylinder(paras[best_fig])
print("円柱の勝ち")
elif best_fig==3:
figure = F.cone(paras[best_fig])
print("円錐の勝ち")
# フィット点描画
ax.plot(X[indices[best_fig]],Y[indices[best_fig]],Z[indices[best_fig]],\
marker=".",linestyle='None',color="orange")
# 図形描画
plot_implicit(ax, figure.f_rep, points, AABB_size=1, contourNum=15)
plt.show()
#フィットした点群を削除
points = np.delete(points, indices[best_fig], axis=0)
normals = np.delete(normals, indices[best_fig], axis=0)
X, Y, Z = Disassemble(points)
###グラフ初期化###
#ax = ViewerInit(points, X, Y, Z, normals)
#plt.show()
##################
print("="*100)
print(len(fitting_figures), fitting_figures)
plt.show()
def OptiViewer(path, fig_type):
#点群,法線,OBBの対角線の長さ 取得
#points, X, Y, Z, normals, length = PreProcess(path)
#自作の点群を扱いたいときはこちら
#points, X, Y, Z, normals, length = PreProcess2()
# PLYデータを扱いたいときはこちら
points, X, Y, Z, normals, length = ViewPLY(path)
#U, V, W = Disassemble(normals)
#法線を描画
#ax.quiver(X, Y, Z, U, V, W, length=0.1, normalize=True)
while True:
print("points:{}".format(points.shape[0]))
#グラフの枠を作っていく
fig = plt.figure()
ax = Axes3D(fig)
#軸にラベルを付けたいときは書く
ax.set_xlabel("X")
ax.set_ylabel("Y")
ax.set_zlabel("Z")
#点群を描画
ax.plot(X,Y,Z,marker="o",linestyle='None',color="white")
#OBBを描画
OBBViewer(ax, points)
###最適化###
#result = figOptimize(points, normals, length, fig_type)
#result = figOptimize2(X, Y, Z, normals, length, fig_type)
result, MX, MY, MZ, num, index = RANSAC(fig_type, points, normals, X, Y, Z, length)
print(result)
#fig_typeに応じた図形を選択
if fig_type==0:
figure = F.sphere(result)
elif fig_type==1:
figure = F.plane(result)
elif fig_type==2:
figure = F.cylinder(result)
else:
figure = F.cone(result)
#最適化された図形を描画
plot_implicit(ax, figure.f_rep, points, AABB_size=1, contourNum=15)
#S_optを検出
#MX, MY, MZ, num, index = CountPoints(figure, points, X, Y, Z, normals, epsilon=0.08*length, alpha=np.pi/9)
print("num:{}".format(num))
ax.plot(MX,MY,MZ,marker=".",linestyle='None',color="red")
# グラフ表示
plt.show()
# フィットした点群を削除
points = np.delete(points, index, axis=0)
normals = np.delete(normals, index, axis=0)
X, Y, Z = Disassemble(points)
def DetectViewer(path):
#点群,法線,OBBの対角線の長さ 取得
#points, X, Y, Z, normals, length = PreProcess(path)
#自作の点群を扱いたいときはこちら
points, X, Y, Z, normals, length = PreProcess2()
#元の点群データを保存しておく
ori_points = points[:, :]
fitting_figures = []
print("points:{}".format(points.shape[0]))
###グラフ初期化###
ax = ViewerInit(points, X, Y, Z, normals)
while points.shape[0] >= ori_points.shape[0] * 0.01:
print("points:{}".format(points.shape[0]))
scores = []
paras = []
indices = []
###最適化###
for fig_type in [0, 1]:
#a = input()
###グラフ初期化##
#ax = ViewerInit(points, X, Y, Z, normals)
#図形フィッティング
#result = figOptimize(points, normals, length, fig_type)
result = figOptimize2(X, Y, Z, normals, length, fig_type)
print(result.x)
#fig_typeに応じた図形を選択
if fig_type==0:
figure = F.sphere(result.x)
elif fig_type==1:
figure = F.plane(result.x)
#図形描画
#plot_implicit(ax, figure.f_rep, points, AABB_size=1, contourNum=50)
#図形に対して"条件"を満たす点群を数える、これをスコアとする
MX, MY, MZ, num, index = CountPoints(figure, points, X, Y, Z, normals, epsilon=0.08*length, alpha=np.pi/9)
print("num:{}".format(num))
#条件を満たす点群, 最適化された図形描画
#ax.plot(MX,MY,MZ,marker=".",linestyle='None',color="orange")
#最後に.show()を書いてグラフ表示
#plt.show()
#スコアとパラメータ,インデックスを保存
scores.append(num)
paras.append(result.x)
indices.append(index)
if sum(scores) <= 5:
print("もっかい！\n")
continue
###グラフ初期化###
#ax = ViewerInit(points, X, Y, Z, normals)
#スコアが最大の図形を描画
best_fig = scores.index(max(scores))
if best_fig==0:
figure = F.sphere(paras[best_fig])
fitting_figures.append("球：[" + ','.join(map(str, list(paras[best_fig]))) + "]")
elif best_fig==1:
figure = F.plane(paras[best_fig])
fitting_figures.append("平面：[" + ','.join(map(str, list(paras[best_fig]))) + "]")
plot_implicit(ax, figure.f_rep, points, AABB_size=1, contourNum=15)
#plt.show()
#フィットした点群を削除
points = np.delete(points, indices[best_fig], axis=0)
normals = np.delete(normals, indices[best_fig], axis=0)
X, Y, Z = Disassemble(points)
###グラフ初期化###
#ax = ViewerInit(points, X, Y, Z, normals)
#plt.show()
##################
print("points:{}".format(points.shape[0]))
print(len(fitting_figures), fitting_figures)
plt.show()
import numpy as np
import figure2 as F
from method import *
def norm_sphere(x, y, z):
return 1 - np.sqrt(x**2 + y**2 + z**2)
S1 = F.sphere([0, 0, 0, 1])
S2 = F.sphere([1, 0, 0, 0.5])
S3 = F.sphere([-1, 0, 0, 0.5])
kirby = F.OR(S1, F.OR(S2, S3))
AND_S = F.AND(S1, S2)
S4 = F.sphere([np.sin(np.pi / 6), np.cos(np.pi / 6), 0, 1])
S5 = F.sphere([-np.sin(np.pi / 6), np.cos(np.pi / 6), 0, 1])
AND_BEN = F.AND(S1, F.AND(S4, S5))
P_Z0 = F.plane([0, 0, -1, 0])
P_Z1 = F.plane([0, 0, 1, 1])
P_Z_1 = F.plane([0, 0, -1, 1])
P_X0 = F.plane([-1, 0, 0, 0])
P_X1 = F.plane([1, 0, 0, 1])
P_X_1 = F.plane([-1, 0, 0, 1])
P_Y0 = F.plane([0, -1, 0, 0])
P_Y1 = F.plane([0, 1, 0, 1])
P_Y_1 = F.plane([0, -1, 0, 1])
