3D-LiDAR（Livox：HAP）とカメラのセンサーフュージョン

安価で高機能な Livox 社製 HAP と USB カメラを使って、センサーフュージョンを実現しました。

・LiDAR の点群にカメラから入力された色情報を付加
・カメラ画像に点群から求めた距離情報を付加

概要
解説
実行結果
参考

概要

ハードウェア構成

Livox　3D-LiDAR	HAP-TX（詳細）
NVIDIA	Jetson AGX-Xavier
BUFFALO　USBカメラ	BSW500MBK

環境

ROS（Melodic）

前提条件

カメラのキャリブレーションは終了済

セットアップ

・Livox_SDK2 をインストール＆セットアップ(github )
・Livox_ros_driver2 をインストール＆セットアップ(github )
・github に公開されている sensor_fusion のパッケージをダウンロードし、コンパイル

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

launchファイル（sensor_fusion.launch）の変更

github よりダウンロードした launch ファイルは Velodyne 用のため、Livox用へ変更する必要がある

<?xml version="1.0"?>
<!--Sensor Fusion-->
<launch>
    <arg name="rviz_enable" default="true"/>

    <!--tf-->
    <node pkg="tf" type="static_transform_publisher" name="livox2camera_link"
            args="-0.15 0 0.25 -1.571 0.0 -1.571 livox_frame head_camera 100" />

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

    <!--USB camera-->
    <node name="head_camera" pkg="usb_cam" type="usb_cam_node">
        <param name="camera_info_url" value="file://$(find livox_camera_calibration)/config/camera.yaml"/>
        <param name="pixel_format" value="yuyv" />
        <param name="image_width" value="640" />
        <param name="image_hight" value="480" />
    </node>

    <!--image_proc-->
    <node name="image_proc" pkg="image_proc" type="image_proc" output="screen">
        <remap from="/camera_info" to="/head_camera/camera_info" />
        <remap from="/image_raw" to="/head_camera/image_raw" />
        <remap from="/image_rect_color" to="/head_camera/image_rect_color" />
    </node>

    <!--sensor fusion-->
    <node pkg="livox_camera_calibration" type="sensor_fusion" name="sensor_fusion">
        <remap from="/image" to="/head_camera/image_rect_color" />
        <remap from="/cinfo" to="/head_camera/camera_info" />
        <remap from="/lidar" to="/livox/lidar" />
        <remap from="/colored_cloud" to="/livox_points/colored" />
        <remap from="/projection" to="/head_camera/projection" />
    </node>

    <!--rviz-->
    <group if="$(arg rviz_enable)">
        <node pkg="rviz" type="rviz" name="rviz" args="-d $(find livox_camera_calibration)/config/sensor_fusion.rviz"/>
    </group>

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

解説

TF による LiDAR とカメラの位置を指定する

LiDAR frame：livox_frame
camera frame：head_camera
LiDAR とカメラの距離：X/Y/Z＝-0.15, 0, 0.25

　LiDAR とカメラの座標軸変換：yaw/pitch/roll＝π/2, 0 , π/2 = -1.571,0,-1.571

<node pkg="tf" type="static_transform_publisher" name="livox2camera_link"
args="-0.15 0 0.25 -1.571 0.0 -1.571 livox_frame head_camera 100" />

livox_ros_drvier の起動

   <include file="$(find livox_ros_driver2)/launch_ROS1/rviz_HAP.launch" />

USB カメラの起動

<!--USB camera-->
    <node name="head_camera" pkg="usb_cam" type="usb_cam_node">
        <param name="camera_info_url" value="file://$(find livox_camera_calibration)/config/camera.yaml"/>
        <param name="pixel_format" value="yuyv" />
        <param name="image_width" value="640" />
        <param name="image_hight" value="480" />
    </node>

image_proc の起動

読み込んだキャリブレーションデータを元の画像を補正
カメラからの生データ（/head_camera/image_raw）とキャリブレーションデータ（camera.yaml）、camera_iinfo をimage_proc に入力し、補正後のカラーデータ（/head_camera/image_rect_color）を取得する。

    <!--image_proc-->
    <node name="image_proc" pkg="image_proc" type="image_proc" output="screen">
        <remap from="/camera_info" to="/head_camera/camera_info" />
        <remap from="/image_raw" to="/head_camera/image_raw" />
        <remap from="/image_rect_color" to="/head_camera/image_rect_color" />
    </node>

センサーフュージョン

補正後のカメラデータ（/head_camera/image_rect_color）、camera_info（head_camera/camera_info）とLiDAR からの点群データ（/livox/lidar）を入力し、色付き点群（/livox_points/colored）と距離情報付きカメラ画像（/head_camera/projection）を出力する

       <!--sensor fusion-->
    <node pkg="livox_camera_calibration" type="sensor_fusion" name="sensor_fusion">
        <remap from="/image" to="/head_camera/image_rect_color" />
        <remap from="/cinfo" to="/head_camera/camera_info" />
        <remap from="/lidar" to="/livox/lidar" />
        <remap from="/colored_cloud" to="/livox_points/colored" />
        <remap from="/projection" to="/head_camera/projection" />
    </node>

実行結果

以下の様に、rviz上にカメラから取得した色が付いた点群と、距離によって色付けされたカメラ画像が出力されます。

表示するTopic
　PointCloud　Topic：/livox_points/colored
　Imahe Topic：/head_camera/projection

もちろん、ros_driver を MID-360 用に変更することにより、MID-360 ＋ Camera のフュージョンも実行できます。

参考

免責事項

本ページでご紹介したシステムは、あくまでもデモ用として確認しています。従いまして、本ページ内容の活用による不具合などの責任は負いかねます。
また、商用利用する場合は、ソースコード作成元へ確認ください。

お問い合わせ

3D-LiDAR（Livox：HAP）とカメラのセンサーフュージョン | LIVOXコラム

3D-LiDAR（Livox:HAP）とカメラのセンサーフュージョン

概要