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 RandomPlane(high=1000):
# 法線作成
n = np.array([0, 0, 0])
while LA.norm(n) == 0:
n = np.random.rand(3)
n = n / LA.norm(n)
a, b, c = n
# d作成
d = Random(-high, high)
return F2.plane([a, b, c, d])
def PlaneDict(points, normals, epsilon, alpha):
X, Y, Z = Disassemble(points)
n = points.shape[0]
N = 5000
# ランダムに3点ずつN組抽出
points_set = points[
np.array([np.random.choice(n, 3, replace=False) for i in range(N)]), :]
#points_set = points[np.random.choice(n, size=(int((n-n%3)/3), 3), replace=False), :]
#print("points:{}".format(points_set.shape))
# 分割
# [a1, b1, c1] -> [a1] [b1, c1]
a0, a1 = np.split(points_set, [1], axis=1)
# a2 = [[b1-a1], ...,[bn-an]]
# [[c1-a1], ...,[cn-an]]
a2 = np.transpose(a1 - a0, (1, 0, 2))
# n = (b-a) × (c-a)
n = np.cross(a2[0], a2[1])
# 単位ベクトルに変換
n = norm(n)
# d = n・a
a0 = np.reshape(a0, (a0.shape[0], 3))
d = np.sum(n * a0, axis=1)
# パラメータ
# p = [nx, ny, nz, d]
d = np.reshape(d, (d.shape[0], 1))
p = np.concatenate([n, d], axis=1)
# 平面生成
Planes = [F.plane(p[i]) for i in range(p.shape[0])]
# フィットしている点の数を数える
Scores = [
CountPoints(Planes[i], points, normals, epsilon, alpha)[1]
for i in range(p.shape[0])
]
print(p[Scores.index(max(Scores))])
return Planes[Scores.index(max(Scores))]
def ConstructAABBObject(max_p, min_p):
px1 = F2.plane([1, 0, 0, max_p[0]])
px2 = F2.plane([-1, 0, 0, -min_p[0]])
py1 = F2.plane([0, 1, 0, max_p[1]])
py2 = F2.plane([0, -1, 0, -min_p[1]])
pz1 = F2.plane([0, 0, 1, max_p[1]])
pz2 = F2.plane([0, 0, -1, -min_p[1]])
AABB = F2.AND(F2.AND(F2.AND(F2.AND(F2.AND(px1, px2), py1), py2), pz1), pz2)
return AABB
import numpy as np
import open3d
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from method import MakePoints
from method2d import Random
import figure2 as F
plane = F.plane([0, 0, 1, 1])
bbox = (0, 2)
AABB = [0, 2, 0, 2, 1.1, 1.3]
n_size = 100
points = MakePoints(plane.f_rep, bbox=bbox, grid_step=30)
p_size = points.shape[0]
print(p_size)
xmin, xmax, ymin, ymax, zmax, zmin = AABB
noise = np.array([[Random(xmin, xmax),
Random(ymin, ymax),
Random(zmin, zmax)] for i in range(n_size)])
points = np.concatenate([points, noise])
size = points.shape[0]
#点群をnp配列⇒open3d形式に
pointcloud = open3d.PointCloud()
pointcloud.points = open3d.Vector3dVector(points)
# color
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()
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])
HALF_S = F.AND(S1, P_Z0)
CUBE = F.AND(F.AND(F.AND(F.AND(F.AND(P_Z0, P_Z1), P_X0), P_X1), P_Y0), P_Y1)
CUBE2 = F.AND(F.AND(F.AND(F.AND(F.AND(P_Z_1, P_Z1), P_X_1), P_X1), P_Y_1),
P_Y1)
S6 = F.sphere([0, 0, 0, 1.2])