- 连续多帧显示点云，需要 点云文件 和 定位信息（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()本文章为转载内容，我们尊重原作者对文章享有的著作权。如有内容错误或侵权问题，欢迎原作者联系我们进行内容更正或删除文章。
