3D LiDARとカメラのセンサフュージョン #OpenCV

3D LiDARとカメラ

LiDARやカメラは自動運転において周辺環境の認識や、自己位置推定にはかかせないセンサです。
お互い優れた能力を持っていますが、以下の弱みがあります。

3D LiDAR：高精度に位置情報を算出することはできるが、データのスパースさにより検出した物体が何であるかを判断するのは困難な場合が多い。
カメラ：深層学習等を用いたObject Detectionは非常に優れているが、位置情報を安定してリアルタイムに算出することは難しい。

この２つのセンサのデータを紐付けることができれば、互いの弱みをカバーできるようになります。この記事では実装方法についてまとめました。

環境

以下のセットアップは完了済みとします。

ros(kinetic)
OpenCV3
PCL1.8

セットアップ

Download Velodyne package

Velodyneを使用するためのパッケージをダウンロードしてコンパイルします。

sudo apt-get install libpcap-dev
cd ~/catkin_ws/src/
git clone https://github.com/ros-drivers/velodyne
cd ~/catkin_ws
catkin_make

Download Sensor Fusion package

git-hubに公開してあるパッケージをダウンロードしてきてコンパイルをして下さい。

cd ~/catkin_ws/src/
git clone https://github.com/Sadaku1993/velodyne_camera_calibration
roscd
catkin_make

Bagデータをダウンロード

明治大学ロボット工学研究室が公開しているBagデータを利用します。
data.bagをダウンロードして下さい。
ダウンロードしてきたBagデータはbagfilesの階層に移動して下さい。

以下のような配置になっているか確認をして下さい。

velodyne_camera_calibration/
  bagfiles/
    data.bag
  CMakeLists.txt
  src/
  config/
  include/
  package.xml
  launch/

利用するTopic Dataは以下の３つです。

/camera/color/camera_info (sensor_msgs/CameraInfo)
/camera/color/image_raw/compressed (sensor_msgs/Image/Compressed)
/velodyne_packets (velodyne_msgs/VelodyneScan)

rosbag infoで確認してみましょう。

velodyne_camera_calibration/bagfiles/

$ rosbag info data.bag
path:        data.bag
version:     2.0
duration:    1:16s (76s)
start:       Oct 31 2018 17:37:11.07 (1540975031.07)
end:         Oct 31 2018 17:38:27.76 (1540975107.76)
size:        273.5 MB
messages:    17592
compression: none [342/342 chunks]
types:     nav_msgs/Odometry           [cd5e73d190d741a2f92e81eda573aca7]
           sensor_msgs/CameraInfo      [c9a58c1b0b154e0e6da7578cb991d214]
           sensor_msgs/CompressedImage [8f7a12909da2c9d3332d540a0977563f]
           sensor_msgs/Imu             [6a62c6daae103f4ff57a132d6f95cec2]
           velodyne_msgs/VelodyneScan  [50804fc9533a0e579e6322c04ae70566]
topics:    /camera/color/camera_info            2283 msgs    : sensor_msgs/CameraInfo     
           /camera/color/image_raw/compressed   2283 msgs    : sensor_msgs/CompressedImage
           /imu/data                            7670 msgs    : sensor_msgs/Imu            
           /odom                                3832 msgs    : nav_msgs/Odometry          
           /velodyne_packets                    1524 msgs    : velodyne_msgs/VelodyneScan

OdometryとImuもBagデータ内にありますが、今回は利用しません。

色付き点群

VelodyneのPointCloudにカメラの色情報を付与します。
PointCloudに対して投影変換を行い、カメラ画像上で対応するピクセルのRGB値を取得してきています。
見づらくてすいません… 時間ができ次第差し替えます。

深度画像

カメラ画像にVelodyneの点群を付与します。
点群に対して投影変換を行い、カメラの画角内に属する点群の場合、対応するピクセルの色を距離情報に基づいて色を変更しています。

実行

１つlaunchファイルを起動すれば全て動作するようになっています。

roslaunch velodyne_camera_calibration sensor_fusion.launch

うまくいけば以下のようなrviz画面が開かれると思います。

bagデータが再生されない場合、bagデータの配置位置を再度確認して下さい。

sensor_fusion.launch

<?xml version="1.0"?>
<launch>
    <!--tf-->
    <node pkg="tf" type="static_transform_publisher" name="velodyne2realsense" 
          args="0.111393 -0.0103084 -0.0137764 0.0 0.0 0.0 velodyne camera_link 100" />
    <node pkg="tf" type="static_transform_publisher" name="camera_link2camera_color_frame" 
          args="0.000 0.015 0.000 0.001 -0.001 -0.017 camera_link camera_color_frame 100"/>
    <node pkg="tf" type="static_transform_publisher" name="camera_color_frame2camera_color_optical_frame" 
          args="0.000 0.000 0.000 -1.571 0.000 -1.571 camera_color_frame camera_color_optical_frame 100"/>

    <!--velodyne-->
    <include file="$(find velodyne_pointcloud)/launch/32e_points.launch" />

    <!--republish-->
    <node name="republish" type="republish" pkg="image_transport" output="screen"
          args="compressed in:=/camera/color/image_raw raw out:=/camera/color/image_raw" />

    <!--sensor fusion-->
    <node pkg="velodyne_camera_calibration" type="sensor_fusion" name="sensor_fusion">
        <remap from="/image" to="/camera/color/image_raw" />
        <remap from="/cinfo" to="/camera/color/camera_info" />
        <remap from="/lidar" to="/velodyne_points" />
        <remap from="/colored_cloud" to="/velodyne_points/colored" />
        <remap from="/projection" to="/camera/color/projection" />
    </node>

    <!--rviz-->
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find velodyne_camera_calibration)/config/sensor_fusion.rviz"/>

    <!--rosbag play-->
    <node pkg="rosbag" type="play" name="play" args="$(find velodyne_camera_calibration)/bagfiles/data.bag" />
</launch>

まとめ

VelodyneとRealSenseのセンサフュージョンにより、色付き点群と距離画像を作成することができます。より高密度な3D-LiDARを利用した場合、次のようにとても密な色付き点群を作ることができます。
最後まで読んでいただきありがとうございました。

注意点

VelodyneやRealsense以外のセンサでも利用可能ですが、うまく動作しない場合以下の原因が考えられます。

時間同期
message_filtersを利用しているため、センサ間のフレームレートが異なってしまうと動作しません。
厳密な時間同期が必要ない場合、message_filtersを利用しないようにプログラムを書き換えてください。
座標変換
tfが正しくはれていない場合、変な位置に色付き点群が出てきたり、色がつきません。
今回のプログラムはRealSense用に作成されているので、センサにより座標変換が異なる可能性が高いです。
投影変換用の内部パラメータがない
LiDARの点群をRGB画像上に投影変換するのにsensor_msgs::CameraInfoを利用しています。
投影変換のために必要なパラメータが含まれているか確認してみて下さい。