使用Realsense 測試 aruco 包

aruco是一種類似二維碼的定位標記輔助工具，通過在環境中部署Markers，可以輔助機器人進行定位，彌補單一傳感器的缺陷，糾正誤差。在之前的手眼標定easy_handeye程序中，需要使用這個工具進行手眼標定。正好手上有realsense深度相機，就根據資料進行了簡單的測試。如下圖是手眼標定程序包中的一段：

通過github下載aruco相關的ROS包，裏面會提供現成的Markers，也可以通過aruco包自己生成，或者通過網站http://chev.me/arucogen/可以生成這種圖案.該包應用的場景很多，很重要的功能就是物體位姿估計和對物體進行定位。從GitHub上下載包後，裏面包括了三個包，其中aruco包中只有一個可運行的節點，發現官網文檔沒細說，我簡單的看了以下程序，發現可以通過運行該節點並指定參數來生成圖案：

$ rosrun aruco optimalmarkers 1 marker 500
第一個參數是生成圖案的個數   
第二個參數是名稱+標記id      
第三個參數是輸出標記圖像的像素大小  

另外一個msgs包不用細看：

 * aruco_ros/Marker.msg

        Header header
        uint32 id
        geometry_msgs/PoseWithCovariance pose
        float64 confidence

 * aruco_ros/MarkerArray.msg

        Header header
        aruco_ros/Marker[] markers

重點關注aruco_ros這個包：包中有三個launch文件，分別是single、double、publish，原本的用例是通過USB攝像頭工作的，對於realsense或者其他相機，就需要修改參數，修改single.launch如下：

<launch>

    <arg name="markerId"        default="582"/>
    <arg name="markerSize"      default="0.034"/>    <!-- in m -->
    <arg name="eye"             default="left"/>
    <arg name="marker_frame"    default="aruco_marker_frame"/>
    <arg name="ref_frame"       default=""/>  <!-- leave empty and the pose will be published wrt param parent_name -->
    <arg name="corner_refinement" default="LINES" /> <!-- NONE, HARRIS, LINES, SUBPIX -->


    <node pkg="aruco_ros" type="single" name="aruco_single">
<!--        <remap from="/camera_info" to="/stereo/$(arg eye)/camera_info" />-->
<!--        <remap from="/image" to="/stereo/$(arg eye)/image_rect_color" />-->
        <remap from="/camera_info" to="/camera/color/camera_info" />
        <remap from="/image" to="/camera/color/image_raw" />
        <param name="image_is_rectified" value="True"/>
        <param name="marker_size"        value="$(arg markerSize)"/>
        <param name="marker_id"          value="$(arg markerId)"/>
        <param name="reference_frame"    value="$(arg ref_frame)"/>   <!-- frame in which the marker pose will be refered -->
<!--        <param name="camera_frame"       value="stereo_gazebo_$(arg eye)_camera_optical_frame"/>-->
        <param name="camera_frame"       value="/camera_link"/>
        <param name="marker_frame"       value="$(arg marker_frame)" />
        <param name="corner_refinement"  value="$(arg corner_refinement)" />
    </node>

</launch>

即將原本的usb或者stereo換成realsense發佈的信息，將相機座標系更換成camera_link：

<remap from="/camera_info" to="/camera/color/camera_info" />        
<remap from="/image" to="/camera/color/image_raw" />

<param name="camera_frame"       value="/camera_link"/>

接下來依次啓動節點，讓相機觀察圖案：

$ roslaunch realsense2_camera rs_camera.launch

# 不指定參數，使用默認參數
$ roslaunch aruco_ros single.launch 
# 或指定參數
$ roslaunch aruco_ros single.launch markerId:=26 markerSize:=0.08 eye:="right"
# 或指定姿態相對的參考座標系
$ roslaunch aruco_ros single.launch markerId:=26 markerSize:=0.08 eye:="right" ref_frame:=/base_link

# 觀察識別效果
$ rosrun image_view image_view image:=/aruco_single/result

# 查看位姿
$ rostopic echo /aruco_single/pose

如圖所示可以檢測到Marker，並識別出其ID，但是對於普通usb攝像頭要想得到pose，還需要使用ROS的標定工具對攝像頭進行標定，獲取其相機內參和畸變係數，標定完成後文件會默認保存在~/.ros/camera_info/head_camera.yaml。如果沒有進行標定，在啓動攝像頭時也會提示~/.ros/camera_info/head_camera.ya不存在。參考：https://blog.csdn.net/learning_tortosie/article/details/83147232

對於realsense之類的深度相機，出廠就有默認標定好的參數了，所以不需要像usb攝像頭那樣，只要launch文件中的參數設置正確，就可以獲取pose信息，對於測試所用的圖案的參數<arg name="markerId"        default="582"/>，注意ID要匹配