Python: 用open3D库，连续多帧显示点云（查看localization pose的好坏）_melally_保存连续帧点云

网络 02-07 4460

连续多帧显示点云，需要点云文件和定位信息（IMU惯导信息），我这里是从bag包里面自己解析出来的定位信息，因为是自己写的节点，所以直接从代码里面跑出来的，不是ros官方定义的，所以没有用官方给出的方法总体思路：将每一帧点云和旋转矩阵进行时间对齐 -----> 再进行空间对齐 ------> 统一对齐到一帧坐标系下可视化时间对齐：就是说我们哪一个时间下录制的pcd要和对应时间下旋转矩阵相乘（我这里没有用严格的时间插值，用的只是他们的差值小于0.01，我就认为是匹配的）空间对齐：1.看看静态物体（比如杆子）有没有对齐? ? 2.看看地面有没有变厚每一帧pcd都是在自车坐标系下录制的所以要转到世界坐标系下，然后再转到某一帧的自车坐标系下，就可以看到物体在移动的拖影，但是静止的非物体不会移动

时间对齐命令可见? ?rosbag解析用ros给出的命令直接解析bag包里面的点云数据，它是以时间戳命名的（unix时间戳），小数点后面有9位

而我解析定位信息得到的如下图所示，第一列也是时间戳，小数点后面只有6位，后面的16列就是自车转世界坐标系的4×4的矩阵

将两个时间做差小于0.01秒的，就认为匹配首先先将pcd的时间戳切出来再把定位信息的时间戳切出来 ?进行差值判断并转到世界坐标系下

在转到某一帧的坐标系下

20帧在统一坐标系下进行可视化

