如何将曲面拟合到一组数据点并获得曲面方程

乌班图 ROS 思维 Python程序

我正在尝试获取适合点云数据中的一组点的表面方程。数据来自激光雷达扫描仪。我在 rviz 中选择整个扫描的一部分，并获得该选择的坐标选定表面的图片。所选曲面并不总是如此线性，因为材质中可能存在轻微的曲线。我擅长数学和编程，所以任何想法都会很棒。我主要用python编程。我有每个点的 (X,Y,Z)，并且我还有数组（x_array、y_array、z_array 等）中的 x、y 和 z 分隔的每个坐标值。我将附上我的测试程序的代码，以防有人想看看我是如何做的。它主要只是从 ROS 主题接收坐标数据，从十六进制转换为浮点值，然后将浮点组织到各种数组中。

#!/usr/bin/env python3
import rospy
from std_msgs.msg import String
from sensor_msgs.msg import PointCloud2
import struct
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import axes3d
from scipy.linalg import lstsq

my_data = 0
array_size = 0
point_size = 0 

def callback(data):
    global my_data
    global array_size
    global point_size
    my_data = data.data
    point_size = data.width
    print("Point Size: ", point_size)
    coord_array_size = (point_size*3)

    i = 1
    current_low = 0
    current_high = 0
    previous_high = 0

    current_hex_string = 0
    hex_string = my_data.hex()

    xyz_points_array = [0]* coord_array_size
    xyz_point_array_counter = 0

    x_point_array = [0]* point_size
    y_point_array = [0]* point_size
    z_point_array = [0]* point_size
    point_array_counter = 0

    while(i <= point_size):
        if(i == 1):
            current_low = 0
            current_high = 32
            previous_high = 32
            i += 1

            current_hex_string = hex_string[current_low:current_high]
            x_coord = x_point_array[point_array_counter] = xyz_points_array[0] = struct.unpack('f', bytes.fromhex(current_hex_string[0:8]))
            y_coord = y_point_array[point_array_counter] = xyz_points_array[1] = struct.unpack('f', bytes.fromhex(current_hex_string[8:16]))
            z_coord = z_point_array[point_array_counter] = xyz_points_array[2] = struct.unpack('f', bytes.fromhex(current_hex_string[16:24]))

            xyz_point_array_counter += 3
            point_array_counter += 1

        elif(i > 1):
            current_low = previous_high
            current_high = (previous_high + 32)
            previous_high = current_high
            i += 1

            current_hex_string = hex_string[current_low:current_high]
            x_coord = x_point_array[point_array_counter] =    xyz_points_array[xyz_point_array_counter] = struct.unpack('f', bytes.fromhex(current_hex_string[0:8]))
            y_coord = y_point_array[point_array_counter] = xyz_points_array[xyz_point_array_counter + 1] = struct.unpack('f', bytes.fromhex(current_hex_string[8:16]))
            z_coord = z_point_array[point_array_counter] = xyz_points_array[xyz_point_array_counter + 2] = struct.unpack('f', bytes.fromhex(current_hex_string[16:24]))

            xyz_point_array_counter += 3
            point_array_counter += 1

    #print("\nCoordinate Array: ", xyz_points_array)
    #print("\nX Coordinate Array: ", x_point_array)
    #print("\nY Coordinate Array: ", y_point_array)
    #print("\nZ Coordinate Array: ", z_point_array)

    #plotting
    fig = plt.figure()
    ax = fig.add_subplot(111, projection='3d')
    ax.scatter(x_point_array, y_point_array, z_point_array, s = 1, c='blue')
    plt.xlabel('X Axis')
    plt.ylabel('Y Axis')
    #plt.zlabel('Z Axis')
    plt.show()


def listener_new():
    rospy.init_node('listener_new', anonymous=True)
    rospy.Subscriber("rviz_selected_points", PointCloud2, callback)
    rospy.spin()

if __name__ == '__main__':
    listener_new()

例如，您可以使用这段非常有趣的代码找到适合您的 3D 点的二次曲面。我修改了代码以仅考虑二阶情况。

The C数组由方程系数组成。

import numpy as np
import scipy.linalg
import matplotlib.pyplot as plt

# some 3-dim points
mean = np.array([0.0,0.0,0.0])
cov = np.array([[1.0,-0.5,0.8], [-0.5,1.1,0.0], [0.8,0.0,1.0]])
data = np.random.multivariate_normal(mean, cov, 50)

# regular grid covering the domain of the data
X,Y = np.meshgrid(np.arange(-3.0, 3.0, 0.5), np.arange(-3.0, 3.0, 0.5))
XX = X.flatten()
YY = Y.flatten()


# best-fit quadratic curve
A = np.c_[np.ones(data.shape[0]), data[:,:2], np.prod(data[:,:2], axis=1), data[:,:2]**2]
C,_,_,_ = scipy.linalg.lstsq(A, data[:,2])

# evaluate it on a grid
Z = np.dot(np.c_[np.ones(XX.shape), XX, YY, XX*YY, XX**2, YY**2], C).reshape(X.shape)

# plot points and fitted surface
fig = plt.figure()
ax = fig.gca(projection='3d')
ax.plot_surface(X, Y, Z, rstride=1, cstride=1, alpha=0.2)
ax.scatter(data[:,0], data[:,1], data[:,2], c='r', s=50)
plt.xlabel('X')
plt.ylabel('Y')
ax.set_zlabel('Z')
ax.axis('auto')
ax.axis('tight')
plt.show()