社区,
我正在尝试使用 Open3D 将点云与检测到的地板对齐。到目前为止,我实施了以下步骤(部分答案):
- 使用 Open3D 的平面分割检测地板
- 将平面平移到坐标中心
- 计算平面法线和z轴之间的旋转角度
- 计算旋转轴
- 使用 Open3Ds
get_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