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.7323

print(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.734409

print(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.804177

print(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).T
    left_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).T
    right_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).T
    top_points_trans[:,-1] = top_intensity.flatten()

    fuson_points = np.vstack([left_points_trans, right_points_trans, top_points_trans])