ros下實作usb相機功能，并基于apriltag_ros實作定位

1.安裝usb_cam軟體包

sudo apt-get install ros-kinetic-usb-cam
           

安裝完成後可以啟動相機

roslaunch usb_cam usb_cam-test.launch
           

launch檔案在 /opt/ros/kinetic/share/usb_cam/launch 目錄下，launch檔案如下：

<launch>
  <node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="screen" >
    <param name="video_device" value="/dev/video1" />
    <param name="image_width" value="640" />
    <param name="image_height" value="480" />
    <param name="pixel_format" value="yuyv" />
    <param name="camera_frame_id" value="usb_cam" />
    <param name="io_method" value="mmap"/>
  </node>
  <node name="image_view" pkg="image_view" type="image_view" respawn="false" output="screen">
    <remap from="image" to="/usb_cam/image_raw"/>
    <param name="autosize" value="true" />
  </node>
</launch>

           

可通過 ls /dev/video* 指令檢視目前的相機裝置，相應的更改launch檔案中的配置/dev/video1，對應裝置号，啟動自己的相機裝置。

2.使用camera_calibration軟體包進行相機内參标定

• 安裝相機标定軟體包

sudo apt-get install ros-kinetic-camera-calibration
           

• 棋盤格下載下傳

  相機的标定是以一個由黑白方塊組成的棋盤為基準進行的，如下圖所示。可以從本文的附件中下載下傳8×6國際象棋棋盤，并列印出來後将其貼到一個平坦的紙闆或者牆面上。作為參考，8×6棋盤橫向有9個方塊，是以有8個交叉點，而豎向有7個方塊，有6個交叉點，是以它被稱為8×6棋盤。

  

• 啟動usb相機

roslaunch usb_cam usb_cam-test.launch
           

• 啟動标定節點

rosrun camera_calibration cameracalibrator.py  --size 8x6 --square 0.029 image:=/usb_cam/image_raw camera:=/usb_cam
           

size :标定棋盤個的内部角點個數，這裡實用的棋盤一共有6行，每行有8個内部角點（不包括最外圍的一圈棋盤格）。

squre：這個參數對應每一個棋盤格的邊長，機關是米，棋盤格列印出來以後要自己再測量一下列印的是或否準确然後自己更改，我列印的棋盤方格大小是29mm的，故squre為0.029。

image和camera：設定攝像頭釋出的圖像話題。

• 标定節點運作後會運作GUI，此時如果用相機對準棋盤，校準将立即開始。在GUI螢幕的右側，可以看到一個标有X、Y、Size和Skew的條形控件。這是校準的進展狀态，都以綠色填滿意味着校準完成。在校準過程中需要将棋盤對着相機朝着左/右/上/下/前/後移動，還需要傾斜棋盤，這是個比較無聊的過程，我們需要拿着校準棋盤不斷的移動，晃蕩個大概5分鐘左右就差不多了，看到基本上綠色已經填滿了即可。校準所需的所有圖像都記錄下來之後，CALIBRATE按鈕會被激活。點選這個按鈕後會進行實際的校準計算，這需要大約3到5分鐘。此時就可以将校準闆放下了，耐心等待計算完成後，點選SAVE按鈕儲存校準資訊，存儲的位址顯示在執行校準的終端視窗中，是存儲在某一個/tmp目錄（如“/tmp/calibrationdata.tar.gz”）中。
• 大佬的這篇部落格講的很清楚，可直接參考此連結，我就不再贅述了。

3.apriltag安裝

• 下載下傳源碼，官方連結（https://github.com/AprilRobotics/apriltag）

git clone https://github.com/AprilRobotics/apriltag
           

• 編譯安裝

cd apriltag
cmake .
sudo make install
           

官方連結裡面有不同的安裝方式，我隻是選擇了其中的一種安裝方式。

4.apriltags2_ros的編譯

apriltag_ros源碼：源碼連結

詳細的wiki教程：wiki教程

安裝過程：

mkdir -p apriltag_ws/src
git clone https://github.com/AprilRobotics/apriltag_ros.git
cd ..
catkin_make
           

編譯完成之後，確定usb相機标定完成并且啟動之後（roslaunch usb_cam usb_cam-test.launch），運作continuous_detection.launch檔案。

roslaunch apriltags2_ros continuous_detection.launch
           

5.apriltags2_ros的配置

