Perception Algorithm Development Example

This example uses FAST-LIVO2 (Fast, Direct LiDAR-Inertial-Visual Odometry) to implement LiDAR-Inertial-Visual fused SLAM on the Lite3 robot. Developers can obtain real-time 3D point cloud maps, visual feature submaps, odometry estimates, and camera RGB images through this system.

Code repository: DeepRoboticsLab/fast-livo2-deep-robotics

Use the hardware expansion and FAST-LIVO2 video or Bilibili with this example. Its unsynchronized sensors cause a reported failure; choose hardware synchronization or soft-sync using the current repository guide. See video tutorials for context.

Functional Positioning

The perception system sits in the high-level perception layer of the Lite3 development ecosystem, with responsibilities including:

• Acquiring 3D point cloud data via Livox Mid-360/Mid-360s LiDAR
• Acquiring high-frequency inertial measurement data via the LiDAR’s built-in IMU
• Acquiring RGB image data via Intel RealSense D435i
• Running FAST-LIVO2 for LiDAR-Inertial-Visual fused odometry estimation
• Outputting real-time 3D point cloud maps, odometry poses, and visual feature submaps

Upper-layer applications (such as navigation, mapping, path planning) obtain perception results by subscribing to ROS2 topics.

System Environment

Item	Requirement
OS	Ubuntu 22.04
ROS2 Version	ROS2 Humble
Robot Model	Lite3 Venture (hardware extension required only for this model)
Onboard Compute	AGX Jetson Orin (Jetpack 6.1, CUDA 12.6)
LiDAR	Livox Mid-360 or Mid-360s
Camera	Intel RealSense D435i

Hardware Installation

AGX Onboard Compute Installation

Lite3 Venture does not come with a high-performance compute unit; an AGX Jetson Orin must be installed separately as the onboard computer.

Assembling the AGX Kit with Adapter Plate

Required parts:

• AGX kit
• Adapter plate bracket
• Four M2.5 × 55 half-thread hex socket head cap screws

Place the AGX kit on the adapter plate bracket, aligning the four mounting holes. Insert the four M2.5 × 55 screws from the bottom of the adapter plate and tighten evenly to ensure the AGX kit sits flat.

Mounting the Component to the Lite3 Top Cover

Required parts:

• Assembled AGX + adapter plate component
• Lite3 top cover
• Three M3 × 6 hex socket head cap screws

Place the component at the designated position on the top cover, align the three mounting points, install the M3 × 6 screws, and tighten evenly.

LiDAR Connection

This repository supports two Livox LiDAR variants:

• Mid-360 (original): Use MID360 launch file and MID360_config.json
• Mid-360s (newer): Use MID360s launch file and MID360s_config.json

Connection steps:

1. Insert the XT30 end of the cable into the Lite3’s 24V XT30 port
2. Connect the Ethernet end to the AGX’s Ethernet port
3. Open Livox Viewer 2 on the host to confirm the LiDAR is connected

Camera Connection

The Intel RealSense D435i connects to the AGX via USB 3.0. Ensure you use the original USB cable for optimal bandwidth.

Software Compilation and Installation

Install Livox SDK and Driver

Compile Livox-SDK2

cd fast-livo2-deep-robotics/src/Livox-SDK2
mkdir build && cd build
cmake .. && make -j
sudo make install

Compile livox_ros_driver2

cd fast-livo2-deep-robotics/src/livox_ros_driver2/
source /opt/ros/humble/setup.bash
./build.sh humble

Install librealsense and realsense-ros

Install librealsense

Follow the librealsense official documentation to install on the AGX.

Common Issue — Library Version Conflict:

If rs-enumerate-devices reports bad optional access, there are two versions of librealsense on the system:

Path	Version	Source	Status
/usr/local/lib	librealsense2.so.2.56.5	Self-compiled (RSUSB support)	✅
/opt/ros/humble/lib	librealsense2.so.2.56.4	ROS2 auto-installed (older)	❌

Solution:

echo "/usr/local/lib" | sudo tee /etc/ld.so.conf.d/99-realsense-local.conf
sudo ldconfig
echo 'export LD_LIBRARY_PATH=/usr/local/lib:$LD_LIBRARY_PATH' >> ~/.bashrc
source ~/.bashrc

