Question

我正在尝试制作一个程序，该程序给出了从点云中获取的一组3d点。它将在点云中找到最接近第一个点的10个点。然后给出这十点，它将找到最合适的平面。我修改了this个关于堆栈交换的问题的代码，该代码在给定一组随机点的情况下找到了最适合的平面，这是未经修改的原始代码：

import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import numpy as np

N_POINTS = 10
TARGET_X_SLOPE = 2
TARGET_y_SLOPE = 3
TARGET_OFFSET  = 5
EXTENTS = 5
NOISE = 5

# create random data
xs = [np.random.uniform(2*EXTENTS)-EXTENTS for i in range(N_POINTS)]
ys = [np.random.uniform(2*EXTENTS)-EXTENTS for i in range(N_POINTS)]
zs = []
for i in range(N_POINTS):
    zs.append(xs[i]*TARGET_X_SLOPE + \
              ys[i]*TARGET_y_SLOPE + \
              TARGET_OFFSET + np.random.normal(scale=NOISE))

# plot raw data
plt.figure()
ax = plt.subplot(111, projection='3d')
ax.scatter(xs, ys, zs, color='b')

# do fit
tmp_A = []
tmp_b = []
for i in range(len(xs)):
    tmp_A.append([xs[i], ys[i], 1])
    tmp_b.append(zs[i])
b = np.matrix(tmp_b).T
A = np.matrix(tmp_A)
fit = (A.T * A).I * A.T * b
errors = b - A * fit
residual = np.linalg.norm(errors)

print "solution:"
print "%f x + %f y + %f = z" % (fit[0], fit[1], fit[2])
print "errors:"
print errors
print "residual:"
print residual

# plot plane
xlim = ax.get_xlim()
ylim = ax.get_ylim()
X,Y = np.meshgrid(np.arange(xlim[0], xlim[1]),
                  np.arange(ylim[0], ylim[1]))
Z = np.zeros(X.shape)
for r in range(X.shape[0]):
    for c in range(X.shape[1]):
        Z[r,c] = fit[0] * X[r,c] + fit[1] * Y[r,c] + fit[2]
ax.plot_wireframe(X,Y,Z, color='k')

ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_zlabel('z')
plt.show()

此代码可以正常工作，并显示最适合10个随机点的平面。我修改了代码，方法是更改数据的生成方式，并添加一个函数以获取十个最接近的点。但是，现在我的代码仅显示平面的点和方程式，而不显示平面本身。这是修改后的代码：

import math
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import numpy as np
import open3d as o3d
from open3d import read_point_cloud
pcd = read_point_cloud("edge_cloud.pcd")
pointsArray = np.asarray(pcd.points)  

N_POINTS = 10
TARGET_X_SLOPE = 2
TARGET_y_SLOPE = 3
TARGET_OFFSET  = 5
EXTENTS = 5
NOISE = 5


def getClosest(point,pointsArray):
    howMany=10
    distances=[]

    idxarray=[]
    for i in range(len(pointsArray)):
        distance=math.sqrt((pointsArray[i][0]-point[0])**2+(pointsArray[i][1]-point[1])**2+(pointsArray[i][2]-point[2])**2)
        if(len(distances)<11):
            idx=0
            if(len(distances)==0):
                distances.insert(idx,distance)
                idxarray.insert(idx,i)
            else:
                while(distances[idx]<distance):
                    idx+=1
                    if(idx==len(distances)):
                        distances.insert(idx,distance)
                        idxarray.insert(idx,i)
                distances.insert(idx,distance)
                idxarray.insert(idx,i)

        else:
            if(distance<distances[len(distances)-1]):
                idx=0
                while(distances[idx]<distance):
                    idx+=1
                distances.insert(idx,distance)
                idxarray.insert(idx,i)
                idxarray.pop()
                distances.pop()
    return idxarray




# create random data
xs = []
ys = []
zs = []
point=pointsArray[0]

idxarray=getClosest(point,pointsArray)

for i in range(10):
   xs.append(pointsArray[idxarray[i]][0])
   ys.append(pointsArray[idxarray[i]][1])
   zs.append(pointsArray[idxarray[i]][2])

# plot raw data
plt.figure()
ax = plt.subplot(111, projection='3d')
ax.scatter(xs, ys, zs, color='b')

# do fit
tmp_A = []
tmp_b = []
for i in range(len(xs)):
    tmp_A.append([xs[i], ys[i], 1])
    tmp_b.append(zs[i])
b = np.matrix(tmp_b).T
A = np.matrix(tmp_A)
fit = (A.T * A).I * A.T * b
errors = b - A * fit
residual = np.linalg.norm(errors)

print("solution:")
print("%f x + %f y + %f = z" % (fit[0], fit[1], fit[2]))
print("errors:")
print(errors)
print("residual:")
print(residual)

# plot plane
xlim = ax.get_xlim()
ylim = ax.get_ylim()
X,Y = np.meshgrid(np.arange(xlim[0], xlim[1]),
                  np.arange(ylim[0], ylim[1]))
Z = np.zeros(X.shape)
for r in range(X.shape[0]):
    for c in range(X.shape[1]):
        Z[r,c] = fit[0] * X[r,c] + fit[1] * Y[r,c] + fit[2]
ax.plot_wireframe(X,Y,Z, color='k')

ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_zlabel('z')
plt.show()

我想我所做的就是更改将数据输入此代码的方式，但是由于某种原因，它不再像以前那样显示平面。

根据3d点制作最合适的飞机

0 个答案: