Self-location estimation and movement trajectory acquisition using 3D-LiDAR (Livox: MID-360)

Self-location estimation and movement trajectory characteristics using 3D-LiDAR (Livox: MID-360)

• Hardware Configuration

• Livox 3D-LiDAR: MID-360

• Intel NUC: NUC11PAHi3 or Notebook PC

• environment

• ROS (Noetic)

set up

• Install and set up Livox_SDK2.

• Livox_SDK2 (GitHub)

• Install and set up Livox_ros_driver2.

• Livox_ros_driver2 (GitHub)

• Install pip.

$ sudo apt intall python3-pip

• Install numpy.

$ sudo apt intall ros-noetic-ros-numpy

• Install open3d.

$ pip install open3d

• After installing open3d, run the test with the following command and if there are no errors, it's OK.

$ python3 -c "import open3d"

• Download the FAST-LIO-LOCALIZATION package published on github.

$git clone https://github.com/HViktorTsoi/FAST_LIO_LOCALIZATION.git
$cd FAST_LIO_LOCALIZATION
$git submodule update --init

If you run catkin_make as is, an error will occur, so fix the following.

• Modify the file for livox_ros_driver2

• Corrected file

FAST-LIO/CMakeLists.txt, LIO-Livox/package.xml
FAST-LIO/src/laserMapping.cpp
FAST-LIO/src/preprocess.cpp
FAST-LIO/src/preprocess.h

• Correction details

livox_ros_driver => livox_ros_driver2

• Compilation error countermeasures

• Corrected file

FAST_LIO_LOCALIZATION/include/common_lib.h

• Correction details

fast_lio/Pose6D.h => fast_lio_localization/Pose6D.h

• Running the compilation

$ catkin_make

• Change the Python files under the script directory to use python3.

• Installing the conversion tool

$ sudo apt-get install 2to3

• Convert files under scripts

$ 2to3 -w ./scripts

• Change the first line of every file

#!/usr/bin/env python2 => #!/usr/bin/env python3

• scripts/global_localization.py

Change o3d.registration => o3d.pipelines.registration.

• Lower the numpy version: 1.20.3

$ pip uninstall -y numpy
$ pip install numpy=1.20.3

Setting parameters

• Launch file (localization_mid360.launch)

The map will not be displayed as it is, so we will change the contents of localization_mid360.launch.
frame_id:=/map => frame_id:=map

In the fast_lio_localization package, you specify the file name of the map data you have previously acquired.
<arg name="map" default="1(find fast_lio_localization)/pcd/scans.pcd" />

• If necessary, downsample the map data (pcd file) using CloudCompare or similar.

• In addition, various parameters are the same as FAST-LIO.

<launch>
<!-- Launch file for Livox MID-360 LiDAR -->

    <arg name="rviz" default="true" />

    <rosparam command="load" file="$(find fast_lio_localization)/config/mid360.yaml" />

    <param name="feature_extract_enable" type="bool" value="0"/>
    <param name="point_filter_num" type="int" value="10"/>
    <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" />
    <param name="pcd_save_enable" type="bool" value="0" />

    <node pkg="fast_lio_localization" type="fastlio_mapping" name="laserMapping" output="screen" />
    <arg name="map" default="$(find fast_lio_localization)/pcd/scans.pcd" />

    <!-- loalization-->
    <node pkg="fast_lio_localization" type="global_localization.py" name="global_localization" output="screen">
    </node>

    <!-- transform  fusion-->
    <node pkg="fast_lio_localization" type="transform_fusion.py" name="transform_fusion" output="screen" />

    <!-- glbal map-->
    <node pkg="pcl_ros" type="pcd_to_pointcloud" name="map_publishe" output="screen"
            args="$(arg map) 5 _frame_id:=map cloud_pcd:=/map" />

    <group if="$(arg rviz)">
    <node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find fast_lio_localization)/rviz_cfg/localization.rviz" />
    </group>

</launch>

• config file (mid360.yaml)

• Regarding the config file, the various parameters are the same as those for FAST-LIO.

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:  true      # 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:
  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

• Edit global_localization.py

Edit the following parameters in the scripts/global_localization.py file as needed:

# only those scan2map-matching with higher fitness than LOCALIZATION_TH will be taken
　　　　LOCALIZATION_TH = 0.98
# FOV(rad), modify this according to your LiDAR type
　　　　FOV = 6.3
# The farthest distance(meters) within FOV
　　　　FOV_FAR = 50

LOCALIZATION_TH: Threshold for determining the correct position by what percentage of the map point cloud and the sensor point cloud must match.
FOV: Horizontal FOV (radians)
FOV_FAR: Maximum distance (m)

How to run

• Terminal-1

$ roslaunch fast_lio_localization localization_mid360.launch

• Terminal-2

$ roslaunch livox_ros_driver2 msg_MID360.launch

FAST-LIO-LOCALIZATION Self-location estimation & movement trajectory
*This video plays rosbag.

If you want to use rosbag, follow the steps below:

• After running fast_lio_localization, rviz will start up and after a while the loaded map data (scan.pcd) will be displayed.
• After the map is displayed, play rosbag and pause it once the point cloud from the LiDAR is displayed (plays for about 1-2 seconds).
• Use "2D Pose Estimate" on rviz to specify the initial position and direction.
• Once the initial position has been successfully specified, resume playback of the rosbag.

Reference

• FAST-LIO-LOCALIZATION
• CloudCompare
• Creating a 3D map using 3D LiDAR (Livox: MID-360) "Implementation of FAST-LIO" | Livox Column

Disclaimer

This system has been tested for demonstration purposes only. Therefore, we cannot take any responsibility for any malfunctions. If you intend to use it for commercial purposes, please contact the source code creator.