• 參數說明

  （1）tag_family (string, 預設值: tag36h11)

  可用于檢測的apriltag系列。一次隻能檢測一種标簽，不支援多種标簽的同時檢測。支援: tag36h11, tag36h10, tag25h9, tag25h7 and tag16h5.

  （2）tag_border (int, 預設值: 1)

  标簽外部黑色邊框的寬度，标簽中每個小正方形的邊長為1位。

  （3）tag_threads (int, 預設值: 4)

  AprilTag 2核心算法應使用多少個線程？

  （4）tag_decimate (double, 預設值: 1.0)

  可以在較低分辨率的圖像上進行四邊形的檢測，進而以提高姿勢精度和稍微降低檢測率的速度提高速度。仍以全分辨率對二進制有效負載進行解碼和姿态估計。

  （5）tag_blur (double, 預設值: 0.0)

  應該對分割後的圖像應用哪種高斯模糊。參數是像素的标準偏差。噪點較多的圖檔用非零值比較合适（例如0.8），大于0是模糊圖像，小于0是銳化圖像。

  （6）tag_refine_edges (int, 預設值: 1)

  如果不為零，則将每個四邊形的邊緣調整為“捕捉”到附近的強漸變。當采用抽取時，這很有用，因為它可以大大提高初始四邊形估計的品質。提高了角點檢測的精度，進而提高了姿态估計的精度，通常建議開啟。如果tag_decimate == 1.0，則忽略該選項。

  （7）tag_refine_decode (int, 預設值: 0)

  當非零時，增加檢測到的标簽數量，增加假陰性檢測率。對于分辨率分辨率門檻值附近的很小的标簽特别有效。

  （8）tag_refine_pose (int, 預設值: 0)

  當非零時，提高提取位姿的精度，但消耗計算資源。這是通過最大化标簽黑白邊框周圍的對比度來完成的。盡管不如tag_refine_decode那樣有效（或快速），但這通常會增加成功檢測到的标簽的數量。

  （9）publish_tf (bool, 預設值: false)

  啟用在/t話題上釋出标簽相機相對位姿，在rviz中實作可視化。

  （10）camera_frame (string, 預設值: camera)

  （11）publish_tag_detections_image (bool, 預設值: false)

  啟用釋出/tag_detections_image話題。

• launch檔案的配置

  launch檔案如下：

<launch>
  <arg name="launch_prefix" default="" /> <!-- set to value="gdbserver localhost:10000" for remote debugging -->
  <arg name="node_namespace" default="apriltag_ros_continuous_node" />
  <arg name="camera_name" default="/usb_cam" />
  <arg name="camera_frame" default="camera" />
  <arg name="image_topic" default="image_raw" />

  <!-- Set parameters -->
  <rosparam command="load" file="$(find apriltag_ros)/config/settings.yaml" ns="$(arg node_namespace)" />
  <rosparam command="load" file="$(find apriltag_ros)/config/tags.yaml" ns="$(arg node_namespace)" />
  
  <node pkg="apriltag_ros" type="apriltag_ros_continuous_node" name="$(arg node_namespace)" clear_params="true" output="screen" launch-prefix="$(arg launch_prefix)" >
    <!-- Remap topics from those used in code to those on the ROS network -->
    <remap from="image_rect" to="$(arg camera_name)/$(arg image_topic)" />
    <remap from="camera_info" to="$(arg camera_name)/camera_info" />

    <param name="camera_frame" type="str" value="$(arg camera_frame)" />
    <param name="publish_tag_detections_image" type="bool" value="true" />      <!-- default: false -->
  </node>
</launch>

           

讀取兩個yaml檔案的地方不需要修改：setting.yaml與tags.yaml

ROS标準的啟動節點（node）寫法，不過不同的是這裡做了一個重映射（remap）。我們主要需要修改的訂閱的topic。程式中需要訂閱相機圖像資料與相機資訊。外接usb相機啟動後會釋出兩個相關的topic：/usb_cam/camera_info和/usb_cam/image_raw

在此之前我們已經對usb_camera進行了相機标定，得到了camra_info的資訊。

camra_info話題的了解可以看我這篇文章

• settings.yaml檔案的配置（配置apriltag算法）

  該檔案主要是對程式一些基礎參數進行配置，包括采用何種apriltags二維碼（tag_family），是否需要釋出tf（publish_tf）等。保持預設，一般不需要修改。

