*
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()
Original: https://blog.csdn.net/melally/article/details/126116894
Author: melally
Title: Python: 用open3D库，连续多帧显示点云（查看localization pose的好坏）
原创文章受到原创版权保护。转载请注明出处：https://www.johngo689.com/719328/
转载文章受原作者版权保护。转载请注明原作者出处！