Attachment 'kinect.patch'
Download 1 Index: pr2_calibration_launch/view_results/head_arm_config.yaml
2 ===================================================================
3 --- pr2_calibration_launch/view_results/head_arm_config.yaml (revision 48511)
4 +++ pr2_calibration_launch/view_results/head_arm_config.yaml (working copy)
5 @@ -65,3 +65,17 @@
6 plot_ops:
7 color: r
8 marker: s
9 +
10 +- name: "Kinect: Left Arm"
11 + 3d: left_arm_chain
12 + cam: kinect_rect
13 + plot_ops:
14 + color: r
15 + marker: o
16 +
17 +- name: "Kinect: Right Arm"
18 + 3d: right_arm_chain
19 + cam: kinect_rect
20 + plot_ops:
21 + color: r
22 + marker: s
23 Index: pr2_calibration_launch/view_results/head_laser_config.yaml
24 ===================================================================
25 --- pr2_calibration_launch/view_results/head_laser_config.yaml (revision 48511)
26 +++ pr2_calibration_launch/view_results/head_laser_config.yaml (working copy)
27 @@ -33,3 +33,10 @@
28 plot_ops:
29 color: m
30 marker: o
31 +
32 +- name: "Tilt Laser: Kinect"
33 + 3d: tilt_laser
34 + cam: kinect_rect
35 + plot_ops:
36 + color: m
37 + marker: o
38 Index: pr2_calibration_launch/capture_data/intersection.launch
39 ===================================================================
40 --- pr2_calibration_launch/capture_data/intersection.launch (revision 48511)
41 +++ pr2_calibration_launch/capture_data/intersection.launch (working copy)
42 @@ -14,6 +14,7 @@
43 <remap from="forearm_right_rect" to="r_forearm_cam/settled_interval" />
44 <remap from="forearm_left_rect" to="l_forearm_cam/settled_interval" />
45 <remap from="prosilica_rect" to="prosilica/settled_interval" />
46 + <remap from="kinect_rect" to="camera/rgb/settled_interval" />
47
48 <remap from="interval" to="request_interval" />
49 </node>
50 @@ -22,4 +23,4 @@
51
52
53
54 -</launch>
55 \ No newline at end of file
56 +</launch>
57 Index: pr2_calibration_launch/capture_data/hardware_config/cam_config.yaml
58 ===================================================================
59 --- pr2_calibration_launch/capture_data/hardware_config/cam_config.yaml (revision 48511)
60 +++ pr2_calibration_launch/capture_data/hardware_config/cam_config.yaml (working copy)
61 @@ -279,3 +279,42 @@
62 subpixel_zero_zone: 1
63 led_detector:
64 active: False
65 +
66 +kinect_rect:
67 + cb_detector_config: /camera/rgb/cb_detector_config
68 + led_detector_config: /camera/rgb/led_detector
69 + settler_config: /camera/rgb/monocam_settler_config
70 +
71 + configs:
72 + cb_7x6:
73 + settler:
74 + tolerance: 2.00
75 + ignore_failures: True
76 + max_step: 1.0
77 + cache_size: 100
78 + cb_detector:
79 + active: True
80 + num_x: 7
81 + num_y: 6
82 + width_scaling: 1
83 + height_scaling: 1
84 + subpixel_window: 4
85 + subpixel_zero_zone: 1
86 + led_detector:
87 + active: False
88 + small_cb_4x5:
89 + settler:
90 + tolerance: 2.00
91 + ignore_failures: True
92 + max_step: 1.0
93 + cache_size: 100
94 + cb_detector:
95 + active: True
96 + num_x: 4
97 + num_y: 5
98 + width_scaling: 1
99 + height_scaling: 1
100 + subpixel_window: 4
101 + subpixel_zero_zone: 1
102 + led_detector:
103 + active: False
104 \ No newline at end of file
105 Index: pr2_calibration_launch/capture_data/samples_pr2_beta/far/far_0000.yaml
106 ===================================================================
107 --- pr2_calibration_launch/capture_data/samples_pr2_beta/far/far_0000.yaml (revision 48511)
108 +++ pr2_calibration_launch/capture_data/samples_pr2_beta/far/far_0000.yaml (working copy)
109 @@ -4,6 +4,7 @@
110 - {cam_id: wide_right_rect, config: cb_7x6}
111 - {cam_id: narrow_right_rect, config: cb_7x6}
112 - {cam_id: prosilica_rect, config: cb_7x6}
113 +- {cam_id: kinect_rect, config: cb_7x6}
114 joint_commands:
115 #- controller: head_traj_controller
116 # segments:
117 Index: pr2_calibration_launch/capture_data/samples_pr2_beta/left/l_arm_0000.yaml
118 ===================================================================
119 --- pr2_calibration_launch/capture_data/samples_pr2_beta/left/l_arm_0000.yaml (revision 48511)
120 +++ pr2_calibration_launch/capture_data/samples_pr2_beta/left/l_arm_0000.yaml (working copy)
121 @@ -4,6 +4,7 @@
122 - {cam_id: wide_right_rect, config: small_cb_4x5}
123 - {cam_id: narrow_right_rect, config: small_cb_4x5}
124 - {cam_id: prosilica_rect, config: small_cb_4x5}
125 +- {cam_id: kinect_rect, config: small_cb_4x5}
126 joint_commands:
127 - controller: head_traj_controller
128 segments:
129 Index: pr2_calibration_launch/capture_data/samples_pr2_beta/left/l_arm_0001.yaml
130 ===================================================================
131 --- pr2_calibration_launch/capture_data/samples_pr2_beta/left/l_arm_0001.yaml (revision 48511)
132 +++ pr2_calibration_launch/capture_data/samples_pr2_beta/left/l_arm_0001.yaml (working copy)
133 @@ -4,6 +4,7 @@
134 - {cam_id: wide_right_rect, config: small_cb_4x5}
135 - {cam_id: narrow_right_rect, config: small_cb_4x5}
136 - {cam_id: prosilica_rect, config: small_cb_4x5}
137 +- {cam_id: kinect_rect, config: small_cb_4x5}
138 joint_commands:
139 - controller: head_traj_controller
140 segments:
141 Index: pr2_calibration_launch/capture_data/samples_pr2_beta/right/r_arm_0000.yaml
142 ===================================================================
143 --- pr2_calibration_launch/capture_data/samples_pr2_beta/right/r_arm_0000.yaml (revision 48511)
144 +++ pr2_calibration_launch/capture_data/samples_pr2_beta/right/r_arm_0000.yaml (working copy)
145 @@ -4,6 +4,7 @@
146 - {cam_id: wide_right_rect, config: small_cb_4x5}
147 - {cam_id: narrow_right_rect, config: small_cb_4x5}
148 - {cam_id: prosilica_rect, config: small_cb_4x5}
149 +- {cam_id: kinect_rect, config: small_cb_4x5}
150 joint_commands:
151 - controller: head_traj_controller
152 segments:
153 Index: pr2_calibration_launch/capture_data/samples_pr2_beta/right/r_arm_0001.yaml
154 ===================================================================
155 --- pr2_calibration_launch/capture_data/samples_pr2_beta/right/r_arm_0001.yaml (revision 48511)
156 +++ pr2_calibration_launch/capture_data/samples_pr2_beta/right/r_arm_0001.yaml (working copy)
157 @@ -4,6 +4,7 @@
158 - {cam_id: wide_right_rect, config: small_cb_4x5}
159 - {cam_id: narrow_right_rect, config: small_cb_4x5}
160 - {cam_id: prosilica_rect, config: small_cb_4x5}
161 +- {cam_id: kinect_rect, config: small_cb_4x5}
162 joint_commands:
163 - controller: head_traj_controller
164 segments:
165 Index: pr2_calibration_launch/capture_data/all_viewers.launch
166 ===================================================================
167 --- pr2_calibration_launch/capture_data/all_viewers.launch (revision 48511)
168 +++ pr2_calibration_launch/capture_data/all_viewers.launch (working copy)
169 @@ -15,6 +15,9 @@
170 <include file="$(find pr2_calibration_launch)/capture_data/monocam_viewer.launch"
171 ns="prosilica" />
172
173 + <include file="$(find pr2_calibration_launch)/capture_data/monocam_viewer.launch"
174 + ns="camera/rgb" />
175 +
176 <include file="$(find pr2_calibration_launch)/capture_data/double_stereo_viewer.launch" />
177
178 <include file="$(find pr2_calibration_launch)/capture_data/monocam_viewer.launch"
179 Index: pr2_calibration_launch/capture_data/all_pipelines.launch
180 ===================================================================
181 --- pr2_calibration_launch/capture_data/all_pipelines.launch (revision 48511)
182 +++ pr2_calibration_launch/capture_data/all_pipelines.launch (working copy)
183 @@ -25,6 +25,13 @@
184 name="forearm_r_throttle"
185 args="messages r_forearm_cam/image_rect 2" />
186
187 + <include file="$(find pr2_calibration_launch)/capture_data/monocam_pipeline.launch"
188 + ns="camera/rgb" />
189 + <node type="throttle"
190 + pkg="topic_tools"
191 + name="kinect_throttle"
192 + args="messages camera/rgb/image_mono 2 camera/rgb/image_rect_throttle" />
193 +
194 <include file="$(find pr2_calibration_launch)/capture_data/intersection.launch" />
195 <include file="$(find pr2_calibration_launch)/capture_data/laser_pipelines.launch" />
196
197 Index: pr2_calibration_launch/capture_data/capture_exec_pr2.launch
198 ===================================================================
199 --- pr2_calibration_launch/capture_data/capture_exec_pr2.launch (revision 48511)
200 +++ pr2_calibration_launch/capture_data/capture_exec_pr2.launch (working copy)
201 @@ -7,6 +7,11 @@
202 output="screen"
203 machine="c1">
204
205 + <remap from="kinect_rect/camera_info" to="camera/rgb/camera_info"/>
206 + <remap from="kinect_rect/image_rect" to="camera/rgb/image_mono"/>
207 + <remap from="kinect_rect/image" to="camera/rgb/image_mono"/>
208 + <remap from="kinect_rect/features" to="camera/rgb/features"/>
209 +
210 <remap from="prosilica_rect/camera_info" to="prosilica/camera_info"/>
211 <remap from="prosilica_rect/image_rect" to="prosilica/image_rect"/>
212 <remap from="prosilica_rect/image" to="prosilica/image_raw"/>
213 Index: pr2_calibration_launch/estimate_params/config_pr2_beta/free_arms_only_1.yaml
214 ===================================================================
215 --- pr2_calibration_launch/estimate_params/config_pr2_beta/free_arms_only_1.yaml (revision 48511)
216 +++ pr2_calibration_launch/estimate_params/config_pr2_beta/free_arms_only_1.yaml (working copy)
217 @@ -51,6 +51,9 @@
218 high_def_frame_joint: [ 0, 0, 0, 0, 0, 0 ]
219 high_def_optical_frame_joint: [ 0, 0, 0, 0, 0, 0 ]
220
221 + openni_rgb_frame_joint: [ 0, 0, 0, 0, 0, 0 ]
222 + openni_rgb_optical_frame_joint: [ 0, 0, 0, 0, 0, 0 ]
223 +
224 l_forearm_roll_adj: [ 0, 0, 0, 0, 0, 0 ]
225 l_forearm_cam_frame_joint: [ 1, 1, 1, 1, 1, 1 ]
226 l_forearm_cam_optical_frame_joint: [ 0, 0, 0, 0, 0, 0 ]
227 Index: pr2_calibration_launch/estimate_params/config_pr2_beta/system.yaml
228 ===================================================================
229 --- pr2_calibration_launch/estimate_params/config_pr2_beta/system.yaml (revision 48511)
230 +++ pr2_calibration_launch/estimate_params/config_pr2_beta/system.yaml (working copy)
231 @@ -78,6 +78,10 @@
232 high_def_frame_joint: [0, -0.109, 0.035, 0, 0, 0]
233 high_def_optical_frame_joint: [0, 0, 0, -1.2091995761561452, 1.2091995761561452, -1.2091995761561452]
234
235 + # Kinect RGB Frames
236 + openni_rgb_frame_joint: [0, -0.109, 0.035, 0, 0, 0]
237 + openni_rgb_optical_frame_joint: [0, 0, 0, -1.2091995761561452, 1.2091995761561452, -1.2091995761561452]
238 +
239 rectified_cams:
240 wide_left_rect:
241 baseline_shift: 0.0
242 @@ -109,6 +113,13 @@
243 cx_shift: 0.0
244 cy_shift: 0.0
245 cov: {u: 0.25, v: 0.25}
246 + kinect_rect:
247 + baseline_shift: 0.0
248 + f_shift: 0.0
249 + cx_shift: 0.0
250 + cy_shift: 0.0
251 + cov: {u: 0.25, v: 0.25}
252 +
253 forearm_left_rect:
254 baseline_shift: 0.0
255 f_shift: 0.0
256 Index: pr2_calibration_launch/estimate_params/config_pr2_beta/free_head_laser_0.yaml
257 ===================================================================
258 --- pr2_calibration_launch/estimate_params/config_pr2_beta/free_head_laser_0.yaml (revision 48511)
259 +++ pr2_calibration_launch/estimate_params/config_pr2_beta/free_head_laser_0.yaml (working copy)
260 @@ -51,6 +51,9 @@
261 high_def_frame_joint: [ 0, 0, 0, 0, 0, 0 ]
262 high_def_optical_frame_joint: [ 0, 0, 0, 0, 0, 0 ]
263
264 + openni_rgb_frame_joint: [ 0, 0, 0, 0, 0, 0 ]
265 + openni_rgb_optical_frame_joint: [ 0, 0, 0, 0, 0, 0 ]
266 +
267 l_forearm_roll_adj: [ 0, 0, 0, 0, 0, 0 ]
268 l_forearm_cam_frame_joint: [ 0, 0, 0, 0, 0, 0 ]
269 l_forearm_cam_optical_frame_joint: [ 0, 0, 0, 0, 0, 0 ]
270 Index: pr2_calibration_launch/estimate_params/config_pr2_beta/free_head_laser_1.yaml
271 ===================================================================
272 --- pr2_calibration_launch/estimate_params/config_pr2_beta/free_head_laser_1.yaml (revision 48511)
273 +++ pr2_calibration_launch/estimate_params/config_pr2_beta/free_head_laser_1.yaml (working copy)
274 @@ -51,6 +51,9 @@
275 high_def_frame_joint: [ 0, 0, 0, 0, 0, 0 ]
276 high_def_optical_frame_joint: [ 0, 0, 0, 0, 0, 0 ]
277
278 + openni_rgb_frame_joint: [ 0, 0, 0, 0, 0, 0 ]
279 + openni_rgb_optical_frame_joint: [ 0, 0, 0, 0, 0, 0 ]
280 +
281 l_forearm_roll_adj: [ 0, 0, 0, 0, 0, 0 ]
282 l_forearm_cam_frame_joint: [ 0, 0, 0, 0, 0, 0 ]
283 l_forearm_cam_optical_frame_joint: [ 0, 0, 0, 0, 0, 0 ]
284 Index: pr2_calibration_launch/estimate_params/config_pr2_beta/free_head_laser_2.yaml
285 ===================================================================
286 --- pr2_calibration_launch/estimate_params/config_pr2_beta/free_head_laser_2.yaml (revision 48511)
287 +++ pr2_calibration_launch/estimate_params/config_pr2_beta/free_head_laser_2.yaml (working copy)
288 @@ -51,6 +51,9 @@
289 high_def_frame_joint: [ 0, 0, 0, 0, 0, 0 ]
290 high_def_optical_frame_joint: [ 0, 0, 0, 0, 0, 0 ]
291
292 + openni_rgb_frame_joint: [ 0, 0, 0, 0, 0, 0 ]
293 + openni_rgb_optical_frame_joint: [ 0, 0, 0, 0, 0, 0 ]
294 +
295 l_forearm_roll_adj: [ 0, 0, 0, 0, 0, 0 ]
296 l_forearm_cam_frame_joint: [ 0, 0, 0, 0, 0, 0 ]
297 l_forearm_cam_optical_frame_joint: [ 0, 0, 0, 0, 0, 0 ]
298 Index: pr2_calibration_launch/estimate_params/config_pr2_beta/free_head_laser_3.yaml
299 ===================================================================
300 --- pr2_calibration_launch/estimate_params/config_pr2_beta/free_head_laser_3.yaml (revision 48511)
301 +++ pr2_calibration_launch/estimate_params/config_pr2_beta/free_head_laser_3.yaml (working copy)
302 @@ -51,6 +51,9 @@
303 high_def_frame_joint: [ 0, 0, 0, 0, 0, 0 ]
304 high_def_optical_frame_joint: [ 0, 0, 0, 0, 0, 0 ]
305
306 + openni_rgb_frame_joint: [ 0, 0, 0, 0, 0, 0 ]
307 + openni_rgb_optical_frame_joint: [ 0, 0, 0, 0, 0, 0 ]
308 +
309 l_forearm_roll_adj: [ 0, 0, 0, 0, 0, 0 ]
310 l_forearm_cam_frame_joint: [ 0, 0, 0, 0, 0, 0 ]
311 l_forearm_cam_optical_frame_joint: [ 0, 0, 0, 0, 0, 0 ]
312 Index: pr2_calibration_launch/estimate_params/config_pr2_beta/free_head_laser_4.yaml
313 ===================================================================
314 --- pr2_calibration_launch/estimate_params/config_pr2_beta/free_head_laser_4.yaml (revision 48511)
315 +++ pr2_calibration_launch/estimate_params/config_pr2_beta/free_head_laser_4.yaml (working copy)
316 @@ -51,6 +51,9 @@
317 high_def_frame_joint: [ 1, 1, 1, 1, 1, 1 ]
318 high_def_optical_frame_joint: [ 0, 0, 0, 0, 0, 0 ]
319
320 + openni_rgb_frame_joint: [ 1, 1, 1, 1, 1, 1 ]
321 + openni_rgb_optical_frame_joint: [ 0, 0, 0, 0, 0, 0 ]
322 +
323 l_forearm_roll_adj: [ 0, 0, 0, 0, 0, 0 ]
324 l_forearm_cam_frame_joint: [ 0, 0, 0, 0, 0, 0 ]
325 l_forearm_cam_optical_frame_joint: [ 0, 0, 0, 0, 0, 0 ]
326 Index: pr2_calibration_launch/estimate_params/config_pr2_beta/sensors.yaml
327 ===================================================================
328 --- pr2_calibration_launch/estimate_params/config_pr2_beta/sensors.yaml (revision 48511)
329 +++ pr2_calibration_launch/estimate_params/config_pr2_beta/sensors.yaml (working copy)
330 @@ -41,6 +41,15 @@
331 dh_link_num: 1 # Connects after head pan
332 after_chain: [head_chain_tip_adj, head_plate_frame_joint, high_def_frame_joint, high_def_optical_frame_joint]
333
334 + - sensor_id: kinect_rect
335 + camera_id: kinect_rect
336 + chain:
337 + before_chain: [head_pan_joint]
338 + chain_id: head_chain
339 + dh_link_num: 1 # Connects after head pan
340 + after_chain: [head_chain_tip_adj, head_plate_frame_joint, openni_rgb_frame_joint, openni_rgb_optical_frame_joint]
341 +
342 +
343 - camera_id: forearm_left_rect
344 sensor_id: forearm_left_rect
345 chain:
346 Index: pr2_calibration_launch/estimate_params/head_then_arms_params.launch
347 ===================================================================
348 --- pr2_calibration_launch/estimate_params/head_then_arms_params.launch (revision 48511)
349 +++ pr2_calibration_launch/estimate_params/head_then_arms_params.launch (working copy)
350 @@ -74,10 +74,11 @@
351 - wide_right_rect
352 - tilt_laser
353 - prosilica_rect
354 + - kinect_rect
355 </rosparam>
356 <param name="output_filename" type="string" value="head_laser_calibrated" />
357 </group>
358 -
359 +
360 <group ns="B - 01 - Locking Head Sensors. Estimating Arm Joint Offsets">
361 <param name="free_params" textfile="$(find pr2_calibration_launch)/estimate_params/config_pr2_beta/free_arms_only_1.yaml" />
362 <param name="use_cov" type="bool" value="True" />
363 Index: pr2_calibration_estimation/test/pr2_head_fk_unittest.py
364 ===================================================================
365 --- pr2_calibration_estimation/test/pr2_head_fk_unittest.py (revision 48511)
366 +++ pr2_calibration_estimation/test/pr2_head_fk_unittest.py (working copy)
367 @@ -217,6 +217,30 @@
368 cmds = self.loadCommands('head_commands')
369 self.run_test(full_chain, 'torso_lift_link', 'high_def_optical_frame', cmds)
370
371 + def test_kinect_def(self):
372 + print ""
373 + config_str = '''
374 + before_chain: [head_pan_joint]
375 + chain_id: head_chain
376 + after_chain: [head_chain_tip_adj, head_plate_frame_joint, openni_rgb_frame_joint]
377 + dh_link_num: 1 '''
378 + full_chain = FullChainRobotParams(yaml.load(config_str))
379 + full_chain.update_config(self.robot_params)
380 + cmds = self.loadCommands('head_commands')
381 + self.run_test(full_chain, 'torso_lift_link', 'openni_rgb_frame', cmds)
382 +
383 + def test_kinect_optical(self):
384 + print ""
385 + config_str = '''
386 + before_chain: [head_pan_joint]
387 + chain_id: head_chain
388 + after_chain: [head_chain_tip_adj, head_plate_frame_joint, openni_rgb_frame_joint, openni_rgb_optical_frame_joint]
389 + dh_link_num: 1 '''
390 + full_chain = FullChainRobotParams(yaml.load(config_str))
391 + full_chain.update_config(self.robot_params)
392 + cmds = self.loadCommands('head_commands')
393 + self.run_test(full_chain, 'torso_lift_link', 'openni_rgb_optical_frame', cmds)
394 +
395 def check_tilt_laser(self, cmd):
396 actual_T = self.robot_params.tilting_lasers["tilt_laser"].compute_pose([cmd])
397 expected_T = self.getExpected("torso_lift_link", "laser_tilt_link", [cmd])
398 Index: pr2_calibration_estimation/src/pr2_calibration_estimation/multi_step_cov_estimator.py
399 ===================================================================
400 --- pr2_calibration_estimation/src/pr2_calibration_estimation/multi_step_cov_estimator.py (revision 48511)
401 +++ pr2_calibration_estimation/src/pr2_calibration_estimation/multi_step_cov_estimator.py (working copy)
402 @@ -43,6 +43,7 @@
403 import yaml
404 import os.path
405 import numpy
406 +import rosrecord
407
408 import stat
409 import os
Attached Files
To refer to attachments on a page, use attachment:filename, as shown below in the list of files. Do NOT use the URL of the [get] link, since this is subject to change and can break easily.You are not allowed to attach a file to this page.
