Point Cloud Library with Velodyne LiDAR

Introduction

中部大学 藤吉研究室にお邪魔してVelodyne VLP-16を触らせていただきました。

Velodyne LiDAR

What is LiDAR?

LiDARは、レーザー測距ユニットを搭載したヘッドを回転させ360°全周の3次元距離と反射率を取得できるセンサーです。
Velodyne社のLiDARは、ロボットカー(自動運転車)やドローンなどに搭載され、周辺環境をセンシングするために活用されています。


VLP-16

VLP-16は、垂直方向+/-15°の範囲に16本のレーザーを飛ばしてデータを取得します。
水平方向には300~1200rpmの速度で回転して0.1~0.4°の分解能でデータを取得します。
大型のHDL-64E/HDL-32Eになるとレーザーの本数が多く垂直方向の分解能が高くなります。

vlp16
vlp16_laser

LiDARからはUDP/IPでデータが送られてきます。
PCで受信したパケットを解析して各種データを取得します。

VeloView

VeloViewはVelodyne LiDARのビューワーソフトです。
LiDARから取得したデータをリアルタイムで表示したり、データを記録してファイルに保存(PCAP、CSV)したり再生したりすることができます。
PCAPファイルには、受信したパケットデータが全て記録されています。
CSVファイルには、1周を1ファイル(1フレーム)として0°~359.9°の反射強度や距離などのデータが記録されています。

PCL with Velodyne LiDAR

PCLにはVelodyne LiDARからデータを入力するためのGrabberが用意されています。
他のGrabberと同様にセンサーからPoint Cloudを簡単に取得することができます。

また、コンストラクタに指定する引数によってセンサーからリアルタイムにデータを取得するかファイルからデータを取得するかを選択することができます。
ただし、ファイルからデータを取得をするためには、WITH_PCAPオプションを有効にしてPCLをビルドする必要があります。

  • HDLGrabber
  • HDL-64E/HDL-32EのためのGrabber。

  • VLPGrabber
  • VLP-16のためのGrabber。

Drawing Point Cloud retrieve from Velodyne LiDAR

サンプルプログラムは以下で公開しています。

Include

pcl::VLPGrabberを使うために、pcl/io/vlp_grabber.hをインクルードします。
pcl::HDLGrabberを使う場合は、pcl/io/hdl_grabber.hをインクルードします。

main.cpp#L7

#include <pcl/io/vlp_grabber.h>
//#include <pcl/io/hdl_grabber.h>

Grabber

pcl::VLPGrabberのインスタンスを生成します。
このとき、コンストラクタに指定する引数によってセンサーからリアルタイムにデータを取得するかファイルからデータを取得するか設定されます。
コンストラクタにPCAPファイルのパスを指定すると、PCAPファイルからデータを取得します。
コンストラクタにIPアドレスとポート番号を指定すると、PCAPファイルからデータを取得します。


*1 pcl::HDLGrabberとHDL-64を使う場合は、キャリブレーションファイルを指定する必要があります。
*2 指定するIPアドレスとポート番号はVelodyne LiDARとNICの設定により異なります。

main.cpp#L82-L90

// VLP Grabber
boost::shared_ptr<pcl::VLPGrabber> grabber;
if( !pcap.empty() ){
    std::cout << "Capture from PCAP..." << std::endl;
    grabber = boost::shared_ptr<pcl::VLPGrabber>( new pcl::VLPGrabber( pcap ) );
}
else if( !ipaddress.empty() && !port.empty() ){
    std::cout << "Capture from Sensor..." << std::endl;
    grabber = boost::shared_ptr<pcl::VLPGrabber>( new pcl::VLPGrabber( boost::asio::ip::address::from_string( ipaddress ), boost::lexical_cast<unsigned short>( port ) ) );
}

Color Handler

Velodyne LiDARから得られるデータは色情報を持ちません。
そのため、ビューワーに表示するためのルールを指定する必要があります。
ここでは、pcl::PointTypeごとにpcl::visualization::PointCloudColorHandlerを作成します。
作成したpcl::visualization::PointCloudColorHandlerは、ビューワーに指定します。

main.cpp#L51-L68

// Point Cloud Color Hndler
pcl::visualization::PointCloudColorHandler<PointType>::Ptr handler;
const std::type_info& type = typeid( PointType );
if( type == typeid( pcl::PointXYZ ) ){
    std::vector<double> color = { 255.0, 255.0, 255.0 };
    boost::shared_ptr<pcl::visualization::PointCloudColorHandlerCustom<PointType>> color_handler( new pcl::visualization::PointCloudColorHandlerCustom<PointType>( color[0], color[1], color[2] ) );
    handler = color_handler;
}
else if( type == typeid( pcl::PointXYZI ) ){
    boost::shared_ptr<pcl::visualization::PointCloudColorHandlerGenericField<PointType>> color_handler( new pcl::visualization::PointCloudColorHandlerGenericField<PointType>( "intensity" ) );
    handler = color_handler;
}
else if( type == typeid( pcl::PointXYZRGBA ) ){
    boost::shared_ptr<pcl::visualization::PointCloudColorHandlerRGBField<PointType>> color_handler( new pcl::visualization::PointCloudColorHandlerRGBField<PointType>() );
    handler = color_handler;
}
else{
    throw std::runtime_error( "This PointType is unsupported." );
}

main.cpp#L98-L110

while( !viewer->wasStopped() ){
    // Update Viewer
    viewer->spinOnce();

    boost::mutex::scoped_try_lock lock( mutex );
    if( lock.owns_lock() && cloud ){
        // Update Point Cloud
        handler->setInputCloud( cloud );
        if( !viewer->updatePointCloud( cloud, *handler, "cloud" ) ){
            viewer->addPointCloud( cloud, *handler, "cloud" );
        }
    }
}

Execution Result

このサンプルプログラムを実行するとVelodyne LiDARから取得したデータをリアルタイムに表示されます。
これは車上にセンサーを搭載して走行しているサンプルデータ(2014-11-10-11-32-17_Velodyne-VLP_10Hz_Monterey Highway.pcap)です。
赤外線レーザーの反射強度(intensity)で色を付けています。
街路樹や建物、すれ違う自動車など道路の周辺環境がわかると思います。


コメントを残す

メールアドレスが公開されることはありません。 * が付いている欄は必須項目です