Python 从ROS点云获取距离太慢，如何优化？

我要做的是将点云（30fps）作为ROS主题的输入，使用

ransac查找平面。find_plane（）

计算法线距离，并将其发布到另一个主题中。一切正常，但如果距离超过2米，ransac函数需要很长时间才能找到飞机

在

find.plane（）

函数中有一个

残差_阈值

，默认值为0.01，这使得计算需要很长时间（3fps），因此增加了很多延迟。通过大量增加它（例如，

残差_阈值=1000

），我可以获得高达30fps的速度，但显然，在不容易计算平面（噪声或其他对象）的地方，质量会下降很多

有没有办法优化这一点？我不知道，哪怕是另一个能提供更快结果的函数。提前谢谢

import numpy as np
import math

from sklearn import linear_model
import open3d as o3d
import matplotlib.pyplot as plt

def find_plane(pcd, th):
    
    xyz = np.asarray(pcd.points)

    xy = xyz[:, :2]
    z = xyz[:, 2]
    ransac = linear_model.RANSACRegressor(residual_threshold=th) #Maximum residual for a data sample to be classified as an inlier.

    ransac.fit(xy, z)
    d = ransac.estimator_.intercept_  # intercept
    return d

这是我的密码：

import open3d as o3d
import ransac

from sensor_msgs.msg import PointCloud2
import std_msgs.msg
from std_msgs.msg import Float32
import sensor_msgs.point_cloud2 as pc2
    
def callback(data):
    global distance
    global pub
    th = 10
    pc = ros_numpy.numpify(data)
    points=np.zeros((pc.shape[0],3))
    points[:,0]=pc['x']
    points[:,1]=pc['y']
    points[:,2]=pc['z']

    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(points)

    dist = ransac.find_plane(pcd, th)
    pub.publish(dist)

def distance_node():
    rospy.init_node('distance_node')
    global pub
    pub = rospy.Publisher('distance', Float32, queue_size=1)
    rospy.Subscriber('/d435/depth/color/points', pc2.PointCloud2, callback, queue_size=None)
    rospy.spin()

if __name__ == '__main__':
    try:
        distance_node()
    except rospy.ROSInterruptException:
        pass