Aligning a Point Cloud with the Floor (plane) Using Open3D

Question

Community,

I am trying to align a point cloud with the detected floor using Open3D. So far I implemented the following steps (partly of this answer):

1. Detecting the floor using Open3D's plane segmentation
2. Translating the plane to the coordinate center
3. Calculating rotation angle between plane normal & z-axis
4. Calculating the axis of rotation
5. Rotating the point cloud using Open3Ds get_rotation_matrix_from_axis_angle function (see 3)

The results are OK, but I have to use an optimization-factor at the end for better results. Is there a mistake or a simpler/ more precise way for the alignment?

# 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

Psychosomatics