3D-LiDAR（Livox：MID-360）を使った3D-MAP作成「FAST-LIOの実装」| Livoxコラム

• 

  ハンディーマップキット

• 

  3D-MAPデータ

システムアーキテクチャ

FAST-LIO（Fast LiDAR-Inertial Odometry）は、計算効率の高い堅牢なLiDAR慣性オドメトリパッケージです。
このパッケージは、LiDARの特徴点とIMUデータを緊密に結合した反復拡張カルマンフィルタを使用して、
高速運動、ノイズの多い環境や混雑した環境での堅牢なナビゲーションを可能にします。

1. オドメトリ最適化のための高速反復カルマンフィルタ
2. 最も安定した環境で自動的に初期化
3. 計算を減らすための並列KDツリー検索

特徴

1. ikd-Treeを使用した増分マッピングにより、より高速で100Hzを超える LiDAR レートを実現
2. Raw LiDAR ポイント（特徴抽出を無効にできます）での直接オドメトリ（スキャンしてマップ）により、精度を向上
3. 特徴抽出の要件がないため、FAST-LIO2 は、スピニング（Velodyne、Ouster）やソリッド ステート（Livox Avia、Horizo​​n、MID-70）LiDAR など多くの種類の LiDAR をサポートでき、簡単に拡張することにより多くの LiDAR をサポート可能
4. 外部 IMU をサポート
5. Khadas VIM3、Nivida TX2、Raspberry Pi 4B（8G RAM）などの ARM ベースのプラットフォームもサポート

セットアップ

$git clone https://github.com/hku-mars/FAST_LIO ~/FAST-LIO/src/FAST-LIO
    $cd ~/FAST_LIO/src/FAST-LIO
    $git submodule update --init
    

$ catkin_make

パラメータの設定

launchファイル（mapping_mid360.launch）

<launch>
        <!-- Launch file for Livox MID360 LiDAR -->
        
            <arg name="rviz" default="true" />
        
            <rosparam command="load" file="$(find fast_lio)/config/mid360.yaml" />
        
            <param name="feature_extract_enable" type="bool" value="0"/>
            <param name="point_filter_num" type="int" value="3"/>
            <param name="max_iteration" type="int" value="3" />
            <param name="filter_size_surf" type="double" value="0.5" />
            <param name="filter_size_map" type="double" value="0.5" />
            <param name="cube_side_length" type="double" value="1000" />
            <param name="runtime_pos_log_enable" type="bool" value="0" />
        
            <node pkg="fast_lio" type="fastlio_mapping" name="laserMapping" output="screen" /> 
        
            <group if="$(arg rviz)">
            <node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find fast_lio)/rviz_cfg/loam_livox.rviz" />
            </group>
        
        </launch>
    

• feature_extract_enableパラメータは、特徴点の抽出を有効にするかどうかを制御するためのパラメータです。
  このパラメータが有効になっている場合、fast-lioはLiDARスキャンから特徴点を抽出し、それらの特徴点を使用して地図を作成および自己位置推定を行います。
  特徴点は、環境の構造や物体の識別に役立ち、SLAMアルゴリズムの精度を向上させるのに役立ちます。
  一方、このパラメータが無効になっている場合、fast-lioは特徴点の抽出を行わず、代わりにLiDARスキャンの生データを使用して地図を作成および自己位置推定を行います。
  この場合、特徴点を使用しないため、処理速度が向上する可能性がありますが、環境の複雑さや精度に影響を与える可能性があります。

configファイル（mid360.yaml）

common:
        lid_topic:  "/livox/lidar"
        imu_topic:  "/livox/imu"
        time_sync_en: false                 # ONLY turn on when external time synchronization is really not possible
        time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README).
                                                         # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, 
please set as 0.0
    
    preprocess:
        lidar_type: 1                # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 
        scan_line: 4
        blind: 0.2
    
    mapping:
        acc_cov: 0.1
        gyr_cov: 0.1
        b_acc_cov: 0.0001
        b_gyr_cov: 0.0001
        fov_degree:    360
        det_range:      50.0
        extrinsic_est_en:  false      # true: enable the online estimation of IMU-LiDAR extrinsic
        extrinsic_T: [ -0.011, -0.02329, 0.04412 ]
        extrinsic_R: [ 1, 0, 0,
                    　        0, 1, 0,
                              0, 0, 1]
    
    publish:
        path_en:  true
        scan_publish_en:  true       # false: close all the point cloud output
        dense_publish_en: true       # false: low down the points number in a global-frame point clouds scan.
        scan_bodyframe_pub_en: true  # true: output the point cloud scans in IMU-body-frame
    
    pcd_save:
        pcd_save_en: true
        interval: -1           # how many LiDAR frames saved in each pcd file; 
                                     # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.

主なパラメータ

• lid_topic：LiDAR点群のトピック名
• imu_topic：IMUのトピック名
• time_sync_en：外部IMUとの時刻同期（LiDAR内のIMUを使う場合はfalse）
• lidar_type：LiDARの種類　1=Livox、2=Velodyne、3=Ouster　但し、Livoxを選択した場合、livox customized pointcloud dataを使用
• scan_line：光源の数　MID-360の場合は4本
• blind：近距離のブラインド　今回は0.2mに設定
• fov_degree：水平方向のFOV
• det_range：検出距離　今回は50mに設定
• extrinsic_est_en：外部IMUの場合、LiDARの原点とIMUの原点を調整可否（LiDAR内のIMUを使う場合はfalse）
• path_en：移動軌跡の表示可否
• scan_publish_en：点群表示の可否
• dense_publish_en：global-frameのポイントクラウドスキャンのポイント数を減らす
• scan_bodyframe_pub_en：IMU-body-frameでポイントクラウドスキャンを出力
• pcd_save_en：pcd保存可否

実行方法

$ roslaunch livox_ros_driver2 msg_MID360.launch
    $ roslaunch fast_lio mapping_mid360.launch

• 

  FAST-LIO　地図作成

実行結果

~/FAST_LIO/src/FAST-LIO/PCD以下にscans.pcdというファイル名で全体のpcdファイルが保存されるので、作成したPCDファイルをCloudCompareを使って編集します。
点群ファイルの編集を参照ください。

• 

  CloudCompare編集後（全体図：上から見たデータ）

• 

  CloudCompare編集後（天井を切り取った全体図：上から見たデータ）

参考

• FAST-LIO
• CloudCompare
• 3D-LiDAR（Livox：MID-360）を使った3D-MAP作成 | LIVOXコラム

免責事項

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

お問い合わせ