q3dviewer 1.0.1__py3-none-any.whl → 1.0.3__py3-none-any.whl
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- {q3dviewer-1.0.1.dist-info → q3dviewer-1.0.3.dist-info}/METADATA +1 -1
- q3dviewer-1.0.3.dist-info/RECORD +5 -0
- q3dviewer-1.0.3.dist-info/top_level.txt +1 -0
- q3dviewer/__init__.py +0 -4
- q3dviewer/basic_viewer.py +0 -56
- q3dviewer/custom_items/__init__.py +0 -9
- q3dviewer/custom_items/axis_item.py +0 -73
- q3dviewer/custom_items/camera_frame_item.py +0 -173
- q3dviewer/custom_items/cloud_io_item.py +0 -83
- q3dviewer/custom_items/cloud_item.py +0 -297
- q3dviewer/custom_items/gaussian_item.py +0 -267
- q3dviewer/custom_items/grid_item.py +0 -37
- q3dviewer/custom_items/image_item.py +0 -169
- q3dviewer/custom_items/text_item.py +0 -53
- q3dviewer/custom_items/trajectory_item.py +0 -78
- q3dviewer/gau_io.py +0 -168
- q3dviewer/tools/__init__.py +0 -6
- q3dviewer/tools/cloud_viewer.py +0 -75
- q3dviewer/tools/example_viewer.py +0 -38
- q3dviewer/tools/gaussian_viewer.py +0 -55
- q3dviewer/tools/lidar_calib.py +0 -274
- q3dviewer/tools/lidar_cam_calib.py +0 -299
- q3dviewer/tools/mesh_viewer.py +0 -57
- q3dviewer/tools/ros_viewer.py +0 -102
- q3dviewer/utils.py +0 -146
- q3dviewer/viewer_widget.py +0 -183
- q3dviewer-1.0.1.dist-info/RECORD +0 -28
- q3dviewer-1.0.1.dist-info/top_level.txt +0 -1
- {q3dviewer-1.0.1.dist-info → q3dviewer-1.0.3.dist-info}/WHEEL +0 -0
- {q3dviewer-1.0.1.dist-info → q3dviewer-1.0.3.dist-info}/entry_points.txt +0 -0
q3dviewer/tools/lidar_calib.py
DELETED
|
@@ -1,274 +0,0 @@
|
|
|
1
|
-
#!/usr/bin/env python3
|
|
2
|
-
|
|
3
|
-
from sensor_msgs.msg import PointCloud2
|
|
4
|
-
from q3dviewer import *
|
|
5
|
-
import rospy
|
|
6
|
-
import numpy as np
|
|
7
|
-
import argparse
|
|
8
|
-
import open3d as o3d
|
|
9
|
-
|
|
10
|
-
viewer = None
|
|
11
|
-
cloud0_accum = None
|
|
12
|
-
clouds0 = []
|
|
13
|
-
cloud1_accum = None
|
|
14
|
-
clouds1 = []
|
|
15
|
-
|
|
16
|
-
|
|
17
|
-
class CustomDoubleSpinBox(QDoubleSpinBox):
|
|
18
|
-
def __init__(self, decimals=4, *args, **kwargs):
|
|
19
|
-
self.decimals = decimals
|
|
20
|
-
super().__init__(*args, **kwargs)
|
|
21
|
-
self.setDecimals(self.decimals) # Set the number of decimal places
|
|
22
|
-
|
|
23
|
-
def textFromValue(self, value):
|
|
24
|
-
return f"{value:.{self.decimals}f}"
|
|
25
|
-
|
|
26
|
-
def valueFromText(self, text):
|
|
27
|
-
return float(text)
|
|
28
|
-
|
|
29
|
-
|
|
30
|
-
class ViewerWithPanel(Viewer):
|
|
31
|
-
def __init__(self, **kwargs):
|
|
32
|
-
self.t01 = np.array([0, 0, 0])
|
|
33
|
-
self.R01 = np.eye(3)
|
|
34
|
-
self.cloud_num = 10
|
|
35
|
-
self.radius = 0.2
|
|
36
|
-
super().__init__(**kwargs)
|
|
37
|
-
|
|
38
|
-
def initUI(self):
|
|
39
|
-
centerWidget = QWidget()
|
|
40
|
-
self.setCentralWidget(centerWidget)
|
|
41
|
-
main_layout = QHBoxLayout()
|
|
42
|
-
centerWidget.setLayout(main_layout)
|
|
43
|
-
|
|
44
|
-
# Create a vertical layout for the settings
|
|
45
|
-
setting_layout = QVBoxLayout()
|
|
46
|
-
|
|
47
|
-
# Add XYZ spin boxes
|
|
48
|
-
label_xyz = QLabel("Set XYZ:")
|
|
49
|
-
setting_layout.addWidget(label_xyz)
|
|
50
|
-
self.box_x = CustomDoubleSpinBox()
|
|
51
|
-
self.box_x.setSingleStep(0.01)
|
|
52
|
-
setting_layout.addWidget(self.box_x)
|
|
53
|
-
self.box_y = CustomDoubleSpinBox()
|
|
54
|
-
self.box_y.setSingleStep(0.01)
|
|
55
|
-
setting_layout.addWidget(self.box_y)
|
|
56
|
-
self.box_z = CustomDoubleSpinBox()
|
|
57
|
-
self.box_z.setSingleStep(0.01)
|
|
58
|
-
setting_layout.addWidget(self.box_z)
|
|
59
|
-
self.box_x.setRange(-100.0, 100.0)
|
|
60
|
-
self.box_y.setRange(-100.0, 100.0)
|
|
61
|
-
self.box_z.setRange(-100.0, 100.0)
|
|
62
|
-
|
|
63
|
-
# Add RPY spin boxes
|
|
64
|
-
label_rpy = QLabel("Set Roll-Pitch-Yaw:")
|
|
65
|
-
setting_layout.addWidget(label_rpy)
|
|
66
|
-
self.box_roll = CustomDoubleSpinBox()
|
|
67
|
-
self.box_roll.setSingleStep(0.01)
|
|
68
|
-
setting_layout.addWidget(self.box_roll)
|
|
69
|
-
self.box_pitch = CustomDoubleSpinBox()
|
|
70
|
-
self.box_pitch.setSingleStep(0.01)
|
|
71
|
-
setting_layout.addWidget(self.box_pitch)
|
|
72
|
-
self.box_yaw = CustomDoubleSpinBox()
|
|
73
|
-
self.box_yaw.setSingleStep(0.01)
|
|
74
|
-
setting_layout.addWidget(self.box_yaw)
|
|
75
|
-
self.box_roll.setRange(-np.pi, np.pi)
|
|
76
|
-
self.box_pitch.setRange(-np.pi, np.pi)
|
|
77
|
-
self.box_yaw.setRange(-np.pi, np.pi)
|
|
78
|
-
|
|
79
|
-
label_cloud_num = QLabel("Set cloud frame number:")
|
|
80
|
-
setting_layout.addWidget(label_cloud_num)
|
|
81
|
-
self.box_cloud_num = QSpinBox()
|
|
82
|
-
self.box_cloud_num.setValue(self.cloud_num)
|
|
83
|
-
self.box_cloud_num.setRange(1, 100)
|
|
84
|
-
self.box_cloud_num.valueChanged.connect(self.update_cloud_num)
|
|
85
|
-
setting_layout.addWidget(self.box_cloud_num)
|
|
86
|
-
|
|
87
|
-
label_res = QLabel("The lidar0-lidar1 trans and quat:")
|
|
88
|
-
setting_layout.addWidget(label_res)
|
|
89
|
-
self.line_trans = QLineEdit()
|
|
90
|
-
self.line_trans.setReadOnly(True)
|
|
91
|
-
setting_layout.addWidget(self.line_trans)
|
|
92
|
-
self.line_quat = QLineEdit()
|
|
93
|
-
self.line_quat.setReadOnly(True)
|
|
94
|
-
setting_layout.addWidget(self.line_quat)
|
|
95
|
-
|
|
96
|
-
label_matching_setting = QLabel("Set matching radius:")
|
|
97
|
-
setting_layout.addWidget(label_matching_setting)
|
|
98
|
-
self.box_radius = CustomDoubleSpinBox(decimals=2)
|
|
99
|
-
self.box_radius.setSingleStep(0.1)
|
|
100
|
-
self.box_radius.setRange(0.1, 3.0)
|
|
101
|
-
self.box_radius.setValue(self.radius)
|
|
102
|
-
setting_layout.addWidget(self.box_radius)
|
|
103
|
-
|
|
104
|
-
self.icp_button = QPushButton("Auto Scan Matching")
|
|
105
|
-
self.icp_button.clicked.connect(self.perform_icp)
|
|
106
|
-
setting_layout.addWidget(self.icp_button)
|
|
107
|
-
|
|
108
|
-
self.line_trans.setText(np.array2string(self.t01, formatter={
|
|
109
|
-
'float_kind': lambda x: "%.4f" % x}, separator=', '))
|
|
110
|
-
quat = matrix_to_quaternion(self.R01)
|
|
111
|
-
self.line_quat.setText(np.array2string(
|
|
112
|
-
quat, formatter={'float_kind': lambda x: "%.4f" % x}, separator=', '))
|
|
113
|
-
|
|
114
|
-
# Connect spin boxes to methods
|
|
115
|
-
self.box_x.valueChanged.connect(self.update_xyz)
|
|
116
|
-
self.box_y.valueChanged.connect(self.update_xyz)
|
|
117
|
-
self.box_z.valueChanged.connect(self.update_xyz)
|
|
118
|
-
self.box_roll.valueChanged.connect(self.update_rpy)
|
|
119
|
-
self.box_pitch.valueChanged.connect(self.update_rpy)
|
|
120
|
-
self.box_yaw.valueChanged.connect(self.update_rpy)
|
|
121
|
-
|
|
122
|
-
# Add a stretch to push the widgets to the top
|
|
123
|
-
setting_layout.addStretch(1)
|
|
124
|
-
|
|
125
|
-
self.viewerWidget = self.vw()
|
|
126
|
-
main_layout.addLayout(setting_layout)
|
|
127
|
-
main_layout.addWidget(self.viewerWidget, 1)
|
|
128
|
-
|
|
129
|
-
timer = QtCore.QTimer(self)
|
|
130
|
-
timer.setInterval(20) # period, in milliseconds
|
|
131
|
-
timer.timeout.connect(self.update)
|
|
132
|
-
self.viewerWidget.setCameraPosition(distance=5)
|
|
133
|
-
self.viewerWidget.setBKColor('#ffffff')
|
|
134
|
-
timer.start()
|
|
135
|
-
|
|
136
|
-
def update_radius(self):
|
|
137
|
-
self.radius = self.box_radius.value()
|
|
138
|
-
|
|
139
|
-
def update_cloud_num(self):
|
|
140
|
-
self.cloud_num = self.box_cloud_num.value()
|
|
141
|
-
|
|
142
|
-
def update_xyz(self):
|
|
143
|
-
x = self.box_x.value()
|
|
144
|
-
y = self.box_y.value()
|
|
145
|
-
z = self.box_z.value()
|
|
146
|
-
self.t01 = np.array([x, y, z])
|
|
147
|
-
self.line_trans.setText(f"[{x:.6f}, {y:.6f}, {z:.6f}]")
|
|
148
|
-
|
|
149
|
-
def update_rpy(self):
|
|
150
|
-
roll = self.box_roll.value()
|
|
151
|
-
pitch = self.box_pitch.value()
|
|
152
|
-
yaw = self.box_yaw.value()
|
|
153
|
-
self.R01 = euler_to_matrix(np.array([roll, pitch, yaw]))
|
|
154
|
-
quat = matrix_to_quaternion(self.R01)
|
|
155
|
-
self.line_quat.setText(f"[{quat[0]:.6f}, {quat[1]:.6f}, {quat[2]:.6f}, {quat[3]:.6f}]")
|
|
156
|
-
|
|
157
|
-
def perform_icp(self):
|
|
158
|
-
global cloud0_accum, cloud1_accum
|
|
159
|
-
|
|
160
|
-
if cloud0_accum is not None and cloud1_accum is not None:
|
|
161
|
-
# Convert to Open3D point clouds
|
|
162
|
-
cloud0_o3d = o3d.geometry.PointCloud()
|
|
163
|
-
cloud0_o3d.points = o3d.utility.Vector3dVector(cloud0_accum['xyz'])
|
|
164
|
-
cloud1_o3d = o3d.geometry.PointCloud()
|
|
165
|
-
cloud1_o3d.points = o3d.utility.Vector3dVector(cloud1_accum['xyz'])
|
|
166
|
-
voxel_size = 0.1
|
|
167
|
-
cloud0_o3d = cloud0_o3d.voxel_down_sample(voxel_size)
|
|
168
|
-
cloud1_o3d = cloud1_o3d.voxel_down_sample(voxel_size)
|
|
169
|
-
cloud0_o3d.estimate_normals(
|
|
170
|
-
search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=self.radius, max_nn=30))
|
|
171
|
-
trans_init = np.eye(4)
|
|
172
|
-
trans_init[:3, :3] = self.R01
|
|
173
|
-
trans_init[:3, 3] = self.t01
|
|
174
|
-
|
|
175
|
-
criteria = o3d.pipelines.registration.ICPConvergenceCriteria(
|
|
176
|
-
relative_fitness=1e-4,
|
|
177
|
-
max_iteration=50)
|
|
178
|
-
|
|
179
|
-
# Auto Scan Matching
|
|
180
|
-
reg_p2p = o3d.pipelines.registration.registration_icp(
|
|
181
|
-
cloud1_o3d, cloud0_o3d, self.radius, trans_init,
|
|
182
|
-
o3d.pipelines.registration.TransformationEstimationPointToPlane(),
|
|
183
|
-
criteria)
|
|
184
|
-
|
|
185
|
-
transformation_icp = reg_p2p.transformation
|
|
186
|
-
# print("ICP Transformation:", transformation_icp)
|
|
187
|
-
|
|
188
|
-
# Update R01 and t01 with ICP result
|
|
189
|
-
R01 = transformation_icp[:3, :3]
|
|
190
|
-
t01 = transformation_icp[:3, 3]
|
|
191
|
-
|
|
192
|
-
# Update the UI with new values
|
|
193
|
-
quat = matrix_to_quaternion(R01)
|
|
194
|
-
rpy = matrix_to_euler(R01)
|
|
195
|
-
self.box_roll.setValue(rpy[0])
|
|
196
|
-
self.box_pitch.setValue(rpy[1])
|
|
197
|
-
self.box_yaw.setValue(rpy[2])
|
|
198
|
-
self.box_x.setValue(t01[0])
|
|
199
|
-
self.box_y.setValue(t01[1])
|
|
200
|
-
self.box_z.setValue(t01[2])
|
|
201
|
-
self.line_trans.setText(f"[{t01[0]:.6f}, {t01[1]:.6f}, {t01[2]:.6f}]")
|
|
202
|
-
self.line_quat.setText(f"[{quat[0]:.6f}, {quat[1]:.6f}, {quat[2]:.6f}, {quat[3]:.6f}]")
|
|
203
|
-
self.t01 = t01
|
|
204
|
-
self.R01 = R01
|
|
205
|
-
print("Matching results")
|
|
206
|
-
print(f"translation: [{t01[0]:.6f}, {t01[1]:.6f}, {t01[2]:.6f}]")
|
|
207
|
-
print(f"Roll-Pitch-Yaw: [{rpy[0]:.6f}, {rpy[1]:.6f}, {rpy[2]:.6f}]")
|
|
208
|
-
print(f"Quaternion: [{quat[0]:.6f}, {quat[1]:.6f}, {quat[2]:.6f}, {quat[3]:.6f}]")
|
|
209
|
-
|
|
210
|
-
def msg2Cloud(data):
|
|
211
|
-
pc = PointCloud.from_msg(data).pc_data
|
|
212
|
-
data_type = [('xyz', '<f4', (3,)), ('color', '<u4')]
|
|
213
|
-
color = pc['intensity'].astype(np.uint32)
|
|
214
|
-
cloud = np.rec.fromarrays(
|
|
215
|
-
[np.stack([pc['x'], pc['y'], pc['z']], axis=1), color],
|
|
216
|
-
dtype=data_type)
|
|
217
|
-
return cloud
|
|
218
|
-
|
|
219
|
-
|
|
220
|
-
def scan0CB(data):
|
|
221
|
-
global viewer, clouds0, cloud0_accum
|
|
222
|
-
cloud = msg2Cloud(data)
|
|
223
|
-
while len(clouds0) > viewer.cloud_num:
|
|
224
|
-
clouds0.pop(0)
|
|
225
|
-
clouds0.append(cloud)
|
|
226
|
-
cloud0_accum = np.concatenate(clouds0)
|
|
227
|
-
viewer['scan0'].setData(data=cloud0_accum)
|
|
228
|
-
|
|
229
|
-
|
|
230
|
-
def scan1CB(data):
|
|
231
|
-
global viewer, clouds1, cloud1_accum
|
|
232
|
-
cloud = msg2Cloud(data)
|
|
233
|
-
while len(clouds1) > viewer.cloud_num:
|
|
234
|
-
clouds1.pop(0)
|
|
235
|
-
clouds1.append(cloud)
|
|
236
|
-
cloud1_accum = np.concatenate(clouds1)
|
|
237
|
-
cloud0_accum_new = cloud1_accum.copy()
|
|
238
|
-
cloud0_accum_new['xyz'] = (
|
|
239
|
-
viewer.R01 @ cloud1_accum['xyz'].T + viewer.t01[:, np.newaxis]).T
|
|
240
|
-
|
|
241
|
-
viewer['scan1'].setData(data=cloud0_accum_new)
|
|
242
|
-
|
|
243
|
-
|
|
244
|
-
def main():
|
|
245
|
-
global viewer
|
|
246
|
-
# Set up argument parser
|
|
247
|
-
parser = argparse.ArgumentParser(
|
|
248
|
-
description="Configure topic names for LiDAR, image, and camera info.")
|
|
249
|
-
parser.add_argument('--lidar0', type=str, default='/lidar0',
|
|
250
|
-
help="Topic name for LiDAR data")
|
|
251
|
-
parser.add_argument('--lidar1', type=str, default='/lidar1',
|
|
252
|
-
help="Topic name for camera image data")
|
|
253
|
-
args = parser.parse_args()
|
|
254
|
-
|
|
255
|
-
app = QApplication(["LiDAR Calib"])
|
|
256
|
-
viewer = ViewerWithPanel(name='LiDAR Calib')
|
|
257
|
-
grid_item = GridItem(size=10, spacing=1, color=(0, 0, 0, 70))
|
|
258
|
-
scan0_item = CloudItem(size=2, alpha=1, color_mode='#FF0000')
|
|
259
|
-
scan1_item = CloudItem(size=2, alpha=1, color_mode='#00FF00')
|
|
260
|
-
viewer.addItems(
|
|
261
|
-
{'scan0': scan0_item, 'scan1': scan1_item, 'grid': grid_item})
|
|
262
|
-
|
|
263
|
-
rospy.init_node('lidar_calib', anonymous=True)
|
|
264
|
-
|
|
265
|
-
# Use topic names from arguments
|
|
266
|
-
rospy.Subscriber(args.lidar0, PointCloud2, scan0CB, queue_size=1)
|
|
267
|
-
rospy.Subscriber(args.lidar1, PointCloud2, scan1CB, queue_size=1)
|
|
268
|
-
|
|
269
|
-
viewer.show()
|
|
270
|
-
app.exec_()
|
|
271
|
-
|
|
272
|
-
|
|
273
|
-
if __name__ == '__main__':
|
|
274
|
-
main()
|
|
@@ -1,299 +0,0 @@
|
|
|
1
|
-
#!/usr/bin/env python3
|
|
2
|
-
|
|
3
|
-
from sensor_msgs.msg import PointCloud2, Image, CameraInfo
|
|
4
|
-
from q3dviewer import *
|
|
5
|
-
import rospy
|
|
6
|
-
import numpy as np
|
|
7
|
-
import argparse
|
|
8
|
-
import cv2
|
|
9
|
-
|
|
10
|
-
viewer = None
|
|
11
|
-
cloud_accum = None
|
|
12
|
-
cloud_accum_color = None
|
|
13
|
-
clouds = []
|
|
14
|
-
remap_info = None
|
|
15
|
-
K = None
|
|
16
|
-
|
|
17
|
-
|
|
18
|
-
class CustomDoubleSpinBox(QDoubleSpinBox):
|
|
19
|
-
def __init__(self, decimals=4, *args, **kwargs):
|
|
20
|
-
self.decimals = decimals
|
|
21
|
-
super().__init__(*args, **kwargs)
|
|
22
|
-
self.setDecimals(self.decimals) # Set the number of decimal places
|
|
23
|
-
|
|
24
|
-
def textFromValue(self, value):
|
|
25
|
-
return f"{value:.{self.decimals}f}"
|
|
26
|
-
|
|
27
|
-
def valueFromText(self, text):
|
|
28
|
-
return float(text)
|
|
29
|
-
|
|
30
|
-
|
|
31
|
-
class ViewerWithPanel(Viewer):
|
|
32
|
-
def __init__(self, **kwargs):
|
|
33
|
-
# b: camera body frame
|
|
34
|
-
# c: camera image frame
|
|
35
|
-
# l: lidar frame
|
|
36
|
-
self.Rbl = np.eye(3)
|
|
37
|
-
self.Rcb = np.array([[0, -1, 0],
|
|
38
|
-
[0, 0, -1],
|
|
39
|
-
[1, 0, 0]])
|
|
40
|
-
self.tcl = np.array([0, 0, 0])
|
|
41
|
-
self.Rcl = self.Rcb @ self.Rbl
|
|
42
|
-
self.psize = 2
|
|
43
|
-
self.cloud_num = 1
|
|
44
|
-
self.en_rgb = False
|
|
45
|
-
super().__init__(**kwargs)
|
|
46
|
-
|
|
47
|
-
def initUI(self):
|
|
48
|
-
centerWidget = QWidget()
|
|
49
|
-
self.setCentralWidget(centerWidget)
|
|
50
|
-
main_layout = QHBoxLayout()
|
|
51
|
-
centerWidget.setLayout(main_layout)
|
|
52
|
-
|
|
53
|
-
# Create a vertical layout for the settings
|
|
54
|
-
setting_layout = QVBoxLayout()
|
|
55
|
-
|
|
56
|
-
# Add a checkbox for RGB
|
|
57
|
-
self.checkbox_rgb = QCheckBox("Enable RGB Cloud")
|
|
58
|
-
self.checkbox_rgb.setChecked(False)
|
|
59
|
-
setting_layout.addWidget(self.checkbox_rgb)
|
|
60
|
-
self.checkbox_rgb.stateChanged.connect(
|
|
61
|
-
self.checkbox_changed)
|
|
62
|
-
|
|
63
|
-
# Add XYZ spin boxes
|
|
64
|
-
label_xyz = QLabel("Set XYZ:")
|
|
65
|
-
setting_layout.addWidget(label_xyz)
|
|
66
|
-
self.box_x = CustomDoubleSpinBox()
|
|
67
|
-
self.box_x.setSingleStep(0.01)
|
|
68
|
-
setting_layout.addWidget(self.box_x)
|
|
69
|
-
self.box_y = CustomDoubleSpinBox()
|
|
70
|
-
self.box_y.setSingleStep(0.01)
|
|
71
|
-
setting_layout.addWidget(self.box_y)
|
|
72
|
-
self.box_z = CustomDoubleSpinBox()
|
|
73
|
-
self.box_z.setSingleStep(0.01)
|
|
74
|
-
setting_layout.addWidget(self.box_z)
|
|
75
|
-
self.box_x.setRange(-100.0, 100.0)
|
|
76
|
-
self.box_y.setRange(-100.0, 100.0)
|
|
77
|
-
self.box_y.setRange(-100.0, 100.0)
|
|
78
|
-
|
|
79
|
-
# Add RPY spin boxes
|
|
80
|
-
label_rpy = QLabel("Set Roll-Pitch-Yaw:")
|
|
81
|
-
setting_layout.addWidget(label_rpy)
|
|
82
|
-
self.box_roll = CustomDoubleSpinBox()
|
|
83
|
-
self.box_roll.setSingleStep(0.01)
|
|
84
|
-
setting_layout.addWidget(self.box_roll)
|
|
85
|
-
self.box_pitch = CustomDoubleSpinBox()
|
|
86
|
-
self.box_pitch.setSingleStep(0.01)
|
|
87
|
-
setting_layout.addWidget(self.box_pitch)
|
|
88
|
-
self.box_yaw = CustomDoubleSpinBox()
|
|
89
|
-
self.box_yaw.setSingleStep(0.01)
|
|
90
|
-
setting_layout.addWidget(self.box_yaw)
|
|
91
|
-
self.box_roll.setRange(-np.pi, np.pi)
|
|
92
|
-
self.box_pitch.setRange(-np.pi, np.pi)
|
|
93
|
-
self.box_yaw.setRange(-np.pi, np.pi)
|
|
94
|
-
|
|
95
|
-
label_psize = QLabel("Set point size:")
|
|
96
|
-
setting_layout.addWidget(label_psize)
|
|
97
|
-
self.box_psize = QSpinBox()
|
|
98
|
-
self.box_psize.setValue(self.psize)
|
|
99
|
-
self.box_psize.setRange(0, 5)
|
|
100
|
-
self.box_psize.valueChanged.connect(self.update_psize)
|
|
101
|
-
setting_layout.addWidget(self.box_psize)
|
|
102
|
-
|
|
103
|
-
label_cloud_num = QLabel("Set cloud frame number:")
|
|
104
|
-
setting_layout.addWidget(label_cloud_num)
|
|
105
|
-
self.box_cloud_num = QSpinBox()
|
|
106
|
-
self.box_cloud_num.setValue(self.cloud_num)
|
|
107
|
-
self.box_cloud_num.setRange(1, 100)
|
|
108
|
-
self.box_cloud_num.valueChanged.connect(self.update_cloud_num)
|
|
109
|
-
setting_layout.addWidget(self.box_cloud_num)
|
|
110
|
-
|
|
111
|
-
label_res = QLabel("The cam-lidar quat and translation:")
|
|
112
|
-
setting_layout.addWidget(label_res)
|
|
113
|
-
self.line_quat = QLineEdit()
|
|
114
|
-
self.line_quat.setReadOnly(True)
|
|
115
|
-
setting_layout.addWidget(self.line_quat)
|
|
116
|
-
self.line_trans = QLineEdit()
|
|
117
|
-
self.line_trans.setReadOnly(True)
|
|
118
|
-
setting_layout.addWidget(self.line_trans)
|
|
119
|
-
|
|
120
|
-
self.line_trans.setText(f"[{self.tcl[0]:.6f}, {self.tcl[1]:.6f}, {self.tcl[2]:.6f}]")
|
|
121
|
-
quat = matrix_to_quaternion(self.Rcl)
|
|
122
|
-
self.line_quat.setText(f"[{quat[0]:.6f}, {quat[1]:.6f}, {quat[2]:.6f}, {quat[3]:.6f}]")
|
|
123
|
-
|
|
124
|
-
# Connect spin boxes to methods
|
|
125
|
-
self.box_x.valueChanged.connect(self.update_xyz)
|
|
126
|
-
self.box_y.valueChanged.connect(self.update_xyz)
|
|
127
|
-
self.box_z.valueChanged.connect(self.update_xyz)
|
|
128
|
-
self.box_roll.valueChanged.connect(self.update_rpy)
|
|
129
|
-
self.box_pitch.valueChanged.connect(self.update_rpy)
|
|
130
|
-
self.box_yaw.valueChanged.connect(self.update_rpy)
|
|
131
|
-
|
|
132
|
-
# Add a stretch to push the widgets to the top
|
|
133
|
-
setting_layout.addStretch(1)
|
|
134
|
-
|
|
135
|
-
self.viewerWidget = self.vw()
|
|
136
|
-
main_layout.addLayout(setting_layout)
|
|
137
|
-
main_layout.addWidget(self.viewerWidget, 1)
|
|
138
|
-
|
|
139
|
-
timer = QtCore.QTimer(self)
|
|
140
|
-
timer.setInterval(20) # period, in milliseconds
|
|
141
|
-
timer.timeout.connect(self.update)
|
|
142
|
-
self.viewerWidget.setCameraPosition(distance=5)
|
|
143
|
-
self.viewerWidget.setBKColor('#ffffff')
|
|
144
|
-
timer.start()
|
|
145
|
-
|
|
146
|
-
def update_psize(self):
|
|
147
|
-
self.psize = self.box_psize.value()
|
|
148
|
-
|
|
149
|
-
def update_cloud_num(self):
|
|
150
|
-
self.cloud_num = self.box_cloud_num.value()
|
|
151
|
-
|
|
152
|
-
def update_xyz(self):
|
|
153
|
-
x = self.box_x.value()
|
|
154
|
-
y = self.box_y.value()
|
|
155
|
-
z = self.box_z.value()
|
|
156
|
-
self.tcl = np.array([x, y, z])
|
|
157
|
-
self.line_trans.setText(f"[{x:.6f}, {y:.6f}, {x:.6f}]")
|
|
158
|
-
|
|
159
|
-
def update_rpy(self):
|
|
160
|
-
roll = self.box_roll.value()
|
|
161
|
-
pitch = self.box_pitch.value()
|
|
162
|
-
yaw = self.box_yaw.value()
|
|
163
|
-
self.Rbl = euler_to_matrix(np.array([roll, pitch, yaw]))
|
|
164
|
-
self.Rcl = self.Rcb @ self.Rbl
|
|
165
|
-
quat = matrix_to_quaternion(self.Rcl)
|
|
166
|
-
self.line_quat.setText(f"[{quat[0]:.6f}, {quat[1]:.6f}, {quat[2]:.6f}, {quat[3]:.6f}]")
|
|
167
|
-
|
|
168
|
-
def checkbox_changed(self, state):
|
|
169
|
-
if state == QtCore.Qt.Checked:
|
|
170
|
-
self.en_rgb = True
|
|
171
|
-
else:
|
|
172
|
-
self.en_rgb = False
|
|
173
|
-
|
|
174
|
-
|
|
175
|
-
def cameraInfoCB(data):
|
|
176
|
-
global remap_info, K
|
|
177
|
-
if remap_info is None: # Initialize only once
|
|
178
|
-
K = np.array(data.K).reshape(3, 3)
|
|
179
|
-
D = np.array(data.D)
|
|
180
|
-
rospy.loginfo("Camera intrinsic parameters set")
|
|
181
|
-
height = data.height
|
|
182
|
-
width = data.width
|
|
183
|
-
mapx, mapy = cv2.initUndistortRectifyMap(
|
|
184
|
-
K, D, None, K, (width, height), cv2.CV_32FC1)
|
|
185
|
-
remap_info = [mapx, mapy]
|
|
186
|
-
|
|
187
|
-
|
|
188
|
-
def scanCB(data):
|
|
189
|
-
global viewer, clouds, cloud_accum, cloud_accum_color
|
|
190
|
-
pc = PointCloud.from_msg(data).pc_data
|
|
191
|
-
data_type = viewer['scan'].data_type
|
|
192
|
-
color = pc['intensity'].astype(np.uint32)
|
|
193
|
-
cloud = np.rec.fromarrays(
|
|
194
|
-
[np.stack([pc['x'], pc['y'], pc['z']], axis=1), color],
|
|
195
|
-
dtype=data_type)
|
|
196
|
-
while len(clouds) > viewer.cloud_num:
|
|
197
|
-
clouds.pop(0)
|
|
198
|
-
clouds.append(cloud)
|
|
199
|
-
cloud_accum = np.concatenate(clouds)
|
|
200
|
-
|
|
201
|
-
if cloud_accum_color is not None and viewer.en_rgb:
|
|
202
|
-
viewer['scan'].setData(data=cloud_accum_color)
|
|
203
|
-
viewer['scan'].setColorMode('RGB')
|
|
204
|
-
else:
|
|
205
|
-
viewer['scan'].setData(data=cloud_accum)
|
|
206
|
-
viewer['scan'].setColorMode('I')
|
|
207
|
-
|
|
208
|
-
|
|
209
|
-
def draw_larger_points(image, points, colors, radius):
|
|
210
|
-
for dx in range(-radius + 1, radius):
|
|
211
|
-
for dy in range(-radius + 1, radius):
|
|
212
|
-
distance = np.sqrt(dx**2 + dy**2)
|
|
213
|
-
if distance <= radius:
|
|
214
|
-
x_indices = points[:, 0] + dx
|
|
215
|
-
y_indices = points[:, 1] + dy
|
|
216
|
-
image[y_indices, x_indices] = colors
|
|
217
|
-
return image
|
|
218
|
-
|
|
219
|
-
|
|
220
|
-
def imageCB(data):
|
|
221
|
-
global cloud_accum, cloud_accum_color, remap_info, K
|
|
222
|
-
if remap_info is None:
|
|
223
|
-
rospy.logwarn("Camera parameters not yet received.")
|
|
224
|
-
return
|
|
225
|
-
|
|
226
|
-
image = np.frombuffer(data.data, dtype=np.uint8).reshape(
|
|
227
|
-
data.height, data.width, -1)
|
|
228
|
-
if data.encoding == 'bgr8':
|
|
229
|
-
image = image[:, :, ::-1] # convert BGR to RGB
|
|
230
|
-
|
|
231
|
-
# Undistort the image
|
|
232
|
-
image_un = cv2.remap(image, remap_info[0], remap_info[1], cv2.INTER_LINEAR)
|
|
233
|
-
|
|
234
|
-
if cloud_accum is not None:
|
|
235
|
-
cloud_local = cloud_accum.copy()
|
|
236
|
-
tcl = viewer.tcl
|
|
237
|
-
Rcl = viewer.Rcl
|
|
238
|
-
pl = cloud_local['xyz']
|
|
239
|
-
po = (Rcl @ pl.T + tcl[:, np.newaxis]).T
|
|
240
|
-
u = (K @ po.T).T
|
|
241
|
-
u_mask = u[:, 2] != 0
|
|
242
|
-
u = u[:, :2][u_mask] / u[:, 2][u_mask][:, np.newaxis]
|
|
243
|
-
|
|
244
|
-
radius = viewer.psize # Radius of the points to be drawn
|
|
245
|
-
u = np.round(u).astype(np.int32)
|
|
246
|
-
valid_x = (u[:, 0] >= radius) & (u[:, 0] < image_un.shape[1]-radius)
|
|
247
|
-
valid_y = (u[:, 1] >= radius) & (u[:, 1] < image_un.shape[0]-radius)
|
|
248
|
-
valid_points = valid_x & valid_y
|
|
249
|
-
u = u[valid_points]
|
|
250
|
-
|
|
251
|
-
intensity = cloud_local['color'][u_mask][valid_points]
|
|
252
|
-
intensity_color = rainbow(intensity).astype(np.uint8)
|
|
253
|
-
draw_image = image_un.copy()
|
|
254
|
-
draw_image = draw_larger_points(draw_image, u, intensity_color, radius)
|
|
255
|
-
rgb = image_un[u[:, 1], u[:, 0]]
|
|
256
|
-
xyz = cloud_local['xyz'][u_mask][valid_points]
|
|
257
|
-
color = (intensity.astype(np.uint32) << 24) | \
|
|
258
|
-
(rgb[:, 0].astype(np.uint32) << 16) | \
|
|
259
|
-
(rgb[:, 1].astype(np.uint32) << 8) | \
|
|
260
|
-
(rgb[:, 2].astype(np.uint32) << 0)
|
|
261
|
-
cloud_accum_color = np.rec.fromarrays(
|
|
262
|
-
[xyz, color],
|
|
263
|
-
dtype=viewer['scan'].data_type)
|
|
264
|
-
viewer['img'].setData(data=draw_image)
|
|
265
|
-
|
|
266
|
-
|
|
267
|
-
def main():
|
|
268
|
-
global viewer
|
|
269
|
-
# Set up argument parser
|
|
270
|
-
parser = argparse.ArgumentParser(
|
|
271
|
-
description="Configure topic names for LiDAR, image, and camera info.")
|
|
272
|
-
parser.add_argument(
|
|
273
|
-
'--lidar', type=str, default='lidar', help="Topic of LiDAR data")
|
|
274
|
-
parser.add_argument('--camera', type=str, default='image_raw',
|
|
275
|
-
help="Topic of camera image data")
|
|
276
|
-
parser.add_argument('--camera_info', type=str,
|
|
277
|
-
default='camera_info', help="Topic of camera info")
|
|
278
|
-
args = parser.parse_args()
|
|
279
|
-
|
|
280
|
-
app = QApplication(['LiDAR Cam Calib'])
|
|
281
|
-
viewer = ViewerWithPanel(name='LiDAR Cam Calib')
|
|
282
|
-
grid_item = GridItem(size=10, spacing=1, color=(0, 0, 0, 70))
|
|
283
|
-
scan_item = CloudItem(size=2, alpha=1, color_mode='I')
|
|
284
|
-
img_item = ImageItem(pos=np.array([0, 0]), size=np.array([800, 600]))
|
|
285
|
-
viewer.addItems({'scan': scan_item, 'grid': grid_item, 'img': img_item})
|
|
286
|
-
|
|
287
|
-
rospy.init_node('lidar_cam_calib', anonymous=True)
|
|
288
|
-
|
|
289
|
-
# Use topic names from arguments
|
|
290
|
-
rospy.Subscriber(args.lidar, PointCloud2, scanCB, queue_size=1)
|
|
291
|
-
rospy.Subscriber(args.camera, Image, imageCB, queue_size=1)
|
|
292
|
-
rospy.Subscriber(args.camera_info, CameraInfo, cameraInfoCB, queue_size=1)
|
|
293
|
-
|
|
294
|
-
viewer.show()
|
|
295
|
-
app.exec_()
|
|
296
|
-
|
|
297
|
-
|
|
298
|
-
if __name__ == '__main__':
|
|
299
|
-
main()
|
q3dviewer/tools/mesh_viewer.py
DELETED
|
@@ -1,57 +0,0 @@
|
|
|
1
|
-
#!/usr/bin/env python3
|
|
2
|
-
|
|
3
|
-
import numpy as np
|
|
4
|
-
import q3dviewer as q3d
|
|
5
|
-
from pyqtgraph.opengl import GLMeshItem
|
|
6
|
-
from stl import mesh
|
|
7
|
-
|
|
8
|
-
|
|
9
|
-
class MeshViewer(q3d.Viewer):
|
|
10
|
-
def __init__(self):
|
|
11
|
-
super(MeshViewer, self).__init__(name="Mesh Viewer")
|
|
12
|
-
self.setAcceptDrops(True)
|
|
13
|
-
|
|
14
|
-
def dragEnterEvent(self, event):
|
|
15
|
-
if event.mimeData().hasUrls():
|
|
16
|
-
event.accept()
|
|
17
|
-
else:
|
|
18
|
-
event.ignore()
|
|
19
|
-
|
|
20
|
-
def dropEvent(self, event):
|
|
21
|
-
for url in event.mimeData().urls():
|
|
22
|
-
file_path = url.toLocalFile()
|
|
23
|
-
self.openMeshFile(file_path)
|
|
24
|
-
|
|
25
|
-
def openMeshFile(self, file):
|
|
26
|
-
mesh_item = self['mesh']
|
|
27
|
-
if mesh_item is None:
|
|
28
|
-
print("Can't find meshitem")
|
|
29
|
-
return
|
|
30
|
-
|
|
31
|
-
print("Try to load %s ..." % file)
|
|
32
|
-
stl_mesh = mesh.Mesh.from_file(file)
|
|
33
|
-
|
|
34
|
-
vertices = stl_mesh.points.reshape(-1, 3)
|
|
35
|
-
faces = np.arange(vertices.shape[0]).reshape(-1, 3)
|
|
36
|
-
mesh_item.setMeshData(vertexes=vertices, faces=faces)
|
|
37
|
-
|
|
38
|
-
|
|
39
|
-
def main():
|
|
40
|
-
app = q3d.QApplication([])
|
|
41
|
-
viewer = MeshViewer()
|
|
42
|
-
|
|
43
|
-
grid_item = q3d.GridItem(size=1000, spacing=20)
|
|
44
|
-
# 'glOptions', 'opaque', 'additive' 'translucent'
|
|
45
|
-
mesh_item = GLMeshItem(smooth=True, drawFaces=True, drawEdges=True,
|
|
46
|
-
color=(0, 1, 0, 0.2),
|
|
47
|
-
edgeColor=(1, 1, 1, 1),
|
|
48
|
-
glOptions='translucent')
|
|
49
|
-
|
|
50
|
-
viewer.addItems({'grid': grid_item, 'mesh': mesh_item})
|
|
51
|
-
|
|
52
|
-
viewer.show()
|
|
53
|
-
app.exec_()
|
|
54
|
-
|
|
55
|
-
|
|
56
|
-
if __name__ == '__main__':
|
|
57
|
-
main()
|