Verify:

ldd $(which rs-enumerate-devices) | grep realsense

The result should point to /usr/local/lib/librealsense2.so.2.56.5.

Install realsense-ros

sudo apt -y install ros-humble-realsense2-*

Verify:

ros2 launch realsense2_camera rs_pointcloud_launch.py

Compile FAST-LIVO2

Install ROS2 dependencies:

sudo apt update
sudo apt -y install ros-humble-pcl-ros ros-humble-compressed-image-transport ros-humble-sophus

Compile:

cd fast-livo2-deep-robotics
source /opt/ros/humble/setup.bash
colcon build --packages-select fast_livo --symlink-install
source install/setup.bash

Verify package:

ros2 pkg list | grep livo

Expected output should include fast_livo and livox_ros_driver2.

Sensor Configuration

Configure LiDAR Network

The Ethernet adapter needs a static IPv4 address on the same subnet as the LiDAR.

# Find Ethernet interface
ip -br address
nmcli -t -f DEVICE,TYPE,STATE,CONNECTION device status

# Configure static IP (e.g., 192.168.1.45; LiDAR IP is 192.168.1.201)
sudo nmcli connection modify "Wired connection 2" \
  ipv4.method manual \
  ipv4.addresses 192.168.1.45/24 \
  ipv4.gateway "" \
  ipv4.dns "" \
  ipv4.never-default yes
sudo nmcli connection down "Wired connection 2"
sudo nmcli connection up "Wired connection 2"

ipv4.never-default yes prevents the LiDAR connection from replacing the WiFi default route.

Verify:

ip route get 192.168.1.201
ping -c 3 192.168.1.201

Configure IP addresses in the driver config files:

• Mid-360: src/livox_ros_driver2/config/MID360_config.json
• Mid-360s: src/livox_ros_driver2/config/MID360s_config.json

Key fields:

"host_ip": "192.168.1.45",
"ip": "192.168.1.201"

Configure Camera

FAST-LIVO2 requires the image dimensions to exactly match the camera model. Using D435i RGB stream 640×480×30 as an example:

Set Image Topic

Modify src/FAST-LIVO2/config/avia.yaml:

common:
  img_topic: "/camera/camera/color/image_raw"
  lid_topic: "/livox/lidar"
  imu_topic: "/livox/imu"

Set Camera Intrinsics

Modify src/FAST-LIVO2/config/camera_pinhole.yaml:

camera:
  model: Pinhole
  width: 640
  height: 480
  scale: 1.0
  fx: 604.3154296875
  fy: 603.8231811523438
  cx: 325.12115478515625
  cy: 261.0074462890625
  d0: 0.0
  d1: 0.0
  d2: 0.0
  d3: 0.0

Obtain camera calibration parameters:

ros2 topic echo --once /camera/camera/color/camera_info

Extract from the K matrix: fx=K[0], fy=K[4], cx=K[2], cy=K[5].

Configure Extrinsic Parameters

IMU-to-LiDAR Extrinsic

Configure in extrin_calib of avia.yaml:

extrin_calib:
  extrinsic_T: [0.04165, 0.02326, -0.0284]  # IMU to LiDAR translation
  extrinsic_R: [1.0, 0.0, 0.0,
                0.0, 1.0, 0.0,
                0.0, 0.0, 1.0]              # IMU to LiDAR rotation

LiDAR-to-Camera Extrinsic

extrin_calib:
  Rcl: [0.00610193,-0.999863,-0.0154172,
        -0.00615449,0.0153796,-0.999863,
        0.999962,0.00619598,-0.0060598]
  Pcl: [0.0194384, 0.104689,-0.0251952]

Time Offset

time_offset:
  imu_time_offset: 0.0    # IMU time offset
  img_time_offset: 0.1    # Camera time offset (adjust based on actual hardware)
  exposure_time_init: 0.0 # Exposure time initial value

Core Interfaces and Data Flow

LIVMapper — Main Mapper

LIVMapper is the core class of FAST-LIVO2, responsible for subscribing to sensor data, performing fused SLAM, and publishing results.

Construction and Initialization

