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] = 2.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] = 2.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., 2.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, 2.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])
关注我的公众号auto_driver_ai(Ai fighting), 第一时间获取更新内容。