-
Notifications
You must be signed in to change notification settings - Fork 0
/
combined_pcd.py
90 lines (61 loc) · 2.19 KB
/
combined_pcd.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
import open3d as o3d
import os
import numpy as np
def extract_timestamp(file_name):
# 提取文件名中的时间戳部分(假设格式如 "file_<秒数>_<纳秒数>.txt")
# 先移除前缀 "file_" 和后缀 ".txt"
timestamp, _ = os.path.splitext(file_name)
sec, nsec = map(int, timestamp.split('_'))
return sec, nsec
def read_odom(path_odom):
odom_lists = []
with open(path_odom) as file:
for line in file:
line = line.strip()
data = list(map(float, line.split()))
odom_lists.append(data)
return odom_lists
def odom_to_T_r(odom_lists, i):
"""
将第i行对应的位姿信息转换成旋转矩阵T和平移向量t
"""
odom = odom_lists[i]
R = np.zeros((3, 3))
t = np.zeros((3, 1))
R[0, :3] = odom[0:3]
R[1, :3] = odom[4:7]
R[2, :3] = odom[8:11]
t[0] = odom[3]
t[1] = odom[7]
t[2] = odom[11]
t = t.flatten()
T = np.eye(4)
T[:3, :3] = R
T[:3, 3] = t
return T, R, t
directory_pcd = "/home/cjs/rosbag/20240926/2/_hesai_pandar"
directory_image = "/home/cjs/rosbag/20240926/2/_camera_infra1_image_rect_raw"
path_odom = "/home/cjs/rosbag/20240926/odom.txt"
path_output = "/home/cjs/rosbag/20240926/combined_pcd"
odom_lists = read_odom(path_odom)
pcd_lists = []
for file_name in sorted(os.listdir(directory_pcd), key=lambda f: extract_timestamp(f)):
if file_name.endswith(".pcd"):
file_path = os.path.join(directory_pcd, file_name)
pcd_lists.append(file_path)
image_lists = []
for file_name in sorted(os.listdir(directory_image), key=lambda f: extract_timestamp(f)):
if file_name.endswith(".png"):
file_path = os.path.join(directory_image, file_name)
image_lists.append(file_path)
N = min(len(pcd_lists), len(image_lists))
for i in range(10, N):
combined_pcd = o3d.geometry.PointCloud()
filename = os.path.basename(pcd_lists[i])
for j in range(i - 5, i + 600):
pcd = o3d.io.read_point_cloud(pcd_lists[j])
T, _, _ = odom_to_T_r(odom_lists, j)
pcd.transform(T)
combined_pcd += pcd
path = os.path.join(path_output, filename)
o3d.io.write_point_cloud(path, combined_pcd)