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

• 连续多帧显示点云，需要 点云文件 和 定位信息（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()