• tags.yaml檔案的配置（定義要查找的标簽和标簽簇）

  （1）同一ID的tag不能該配置檔案中以不同的大小出現兩次，也不能出現在一張圖檔的兩個地方。顯然，這些都将在檢測中産生歧義，確定列印的标簽至少被1位寬的白色邊框包圍。

  （2）隻要具有相同的大小，在standalone_tags（單标簽）和tag_bundles（标簽簇）中列出具有相同ID的标記就可以了，标簽簇的檢測比單個标簽的檢測更加準确。

  （3）確定列印的tag至少包含1位寬的白色邊框，AprilTag2算法對周圍的白色邊框進行采樣。

  配置示例：

standalone_tags:
  [
    {id: 0, size: 0.05},
    {id: 1, size: 0.05},
    {id: 2, size: 0.05},
    {id: 3, size: 0.05},
    {id: 4, size: 0.05}
  ]

tag_bundles:
  [
    {
      name: 'my_bundle',
      layout:
        [
          {id: 0, size: 0.05, x: 0.0000, y: 0.0000, z: 0.0000, qw: 1.0000, qx: 0.0000, qy: 0.0000, qz: 0.0000},
          {id: 4, size: 0.05, x: 0.0548, y: -0.0522, z: -0.0057, qw: 1.0000, qx: 0.0063, qy: -0.0016, qz: 0.0010},
          {id: 3, size: 0.05, x: -0.0580, y: -0.0553, z: 0.0015, qw: 1.0000, qx: -0.0039, qy: -0.0006, qz: 0.0016},
          {id: 2, size: 0.05, x: 0.0543, y: 0.0603, z: -0.0043, qw: 1.0000, qx: -0.0016, qy: 0.0082, qz: 0.0018},
          {id: 1, size: 0.05, x: -0.0582, y: 0.0573, z: 0.0012, qw: 1.0000, qx: -0.0057, qy: -0.0028, qz: 0.0006}
        ]
    }
  ]
           

6.apriltags2_ros的運作

• 整體工作方式如下圖所示：

  

  訂閱的topic：

  （1）/camera/image_rect：包含圖像的sensor_msgs/Image話題（例如，來自錄影機的視訊流的幀）。 假定圖像是無畸變的，即由針孔照相機産生。 筆記本電腦網絡攝像頭通常已經提供了這種圖像，而其他相機可能需要中間節點（例如image_proc）進行處理，我購買的是無畸變的外接usb攝像頭。

  （2）/camera/camera_info: sensor_msgs/CameraInfo話題，其中包含/camera/camera_info/K中的相機内參矩陣。可以通過使用例如 camera_calibration 工具包進行相機标定。

  釋出的topic：

  （1）/tf：錄影機坐标與每個檢測到的标簽或标簽簇的坐标之間的相對位姿關系。僅在publish_tf：config / settings.yaml中為true時才釋出/tf。

  （2）/tag_detections：與/tf話題提供的資訊相同，但作為自定義消息，其中包含标簽ID，大小和geometry_msgs/PoseWithCovarianceStamped姿勢資訊。

  （3）/tag_detections_image：與/camera/image_rect輸入的圖像相同，但突出顯示檢測到的标簽。僅在launch/continuous_detection.launch中的publish_tag_detections_image==true時才釋出。

• 現在運作連續标簽檢測器：

roslaunch apriltags2_ros continuous_detection.launch
           

使用rostopic list可以看到程式釋出了/tag_detections相關的topic

**該算法輸出的資訊是相機在二維碼坐标系下的精确位姿與方向。**包括平移矢量position（也就是位置坐标）與旋轉四元數orientation（也就是朝向）。我最終希望得到二維碼相對于我們的移動機器人的位姿，是以需要進行坐标變換。

其中/tag_detections就是apriltags2_ros最後輸出的定位結果，截取其中一幀的結果：

header: 
  seq: 10416
  stamp: 
    secs: 1610698673
    nsecs: 558949073
  frame_id: "usb_cam"
detections: 
  - 
    id: [10]
    size: [0.15]
    pose: 
      header: 
        seq: 10652
        stamp: 
          secs: 1610698673
          nsecs: 558949073
        frame_id: "usb_cam"
      pose: 
        pose: 
          position: 
            x: 0.0408862981401
            y: -0.265477154272
            z: 0.73617112622
          orientation: 
            x: -0.191289591513
            y: 0.827204092907
            z: 0.512393188318
            w: 0.128821199427
        covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

           

還可以在riviz可視化界面上顯示檢測結果，如下圖所示：

參考連結

生成apriltag标簽