'How to fit a curved surface to a set of data points and obtain the equation for the surface

Ubuntu ROS Noetic Python Program

I am attempting to get an equation of a surface that will fit a set of points from point cloud data. The data originates from a lidar scanner. I select a portion of the entire scan in rviz and the coordinates of that selection are obtained picture of selected surface. The selected surfaces will not always be so linear, as there might be a slight curve in the material. I am some what good at math and programming so any ideas would be great. I mainly program in python. I have the (X,Y,Z) of each of the points, and i also have each value of the coordinates separated by their x, y, and z in arrays (x_array, y_array, z_array, etc.). I will attach my code of my test program in case anyone would want to see how I did things. It is mainly just receiving coordinate data from a ROS topic, going from hexadecimal to float values, and then organizing the floats into various arrays.

#!/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()


Sources

This article follows the attribution requirements of Stack Overflow and is licensed under CC BY-SA 3.0.

Source: Stack Overflow

Solution Source