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.
@@ -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()
@@ -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()