Capture Laser Data retrieve from Velodyne LiDAR

VelodyneCapture

Velodyne LiDARからデータを取得するVelodyneCaptureクラスを書きました。
VelodyneCaptureはヘッダファイルをインクルードするだけで、簡単に利用することができます。

VelodyneCaptureは以下で公開しています。

What is Data?

LiDARが360°回転して測定した各レーザーのデータを1フレームとして取得できます。
各レーザーのデータは以下の情報を持っています。

  • azimuth (回転角度)
  • vertical (レーザー角度)
  • distance (距離)
  • intensity (反射強度)
  • id (レーザーID)

What is Dependent Libraries?

直接センサーからリアルタイムでキャプチャするだけでなく、PCAPファイルからオフラインでキャプチャすることもできます。
それぞれのキャプチャ方法によって以下のライブラリに依存します。*1

*1 HAVE_BOOSTHAVE_PCAPをプリプロセッサに定義してください。

  • Direct Capture from Sensor
  • Boost.Asio

  • Offline Capture from PCAP
  • libpcap ( or WinPCAP )

Basic usage for VelodyneCapture

VelodyneCaptureの基本的な利用方法を紹介します。

Include Header

VelodyneCaptureを使うために、VelodyneCapture.hをインクルードします。

#include "VelodyneCapture.h"

Generate Instance

キャプチャクラスのインスタンスを生成します。
Velodyneセンサーごとにキャプチャクラスが用意されています。

  • velodyne::VLP16Capture
  • Capture Class for VLP-16

  • velodyne::HDL32ECapture
  • Capture Class for HDL-32E

// Capture Class for VLP-16
velodyne::VLP16Capture capture;

// Capture Class for HDL-32E
velodyne::HDL32ECapture capture;

Open Capture

IPアドレスとポート番号を与えるとセンサーからリアルタイムでキャプチャします。
PCAPファイルのパスを与えるとファイルからオフラインでキャプチャします。
正常にキャプチャを開始できたかどうかはisOpen()で確認することができます。

// Open Direct Capture retrieve from Sensor
const boost::asio::ip::address address = boost::asio::ip::address::from_string( "192.168.1.21" );
const unsigned short port = 2368;
capture.open( address, port );
if( !capture.isOpen() ){
    std::cerr << "Can't open VelodyneCapture." << std::endl;
    return -1;
}

// Open Offline Capture retrieve from PCAP
const std::string filename = "../file.pcap";
capture.open( filename );
if( !capture.isOpen() ){
    std::cerr << "Can't open VelodyneCapture." << std::endl;
    return -1;
}

Retrieve One Rotation Data

LiDARが360°回転して測定した1フレーム分のデータを取得します。
データはvelodyne::Laser構造体の配列として取得できます。
ここでは>>オペレータを使っていますが、retrieve()を使うこともできます。

取得したデータは角度やレーザーIDがソートされていません。
必要に応じてstd::sort()を使ってソートすることができます。

while( capture.isRun() ){
    // Capture One Rotation Laser Data
    std::vector<velodyne::Laser> lasers;
    capture >> lasers;
    if( lasers.empty() ){
        continue;
    }

    /*
    // Sort Laser Data ( 0 degree -> 359 degree, 0 id -> n id )
    std::sort( lasers.begin(), lasers.end() );
    */

    // Access to Laser Data
    for( const velodyne::Laser& laser : lasers ){
        // Laser Azimuth ( degree )
        const double azimuth = laser.azimuth;

        // Laser Vertical ( degree )
        const double vertical = laser.vertical;

        // Laser Distance ( centimeter )
        const unsigned short distance = laser.distance;

        // Laser Intensity
        const unsigned char intensity = laser.intensity;

        // Laser ID ( VLP-16 : 0 - 15, HDL-32E : 0 - 31 )
        const unsigned char id = laser.id;
    }
}

Draw Velodyne LiDAR Data using OpenCV Viz module

Velodyne LiDARから取得したデータをOpenCVのVizモジュールを使って描画するサンプルプログラムを紹介します。
このサンプルプログラムは以下で公開しています。

Convert to 3-dimention Coordinates

VelodyneCaptureで取得したデータをもとに3次元座標にプロットします。
3次元座標にプロットするには、距離と垂直角度、水平角度から以下のように計算します。
三角関数の仕様に合わせて、垂直角度と水平角度はあらかじめ弧度法(ラジアン)に変換しています。

// Convert to 3-dimention Coordinates
std::vector<cv::Vec3f> buffer( lasers.size() );
for( const velodyne::Laser& laser : lasers ){
    const double distance = static_cast<double>( laser.distance );
    const double azimuth  = laser.azimuth  * CV_PI / 180.0;
    const double vertical = laser.vertical * CV_PI / 180.0;

    float x = static_cast<float>( ( distance * std::cos( vertical ) ) * std::sin( azimuth ) );
    float y = static_cast<float>( ( distance * std::cos( vertical ) ) * std::cos( azimuth ) );
    float z = static_cast<float>( ( distance * std::sin( vertical ) ) );

    if( x == 0.0f && y == 0.0f && z == 0.0f ){
        x = std::numeric_limits<float>::quiet_NaN();
        y = std::numeric_limits<float>::quiet_NaN();
        z = std::numeric_limits<float>::quiet_NaN();
    }

    buffer.push_back( cv::Vec3f( x, y, z ) );
}

Execution Result

このサンプルプログラムを実行するとVelodyne LiDARから取得したデータがリアルタイムに表示されます。
これは車上にセンサーを搭載して走行しているサンプルデータ(HDL32-V2_Monterey Highway.pcap)です。


コメントを残す

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