LIVMapper mapper(nh, "laserMapping", options);
mapper.initializeSubscribersAndPublishers(nh, it_);
mapper.run(nh);

Subscribed Topics

Topic	Message Type	Description
/livox/lidar	livox_ros_driver2/msg/CustomMsg	Livox point cloud data
/livox/imu	sensor_msgs/msg/Imu	Livox IMU data
/left_camera/image	sensor_msgs/msg/Image	Camera RGB image

Published Topics

Topic	Message Type	Description
/cloud_registered	sensor_msgs/msg/PointCloud2	Registered colored/intensity point cloud
/aft_mapped_to_init	nav_msgs/msg/Odometry	Odometry pose
/path	nav_msgs/msg/Path	Trajectory path
/Laser_map	sensor_msgs/msg/PointCloud2	Global map
/cloud_effected	sensor_msgs/msg/PointCloud2	Effective feature points
/rgb_img	sensor_msgs/msg/Image	Rendered RGB image
/LIVO2/imu_propagate	nav_msgs/msg/Odometry	IMU preintegration odometry
/mavros/vision_pose/pose	geometry_msgs/msg/PoseStamped	MAVROS pose

Operating Modes

The system automatically selects the operating mode based on configuration:

Mode	Condition	Description
LIVO	img_en=1 and lidar_en=1	LiDAR + Inertial + Visual fusion
ONLY_LIO	imu_en=true and img_en=0	LiDAR + Inertial only
ONLY_LO	No IMU, no camera	LiDAR only

Data Flow

graph LR
    A["Livox LiDAR"] -->|/livox/lidar| B["FAST-LIVO2 /laserMapping"]
    C["Livox IMU"] -->|/livox/imu| B
    D["RealSense D435i"] -->|/camera/color/image_raw| B
    B -->|/cloud_registered| E["RViz / Point Cloud Map"]
    B -->|/aft_mapped_to_init| F["Odometry"]
    B -->|/path| G["Trajectory"]
    B -->|/rgb_img| H["RGB Image"]

Core Processing Pipeline

void LIVMapper::run(rclcpp::Node::SharedPtr &node) {
    rclcpp::Rate rate(5000);
    while (rclcpp::ok()) {
        rclcpp::spin_some(this->node);
        if (!sync_packages(LidarMeasures)) {
            rate.sleep();
            continue;
        }
        handleFirstFrame();    // First frame initialization
        processImu();          // IMU preintegration
        stateEstimationAndMapping();  // State estimation and mapping
    }
    savePCD();                 // Save point cloud
}

Each cycle executes in sequence:

1. Data Synchronization (sync_packages): Align LiDAR, IMU, and image data by timestamp
2. IMU Processing (processImu): Preintegration, debiasing, state propagation
3. State Estimation (stateEstimationAndMapping): Dispatch to VIO or LIO processing based on mode

Multi-Sensor Time Synchronization

Problem Background

The Livox Mid-360s LiDAR’s LiDAR and IMU use different hardware clocks with no shared reference. Without correction, the timestamp offset between them drifts at approximately 1 second/second, causing FAST-LIVO2 to stop working after about 20 seconds.

Hardware synchronization (PTP, GPIO) is not feasible in this scenario: PTP can reach the Livox unit but not its internal IMU oscillator; the RealSense D435i’s RGB and depth sensors are on different PCBs, so GPIO synchronization cannot reach the RGB stream.

Synchronization Solution

graph TB
    A["Livox IMU<br/>Own clock, drifts"] --> B["EMA Offset Filter<br/>α = 0.01, tracks drift"]
    C["Livox LiDAR<br/>Reference clock"] --> D["sync_packages()<br/>Mutex-protected merge"]
    E["RealSense D435i<br/>Host clock"] --> F["Static Offset<br/>Fixed YAML value"]
    B --> D
    F --> D
    D --> G["FAST-LIVO2 SLAM"]

IMU-LiDAR Offset Correction

An Exponential Moving Average (EMA) filter ($\alpha = 0.01$) is used in imu_cbk to continuously re-estimate the clock offset:

