python - 使用 Open3D 将点云与地板（平面）对齐

Question

社区，

我正在尝试使用 Open3D 将点云与检测到的地板对齐。到目前为止，我实施了以下步骤（部分答案）：

1. 使用 Open3D 的平面分割检测地板
2. 将平面平移到坐标中心
3. 计算平面法线和z轴之间的旋转角度
4. 计算旋转轴
5. 使用 Open3Dsget_rotation_matrix_from_axis_angle函数旋转点云（参见3）

结果还可以，但我必须在最后使用优化因子以获得更好的结果。对齐是否有错误或更简单/更精确的方法？

# See functions below

# Get the plane equation of the floor → ax+by+cz+d = 0
floor = get_floor_plane(pcd)
a, b, c, d = floor

# Translate plane to coordinate center
pcd.translate((0,-d/c,0))

# Calculate rotation angle between plane normal & z-axis
plane_normal = tuple(floor[:3])
z_axis = (0,0,1)
rotation_angle = vector_angle(plane_normal, z_axis)

# Calculate rotation axis
plane_normal_length = math.sqrt(a**2 + b**2 + c**2)
u1 = b / plane_normal_length
u2 = -a / plane_normal_length
rotation_axis = (u1, u2, 0)

# Generate axis-angle representation
optimization_factor = 1.4
axis_angle = tuple([x * rotation_angle * optimization_factor for x in rotation_axis])

# Rotate point cloud
R = pcd.get_rotation_matrix_from_axis_angle(axis_angle)
pcd.rotate(R, center=(0,0,0))

# FUNCTIONS    
def vector_angle(u, v):
    return np.arccos(np.dot(u,v) / (np.linalg.norm(u)* np.linalg.norm(v)))

def get_floor_plane(pcd, dist_threshold=0.02, visualize=False):
    plane_model, inliers = pcd.segment_plane(distance_threshold=dist_threshold,
                                             ransac_n=3,
                                             num_iterations=1000)
    [a, b, c, d] = plane_model    
    return plane_model

标签： pythonnumpy3drotational-matricesopen3d