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 CylinderDict(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] # lambda式が長くなりそうなのでnpメソッドの省略 N = lambda v: np.linalg.norm(v) D = lambda v1, v2: np.dot(v1, v2) # 各パラメータの算出式 radius1 = lambda p1, p2, n1, n2 : (N(n2)**2*D(p1-p2,n1) - D(n1,n2)*D(p1-p2,n2)) / ((N(n1)*N(n2))**2 - D(n1,n2)**2) radius2 = lambda p1, p2, n1, n2 : (D(n1,n2)*D(p1-p2,n1) - N(n1)**2*D(p1-p2,n2)) / ((N(n1)*N(n2))**2 - D(n1,n2)**2) point1 = lambda p1, n1, r1: p1 - r1*n1 point2 = lambda p2, n2, r2: p2 - r2*n2 #direction = lambda q1, q2: norm(q2-q1) truth_radius = lambda p1, q1: N(p1 - q1) # q1, q2:方向ベクトルの2点 # w:方向ベクトル # R:半径 r1 = [radius1(points_set[i][0], points_set[i][1], normals_set[i][0], normals_set[i][1]) for i in range(num)] r2 = [radius2(points_set[i][0], points_set[i][1], normals_set[i][0], normals_set[i][1]) for i in range(num)] q1 = [point1(points_set[i][0], normals_set[i][0], r1[i]) for i in range(num)] q2 = [point2(points_set[i][1], normals_set[i][1], r2[i]) for i in range(num)] # wは正規化 w = [norm(q2[i]-q1[i]) for i in range(num)] R = [truth_radius(points_set[i][0], q1[i]) for i in range(num)] print(num) ### R < lengthの条件を満たさないものを削除 ### index = np.where(R >= length) R = np.delete(R, index) q1 = np.delete(q1, index, axis=0) w = np.delete(w, index, axis=0) num = len(R) print(num, q1.shape, w.shape) # パラメータ # p = [x0, y0, z0, a, b, c, r] R = np.reshape(R, (num,1)) p = np.concatenate([q1, w, R], axis=1) # 球面生成 Cylinders = [F.cylinder(p[i]) for i in range(num)] # フィットしている点の数を数える Scores = [CountPoints(Cylinders[i], points, X, Y, Z, normals, epsilon=0.01*length, alpha=np.pi/10)[3] for i in range(num)] print(p[Scores.index(max(Scores))]) return p[Scores.index(max(Scores))], Cylinders[Scores.index(max(Scores))]
def Score(self, drawfig=False): # postorder_listに逆ポーランド記法を書き込む self.postorder_list = [] self.Postorder(0) # 演算の定義 operator = { 'and': (lambda p1, p2: F.AND(p1, p2)), 'or': (lambda p1, p2: F.OR(p1, p2)), 'not': (lambda p: F.NOT(p)) } stack = [] for z in self.postorder_list: #演算子じゃなかったらスタック if z not in operator.keys(): stack.append(z) continue #not elif z == "not": x = stack.pop() #print("not {} = {}".format(x, operator[z](x))) stack.append(operator[z](x)) #and, or else: y = stack.pop() x = stack.pop() stack.append(operator[z](x, y)) #print('{} {} {} = {}'.format(x, z, y, operator[z](x, y))) # 構文木によって演算された図形がstackに残る figure = stack[0] # フィットした点の数がスコア points, X, Y, Z, normals, length = PreProcess2() MX, MY, MZ, score, _ = CountPoints(figure, points, X, Y, Z, normals, epsilon=0.03 * length, alpha=np.pi) # グラフ作成したいとき if drawfig: DrawFig(figure, X, Y, Z, MX, MY, MZ) return score
def PlaneDict(points, normals, X, Y, Z, length): #print("10000個抽出") n = points.shape[0] #print(n) #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)) #print("計算") # 分割 # [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) #print("平面生成") # 平面生成 Planes = [F.plane(p[i]) for i in range(p.shape[0])] #print("点の数を数える") # フィットしている点の数を数える Scores = [CountPoints(Planes[i], points, X, Y, Z, normals, epsilon=0.08*length, alpha=np.pi/9)[3] for i in range(p.shape[0])] print(p[Scores.index(max(Scores))]) return p[Scores.index(max(Scores))], Planes[Scores.index(max(Scores))]
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))]
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()
fig = plt.figure() ax = Axes3D(fig) #軸にラベルを付けたいときは書く ax.set_xlabel("X") ax.set_ylabel("Y") ax.set_zlabel("Z") points, X, Y, Z, normals, length = PreProcess2() print(length) figure = P_Z0 #figure = SAMPLE_PLANE MX, MY, MZ, num, index = CountPoints(figure, points, X, Y, Z, normals, epsilon=0.03 * length, alpha=np.pi) (UX, UY, UZ), (HX, HY, HZ), hull = MakeContour(points, figure) # [0,1,2,..n] -> [1,2,...,n,0] hull2 = list(hull[:]) a = hull2.pop(0) hull2.append(a) hull2 = np.array(hull2) #点群を描画 ax.plot(X, Y, Z, marker="o", linestyle='None', color="white") ax.plot(MX, MY, MZ, marker=".", linestyle='None', color="green") ax.plot(UX, UY, UZ, marker=".", linestyle='None', color="blue")
def ConeDict(points, normals, X, Y, Z, length): n = points.shape[0] N = 5000 # ランダムに3点ずつN組抽出 index = np.array([np.random.choice(n, 3, replace=False) for i in range(N)]) #index = np.random.choice(n, size=(int((n-n%3)/3), 3), replace=False) points_set = points[index, :] normals_set = normals[index, :] num = points_set.shape[0] # 省略 DET = lambda v1, v2, v3: np.linalg.det(np.stack([v1, v2, v3])) DOT = lambda v1, v2: np.dot(v1, v2) # 各パラメータの算出式 """ det_A = lambda n1, n2, n3: np.linalg.det(np.stack([n1, n2, n3])) det_A1 = lambda p1, n2, n3: np.linalg.det(np.stack([p1, n2, n3])) det_A2 = lambda p2, n1, n3: np.linalg.det(np.stack([n1, p2, n3])) det_A3 = lambda p3, n1, n2: np.linalg.det(np.stack([n1, n2, p3])) apex = lambda A, A1, A2, A3 : np.array([A1/A, A2/A, A3/A]) """ d_list = lambda p1, p2, p3, n1, n2, n3: np.array([DOT(n1,p1), DOT(n2,p2), DOT(n3,p3)]) apex = lambda p1, p2, p3, n1, n2, n3: \ np.array([DET(d_list(p1,p2,p3,n1,n2,n3), n2, n3) / DET(n1, n2, n3),\ DET(n1, d_list(p1,p2,p3,n1,n2,n3), n3) / DET(n1, n2, n3), \ DET(n1, n2, d_list(p1,p2,p3,n1,n2,n3)) / DET(n1, n2, n3)]) """ point = lambda p1, p2, p3, c: np.array([c+norm(p1-c), c+norm(p2-c), c+norm(p2-c)]) normal = lambda a1, a2, a3: np.cross(a2-a1, a3-a1) """ # 平面の法線(=direction)の向きをaがない半空間の方向にしたいので、 # f(a)>0のときnormal, f(a)<0のとき-normalを返す # (f=d-(ax+by+cz)だとf(a)>0のときnはaがない方向、つまりnは内部(領域)から発散する方向に向いている) #direction = lambda p1, p2, p3, a: norm(np.cross(norm(p2-a)-norm(p1-a), norm(p3-a)-norm(p1-a))) normal = lambda p1, p2, p3, a: norm(np.cross(norm(p2-a)-norm(p1-a), norm(p3-a)-norm(p1-a))) plane_frep = lambda p1, p2, p3, a: lambda x: DOT(normal(p1,p2,p3,a), a+norm(p1-a)) - DOT(normal(p1,p2,p3,a), x) direction = lambda p1, p2, p3, a: normal(p1,p2,p3,a) if plane_frep(p1,p2,p3,a)(a) > 0 else -normal(p1,p2,p3,a) theta = lambda p1, a, w: np.arccos(np.dot(norm(p1-a), w)) #theta2 = lambda p2, a, w: np.arccos(np.dot(norm(p2-a), w)) #theta3 = lambda p3, a, w: np.arccos(np.dot(norm(p3-a), w)) # q1, q2:方向ベクトルの2点 # w:方向ベクトル # R:半径 a = np.array([apex(points_set[i][0], points_set[i][1], points_set[i][2], normals_set[i][0], normals_set[i][1], normals_set[i][2]) for i in range(num)]) w = np.array([direction(points_set[i][0], points_set[i][1], points_set[i][2], a[i]) for i in range(num)]) t = np.array([theta(points_set[i][0], a[i], w[i]) for i in range(num)]) #t2 = [theta2(points_set[i][1], a[i], w[i]) for i in range(num)] #t3 = [theta3(points_set[i][2], a[i], w[i]) for i in range(num)] #t = np.array([(t[i]+t2[i]+t3[i])/3 for i in range(num)]) print(w[:5]) print(t[:5]) print(num) ### 10 < theta < 60の条件を満たさないものを削除 ### index = np.where((t < np.pi/(180/10)) | (t > np.pi/(180/60))) t = np.delete(t, index) a = np.delete(a, index, axis=0) w = np.delete(w, index, axis=0) num = len(t) print(num) # パラメータ # p = [x0, y0, z0, a, b, c, theta] t = np.reshape(t, (num,1)) p = np.concatenate([a, w, t], axis=1) # 球面生成 Cones = [F.cone(p[i]) for i in range(num)] # フィットしている点の数を数える Scores = [CountPoints(Cones[i], points, X, Y, Z, normals, epsilon=0.03*length, alpha=np.pi/9)[3] for i in range(num)] print(p[Scores.index(max(Scores))]) return p[Scores.index(max(Scores))], Cones[Scores.index(max(Scores))]