// Detect IMU-to-LiDAR timestamp deviation
if (abs(last_timestamp_imu - stamp2Sec(msg->header.stamp)) > 1.0) {
    double timediff_imu_wrt_lidar = last_timestamp_imu - stamp2Sec(msg->header.stamp);
    RCLCPP_INFO(this->node->get_logger(),
        "Self sync IMU and LiDAR, HARD time lag is %.10lf", timediff_imu_wrt_lidar - 0.100);
}

Camera Offset Correction

RealSense streams are corrected using a fixed img_time_offset, since the driver already timestamps based on the host clock:

time_offset:
  img_time_offset: 0.1    # Set in YAML, not computed at runtime

Non-blocking Timestamp Handling

The original logic that discards IMU/image frames when timestamp jumps are detected has been replaced with warning log messages, preventing the pipeline from stalling due to transient jitter.

Branch Selection

Branch	Applicable Scenario
main	Sensors are hardware-synchronized, or running public datasets (e.g., Retail_Street.db3)
soft-sync	IMU, LiDAR, and camera use independent unsynchronized clocks

Switch branch:

git checkout soft-sync

Running Examples

Run via ROS2 Bag (Simulation/Playback)

Download the FAST-LIVO2 original dataset (Retail_Street):

# Install sqlite3 tool
sudo apt update && sudo apt install sqlite3

# Update topic types in the dataset (for livox_ros_driver2 compatibility)
sqlite3 Retail_Street.db3 "UPDATE topics SET type = 'livox_ros_driver2/msg/CustomMsg' WHERE name = '/livox/lidar';"

Run the algorithm:

cd fast-livo2-deep-robotics
source install/setup.bash
ros2 launch fast_livo mapping_avia.launch.py use_rviz:=True

Play the dataset:

source install/setup.bash
ros2 bag play Retail_Street.db3

Run on a Real Lite3 Robot

Start the LiDAR

Terminal 1 — LiDAR (Mid-360s):

cd fast-livo2-deep-robotics
source /opt/ros/humble/setup.bash
source install/setup.bash
ros2 launch livox_ros_driver2 msg_MID360s_launch.py

For Mid-360, use msg_MID360_launch.py.

Start the Camera

Terminal 2 — D435i RGB camera:

ros2 launch realsense2_camera rs_launch.py \
  enable_rgbd:=false \
  enable_sync:=false \
  align_depth.enable:=false \
  enable_color:=true \
  enable_depth:=false \
  rgb_camera.color_profile:=640x480x30

Confirm the launch output shows Width: 640, Height: 480, FPS: 30.

Start FAST-LIVO2

Terminal 3 — FAST-LIVO2 and RViz:

cd fast-livo2-deep-robotics
source /opt/ros/humble/setup.bash
source install/setup.bash
ros2 launch fast_livo mapping_avia.launch.py use_rviz:=True

Note: Keep the sensor components stationary for several seconds after startup to complete IMU initialization. After initialization is complete, slowly move the robot to build the map.

Configuration Parameters

Common Configuration

common:
  img_topic: "/left_camera/image"    # Camera topic
  lid_topic: "/livox/lidar"          # LiDAR topic
  imu_topic: "/livox/imu"            # IMU topic
  img_en: 1                          # Enable camera (0=disable)
  lidar_en: 1                        # Enable LiDAR (0=disable)
  ros_driver_bug_fix: false          # ROS driver timestamp fix

Preprocessing Configuration

preprocess:
  point_filter_num: 1        # Point cloud downsampling factor
  filter_size_surf: 0.1      # Voxel filter size (meters)
  lidar_type: 1              # 1=Livox Avia
  scan_line: 6               # Number of scan lines
  blind: 0.8                 # Blind zone distance (meters)

VIO Configuration

vio:
  max_iterations: 5          # Maximum iterations
  outlier_threshold: 1000    # Outlier threshold
  img_point_cov: 100         # Image point covariance
  patch_size: 8              # Image patch size
  patch_pyrimid_level: 4     # Pyramid levels
  normal_en: true            # Normal constraint
  exposure_estimate_en: true # Exposure estimation

LIO Configuration

lio:
  max_iterations: 5          # Maximum iterations
  voxel_size: 0.5            # Voxel size (meters)
  max_layer: 2               # Maximum voxel layers
  max_points_num: 50         # Max points per voxel