总代码： import os from os import listdir import open3d as o3d import numpy as np #获取pcd的名字 p=[] def get_name_dict(): name_dict = "./out_pcd" pcd_time = [] for i,j,k in os.walk(name_dict): pcd_time = k for t in pcd_time: p.append(t.split(".pcd")[0]) # pcds = listdir("./out_pcd") # pcd_name = [] # for j,l in enumerate(pcds): # pcd_path = os.path.join("./out_pcd",l) # pcd_name.append(l) # # print(l) return p #获取txt文件的每一行 def get_time_txt(): txtname = './vehicle2globle_mat.txt' txt_time = [] with open(txtname,"r+",encoding="utf-8") as f: for line in f: a = line.split() txt_time.append(a) # print(txt_time[0][0],txt_time[1][0]) return txt_time #pcd与旋转矩阵相乘 def Trans(pcd, R): ''' Input: pcd, (N, C) R, (4, 4) ''' pcd_trans = pcd.copy() pcd_hm = np.pad(pcd[:, :3], ((0, 0), (0, 1)), 'constant', constant_values=1) #(N, 4) pcd_hm_trans = np.dot(R, pcd_hm.T).T pcd_trans[:, :3] = pcd_hm_trans[:, :3] return pcd_trans #将每一个 def v_to_w(): dd = [] R = [] tt = get_time_txt() for o in p: # print(o) for i in range(200): a= '%.6f'%float(tt[i][0]) o=float(o) # print("a",a) # print("o",o) if -0.01 < o-float(a) < 0.01 : #dd是个列表，存放两个符合条件的txt时间戳，总是dd[0]小，而我也只要小的所以以下就有dd[0]使用 dd.append(float(a)) print("pcd",o) print("txt",a) if len(dd) == 2: print(dd) pcd_path= os.path.join("./out_pcd/",str(o)+'.pcd') # print(pcd_path) #循环txt中每一个元素,切出4*4矩阵 for r in range(len(tt)): for t in range(len(tt[0])): if float(tt[r][0]) == dd[0] and t > 0: #求得4*4旋转矩阵 R.append(tt[r][t]) if len(R) == 16: print("R",R) R = np.array(R).reshape(4,4) print("R.shape",R) #画图 pcd = o3d.io.read_point_cloud(pcd_path) pcd_xyz = np.asarray(pcd.points) valid_mask = ~np.isnan(pcd_xyz.sum(axis=1)) pcd_xyz = pcd_xyz[valid_mask] R = R.astype(np.float32) print(R) pcd_xyz_world = Trans(pcd_xyz, R) pcd_xyz_world_point = o3d.geometry.PointCloud() pcd_xyz_world_point.points = o3d.utility.Vector3dVector(pcd_xyz_world) o = str(o) # o3d.io.write_point_cloud("./out_pcd_w/"+o+".pcd",pcd_xyz_world_point,write_ascii=True,compressed=False,print_progress=True) R = R.tolist() R = R * 0 dd.clear() else: pass def w_to_R0(): pcd_dict = os.listdir("./out_pcd_w") for i in pcd_dict: print(i) pcd = o3d.io.read_point_cloud("./out_pcd_w/"+i) pcd_xyz = np.asarray(pcd.points) valid_mask = ~np.isnan(pcd_xyz.sum(axis=1)) pcd_xyz = pcd_xyz[valid_mask] R0 = np.array([[-0.815266 ,0.578755 ,0.019607 ,656827.306071 ], [-0.578086 ,-0.815380 ,0.031173 ,2967527.172453], [0.034029 ,0.014080 ,0.999322 ,59.210000 ], [0.000000 ,0.000000,0.000000 ,1.000000]]) R0 = np.linalg.inv(R0) pcd_R0 =Trans(pcd_xyz,R0) pcd_w_R0 = o3d.geometry.PointCloud() pcd_w_R0.points = o3d.utility.Vector3dVector(pcd_R0) o3d.io.write_point_cloud("./w_to_R0/" + i, pcd_w_R0, write_ascii=False, compressed=False, print_progress=False) def vis_RO(): pcds = [] pcds_p = [] pcd_20 = o3d.geometry.PointCloud() files = os.listdir("./w_to_R0") for f in files: pcd_path = os.path.join("./w_to_R0/" + f) pcds_p.append(pcd_path) pcd = o3d.io.read_point_cloud(pcd_path) # 降采样 pcd_dow = pcd.voxel_down_sample(voxel_size=0.2) pcds.append(pcd_dow) for i in range(len(pcds_p)): if i == 0: pcd0 = o3d.io.read_point_cloud(pcds_p[0]) o3d.io.write_point_cloud("pcd_20_20.pcd",pcd0) else: pcd1 = o3d.io.read_point_cloud("pcd_20_20.pcd") pcd2 = pcd1 + o3d.io.read_point_cloud(pcds_p[i]) o3d.io.write_point_cloud("pcd_20_20.pcd",pcd2) o3d.visualization.draw_geometries([pcd2], window_name="拼接20个点云", width=1024, height=768, left=50, top=50, mesh_show_back_face=False) # # ---------------将两个点云进行拼接------------------ # pcd0 = o3d.io.read_point_cloud(pcds_p[0]) # pcd1 = o3d.io.read_point_cloud(pcds_p[1]) # pcd_combined = o3d.geometry.PointCloud() # pcd_20 = pcd0 + pcd1 # # 保存点云 # o3d.io.write_point_cloud("pcd_20.pcd", pcd_combined) # for i in range(2, 16): # print(i) # # pcd_2 = o3d.io.read_point_cloud("pcd_20.pcd") # pcd1 = o3d.io.read_point_cloud(pcds_p[i]) # # pcd_combined = pcd_2 + pcd1 # o3d.io.write_point_cloud("pcd_20.pcd", pcd_combined) # o3d.visualization.draw_geometries([pcd_combined], window_name="拼接20个点云", # width=1024, height=768, # left=50, top=50, # mesh_show_back_face=False) # o3d.visualization.draw_geometries(pcd_20, # window_name="点云旋转", # point_show_normal=False, # width=800, # 窗口宽度 # height=600) if __name__ == '__main__': # print(get_time_txt()) # get_time_txt() # get_name_dict() # v_to_w() # w_to_R0() vis_RO()