forked from felixchenfy/3D-Scanner-by-Baxter
-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathcalib_camera_pose.launch
More file actions
66 lines (48 loc) · 2.79 KB
/
calib_camera_pose.launch
File metadata and controls
66 lines (48 loc) · 2.79 KB
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
<launch>
<!-- Though display in opencv, I als pub the result image to topic. -->
<arg name="topic_to_pub_calib_result_image" default="my/image_chessboard_pose" doc=""/>
<node name="calib_camera_pose" type="calib_camera_pose.py" pkg="scan3d_by_baxter" output = "screen">
<!-- !!! Set to false if not debug !!! -->
<param name="DEBUG_MODE" value="false" />
<!-- <param name="DEBUG_MODE" value="true" /> -->
<!-- Input image topics -->
<param name="topic_baxter_left_hand_camera" value="/cameras/left_hand_camera/image" />
<param name="topic_rgbd_camera" value="/camera/color/image_raw" />
<!-- WARNING: Baxter's camera info is for its 1280x800 size image. If image resized, info won't changed, which is wrong! S**t!
So if you resize the image, you need to manually input the camera info.
Here I simply use the default camera size, which is 1280x800:
rosrun baxter_tools camera_control.py -o left_hand_camera -r 1280x800 -->
<!-- Chessboard params -->
<param name="chessboard_square_size" value="0.02455555" />
<param name="chessboard_checker_rows" value="7" />
<param name="chessboard_checker_cols" value="9" />
<!-- Baxter: which frame is rgb-d camera connected to -->
<param name="the_frame_depth_camera_connected_to" value="/left_lower_forearm" />
<!-- Output file to store calibration result (T) -->
<param name="file_folder" value="$(find scan3d_by_baxter)/config/" />
<param name="file_name_T_baxter_to_chess" value="T_baxter_to_chess.txt" />
<param name="file_name_T_arm_to_depth" value="T_arm_to_depth.txt" />
<!-- Output to rqt_image_viewer -->
<param name="topic_to_pub_calib_result_image" value="$(arg topic_to_pub_calib_result_image)" />
</node>
<!-- Display result in rqt_image_view -->
<!-- <node name="rqt_image_view" type="rqt_image_view" pkg="rqt_image_view"
output = "screen" args="image:=/$(arg topic_to_pub_calib_result_image)">
</node> -->
<!-- Show image from depth_camera in rviz -->
<node type="rviz" name="rviz_depth_image" pkg="rviz"
args="-d $(find scan3d_by_baxter)/config/rviz_depth_image_2.rviz"
/>
</launch>
<!-- A typical calibration result is shown here:
Get/Save T_baxter_to_chess:
[[ 0.9950652 0.07175472 -0.0685311 0.42526041]
[ 0.06799306 -0.99612797 -0.05573167 0.14072247]
[-0.07226475 0.05079701 -0.99609109 -0.87945118]
[ 0. 0. 0. 1. ]]
Get/Save T_arm_to_depth:
[[ 0.98484448 -0.16254042 -0.06051409 0.011358 ]
[ 0.16413439 0.98618497 0.0223407 -0.13841626]
[ 0.05604682 -0.03193455 0.9979173 -0.13285034]
[ 0. 0. 0. 1. ]]
-->