IMU Configuration

imu:
  imu_en: true               # Enable IMU
  imu_int_frame: 30          # IMU initialization frames
  acc_cov: 0.5               # Acceleration covariance
  gyr_cov: 0.3               # Angular velocity covariance
  b_acc_cov: 0.0001          # Acceleration bias covariance
  b_gyr_cov: 0.0001          # Angular velocity bias covariance

Verification and Troubleshooting

Verify Topics

# Check topic types
ros2 topic type /livox/lidar                    # Expected: livox_ros_driver2/msg/CustomMsg
ros2 topic type /livox/imu                      # Expected: sensor_msgs/msg/Imu
ros2 topic type /camera/camera/color/image_raw  # Expected: sensor_msgs/msg/Image

# Check data rates
ros2 topic hz /livox/lidar                      # Expected: ~10 Hz
ros2 topic hz /livox/imu                        # Expected: ~200 Hz
ros2 topic hz /camera/camera/color/image_raw    # Expected: ~30 Hz

Verify FAST-LIVO2 Output

ros2 topic hz /cloud_registered       # Registered point cloud
ros2 topic hz /aft_mapped_to_init     # Odometry
ros2 topic hz /path                   # Trajectory

Check Nodes

ros2 node list
# Expected to include: /livox_lidar_publisher, /camera/camera, /laserMapping

ros2 topic info -v /livox/lidar
# Expected: 1 publisher, /laserMapping as subscriber

Common Troubleshooting

Issue	Troubleshooting Steps
No point cloud in RViz	Check ros2 node list for all expected nodes
No LiDAR data	Confirm msg_MID360s_launch.py matches the physical device
No camera data	Confirm resolution in camera_pinhole.yaml matches actual settings
IMU initialization failure	Keep sensors stationary for several seconds; check IMU topic frequency
Timestamp jumps	Check for multiple Livox driver instances competing for UDP ports
Driver bind failure	Confirm host_ip is assigned to a local interface
Library version conflict	Check ldd output; ensure /usr/local/lib version is loaded
Stops after 20 seconds	Switch to soft-sync branch to enable software time synchronization

Advanced Features

Point Cloud Saving

pcd_save:
  pcd_save_en: true          # Enable point cloud saving
  colmap_output_en: false    # Colmap format output
  filter_size_pcd: 0.15      # Voxel filter size on save
  interval: -1               # -1 = all frames saved to one file

Save paths: Log/PCD/all_raw_points.pcd and Log/PCD/all_downsampled_points.pcd.

Colmap Output

pcd_save:
  colmap_output_en: true
  interval: -1

Output path: Log/Colmap/sparse/0/points3D.txt.

Trajectory Evaluation

evo:
  seq_name: "CBD_Building_01"  # Sequence name
  pose_output_en: true         # Enable pose output

Output path: Log/result/<seq_name>.txt.

Dynamic Object Detection

FAST-LIVO2 supports dynamic object detection and removal:

Topic	Description
/dyn_obj	Detected dynamic object point cloud
/dyn_obj_removed	Point cloud after removing dynamic objects
/dyn_obj_dbg_hist	Dynamic object debug history

IMU Preintegration Odometry

uav:
  imu_rate_odom: true    # Enable high-frequency IMU preintegration odometry

Output topic: /LIVO2/imu_propagate (250Hz).

Safety Precautions

1. Hardware Installation Safety: When installing the AGX kit, ensure screws are tightened evenly to avoid loosening.
2. Cable Management: Ensure all cables are securely fastened to prevent disconnection during robot motion.
3. Thermal Management: The AGX Jetson Orin generates significant heat under heavy load; ensure adequate cooling.
4. Network Isolation: Set ipv4.never-default yes for the LiDAR Ethernet connection to avoid disrupting normal networking.
5. Sensor Protection: LiDAR and camera are precision optical devices; avoid collisions and direct exposure to intense light.
6. Software Synchronization Limitations: Software time synchronization is an approximate solution; sharp turns may cause camera-LiDAR frame misalignment, and accumulated drift in large environments may exceed the filter’s correction capability.