• 連續(xù)多幀顯示點(diǎn)云，需要 點(diǎn)云文件 和 定位信息（IMU慣導(dǎo)信息），我這里是從bag包里面自己解析出來的定位信息，因?yàn)槭亲约簩懙墓?jié)點(diǎn)，所以直接從代碼里面跑出來的，不是ros官方定義的，所以沒有用官方給出的方法
• 總體思路：將每一幀點(diǎn)云和旋轉(zhuǎn)矩陣進(jìn)行 時間對齊 -----> 再進(jìn)行空間對齊 ------> 統(tǒng)一對齊到一幀坐標(biāo)系下可視化
  • 時間對齊：就是說我們哪一個時間下錄制的pcd要和對應(yīng)時間下旋轉(zhuǎn)矩陣相乘（我這里沒有用嚴(yán)格的時間插值，用的只是他們的差值小于0.01，我就認(rèn)為是匹配的）
  • 空間對齊：1.看看靜態(tài)物體（比如桿子）有沒有對齊? ? 2.看看地面有沒有變厚
  • 每一幀pcd都是在自車坐標(biāo)系下錄制的所以要轉(zhuǎn)到世界坐標(biāo)系下，然后再轉(zhuǎn)到某一幀的自車坐標(biāo)系下，就可以看到物體在移動的拖影，但是靜止的非物體不會移動

  • 

• 時間對齊

  • 命令可見? ?rosbag解析
  • 用ros給出的命令直接解析bag包里面的點(diǎn)云數(shù)據(jù)，它是以時間戳命名的（unix時間戳），小數(shù)點(diǎn)后面有9位

  • 

  • 而我解析定位信息得到的如下圖所示，第一列也是時間戳，小數(shù)點(diǎn)后面只有6位，后面的16列就是 自車轉(zhuǎn)世界坐標(biāo)系 的4×4的矩陣

  • 

  • 將兩個時間做差小于0.01秒的，就認(rèn)為匹配
  • 首先先將pcd的時間戳切出來
  • 
  • 再把定位信息的時間戳切出來
    • ?
  • 進(jìn)行差值判斷并轉(zhuǎn)到世界坐標(biāo)系下

    

  ?

  • 在轉(zhuǎn)到某一幀的坐標(biāo)系下

    

  • 20幀在統(tǒng)一坐標(biāo)系下進(jìn)行可視化

    文章來源地址http://www.zghlxwxcb.cn/news/detail-410505.html

• 總代碼：

• 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與旋轉(zhuǎn)矩陣相乘
  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)
                      #循環(huán)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旋轉(zhuǎn)矩陣
                                  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個點(diǎn)云",
                                                width=1024, height=768,
                                                left=50, top=50,
                                                mesh_show_back_face=False)
  
  
  
  
  
      # # ---------------將兩個點(diǎn)云進(jìn)行拼接------------------
      # 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
      # # 保存點(diǎn)云
      # 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個點(diǎn)云",
      #                                   width=1024, height=768,
      #                                   left=50, top=50,
      #                                   mesh_show_back_face=False)
  
  
  
      # o3d.visualization.draw_geometries(pcd_20,
      #                                   window_name="點(diǎn)云旋轉(zhuǎn)",
      #                                   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()
  

到了這里，關(guān)于Python: 用open3D庫，連續(xù)多幀顯示點(diǎn)云（查看localization pose的好壞）的文章就介紹完了。如果您還想了解更多內(nèi)容，請在右上角搜索TOY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！