多帧激光点云基于标定参数进行融合拼接

1、前言

在三维视觉技术蓬勃发展的今天，点云作为捕获和表示三维环境的基础数据形式，扮演着至关重要的角色。点云融合拼接技术，作为连接孤立点云片段、构建连续、全面三维场景的核心过程，对于自动驾驶、机器人导航、三维建模以及地理信息系统等领域具有重要意义。在自动实际应用中，为了获得一个的360度的三维点云，我们可能会安装主雷达加补盲雷达，在不同的视点进行数据采集，然后将这些采集到的点云数据拼接到一起。

2、将标定参数转换成转换矩阵

print("transform")
base2left= np.eye(4)
base2left[:3, :3] = pcd_left.get_rotation_matrix_from_xyz((0, -0, 0.9404))
base2left[0, 3] = 7.4 
base2left[1, 3] = 1.838 
base2left[2, 3] = 0.7323print(base2left)base2right = np.eye(4)
base2right[:3, :3] = pcd_right.get_rotation_matrix_from_xyz((-0.0234488, 0.0245959, -0.76741))
base2right[0, 3] = 7.41104
base2right[1, 3] = -1.85054
base2right[2, 3] = 0.734409print(base2right)base2top = np.eye(4)
base2top[:3, :3] = pcd_top.get_rotation_matrix_from_xyz((0.0345518, 0.0744975, -3.12372))
base2top[0, 3] = 1.48886 
base2top[1, 3] = -0.0218449 
base2top[2, 3] = 0.804177print(base2top)

3、将点云进行拼接融合

    left_add_col = np.ones((len(pcd_left), 1))left_points_1 = np.hstack([pcd_left, left_add_col])pcd_left_base = np.array([ [ 0.58946495, -0.80779395,  0.,           7.4      ],[ 0.80779395,  0.58946495,  0.,          1.838     ],[ 0. ,         0. ,         1. ,         0.7323    ],[ 0. ,         0. ,         0. ,         1.        ]])left_points_trans = np.dot(pcd_left_base, left_points_1.T).Tleft_points_trans[:,-1] = left_intensity.flatten()right_add_col = np.ones((len(pcd_right), 1))right_points_1 = np.hstack([pcd_right, right_add_col])pcd_right_base = np.array([[ 7.19493563e-01,  6.94063525e-01,  2.45934202e-02,  7.41104000e+00],[-6.94497664e-01,  7.19113053e-01,  2.34395594e-02, -1.85054000e+00],[-1.41690622e-03, -3.39446850e-02,  9.99422709e-01,  7.34409000e-01],[ 0.00000000e+00,  0.00000000e+00,  0.00000000e+00,  1.00000000e+00]])right_points_trans = np.dot(pcd_right_base, right_points_1.T).Tright_points_trans[:,-1] = right_intensity.flatten()top_add_col = np.ones((len(pcd_top), 1))top_points_1 = np.hstack([pcd_top, top_add_col])pcd_top_base = np.array([ [-0.99706708,  0.01782213,  0.07442861, -1.48886   ],[-0.02043176, -0.99919758, -0.03444911, -0.0218449 ],[ 0.07375493, -0.03586878,  0.99663115,  0.804177  ],[ 0.,          0.,          0.,          1.        ]])top_points_trans = np.dot(pcd_top_base, top_points_1.T).Ttop_points_trans[:,-1] = top_intensity.flatten()fuson_points = np.vstack([left_points_trans, right_points_trans, top_points_trans])