kevin2431 / traj-lo

[RA-L 2024] In Defense of LiDAR-Only Odometry Using an Effective Continuous-Time Trajectory

Traj-LO

A LiDAR-only Odometry from Continuous-Time perspective

🛠️ Installation | 🎥 Video | 📖 Paper

What is Traj-LO

Traj-LO aims to explore the limits of state estimation using only LiDAR sensors.

Nowadays, LO approaches heavily rely on IMU data for accurate state estimation but neglect the true capabilities of LiDAR sensors. Through the continuous-time perspective, Traj-LO matches the performance of state-of-the-art LIO methods in diverse scenarios.

The spatial-temporal movement of LiDAR is parameterized by a simple yet effective continuous-time trajectory, which consists of multiple piecewise linear functions. By coupling the geometric information from streaming LiDAR points and kinematic constraints from trajectory smoothness, it can work even in scenarios where the motion state exceeds the IMU's measuring range. Besides, the framework is generalized for different kinds of LiDAR as well as multi-LiDAR systems.

How to use Traj-LO

Traj-LO is a ROS-independent project and is suitable for cross-platform applications. For convenience, we provide a ROSbag data loader that can read public datasets and your own recorded data.

Supported Dataset

Currently, the released code only supports one LiDAR configuration. We will update it as soon as possible to provide multi-LiDAR support. The provided ROSbag data loader supports different types of LiDAR, including Livox, Ouster, Hesai, Robosense, and Velodyne. We have tested Traj-LO with the following datasets.

Dataset	LiDAR	Message Type	Configuration file
NTU VIRAL	Two Ouster OS1-16	sensor_msgs/PointCloud2	config_ntu.yaml
Hilti 2021	Ouster OS0-64 Livox mid-70	sensor_msgs/PointCloud2 livox_ros_driver/CustomMsg	config_ouster.yaml config_livox.yaml
R3LIVE	Livox Avia	livox_ros_driver/CustomMsg	config_livox.yaml
Point-LIO	Livox Avia	livox_ros_driver/CustomMsg	config_pointlio.yaml
New College	Ouster OS-1 64 Ouster OS-0 128	sensor_msgs/PointCloud2	config_ouster.yaml
Hilti 2022 &2023	Hesai PandarXT-32	sensor_msgs/PointCloud2	config_hesai.yaml
SubT-MRS	Velodyne VLP16	sensor_msgs/PointCloud2	config_velodyne.yaml

The corresponding configuration files are located in the "data" directory. For optimal performance, you will need to fine-tune the parameters.

Since Traj-LO is a LiDAR-only method, it may fail in narrow spaces where there are few valid points for a long time.

Dependency

In addition to the ROSbag data loader, Traj-LO also provides a simple custom GUI for visualization and uses Eigen-based Gauss-Newton for pose optimization. Here are the major libraries we will use.

• Optimization: Eigen, Sophus
• GUI: ImGui, OpenGL, GLM
• DataLoader: oneTBB, Boost

Although major dependencies are included in the third-party folder, you may still need to run the script install_deps.sh to install libraries like Boost, etc.

Build

You can install the Traj-LO project by following these steps:

git clone --recursive https://github.com/kevin2431/Traj-LO.git
cd Traj-LO
./scripts/install_deps.sh # make sure we have all the dependency
mkdir build && cd build
cmake .. 
make -j8

Run

After modifying the config file for your environment, you can run Traj-LO. Here is an example to test it with a Livox LiDAR.

./trajlo ../data/config_livox.yaml

Some Tips

• Traj-LO is a continuous-time method, so each point in your rosbag should have a corresponding timestamp.
• When the motion profile is aggressive, you can decrease seg_interval or increase kinematic_constraint.

Cross-platform Support

Linux

Ubuntu 20.04, 22.04

Windows

You can use WSL2 to install the Ubuntu subsystem and then follow the above instructions to test Traj-LO. To enable OpenGL accelerated rendering in WSLg, you may need to select Nvidia GPU.

MacOS

Make sure you have Homebrew to run the srcipt install_deps.shto install dependencies. We have tested Traj-LO on M2 Mac Mini (macOS 14.4.1).

ROS

Still working on it!

Citation

If you use this project for any academic work, please cite our RA-L paper.

@ARTICLE{zheng2024traj,
    author={Zheng, Xin and Zhu, Jianke},
    journal={IEEE Robotics and Automation Letters},
    title={Traj-LO: In Defense of LiDAR-Only
    Odometry Using an Effective Continuous-Time
    Trajectory},
    year={2024},
    volume={9},
    number={2},
    pages={1961-1968},
    doi={10.1109/LRA.2024.3352360}
}

For those interested in multi-sensor fusion, particularly in multi-LiDAR and multi-IMU systems, I recommend checking out my latest work, Traj-LIO, which is a resilient state estimator through sparse Gaussian Processes.

Contributing

Traj-LO is currently in beta version, and we are actively working on it. We welcome community users to participate in this project.

Acknowledgement

Thanks for these pioneering works Basalt (Batch Optimization), CT-ICP (Continuous-time Idea), and KISS-ICP (VoxelMap Management).

traj-lo's Issues

Loop Closure And Map Saving

You have done a really amazing job regarding map quality!

But I would like to know if there is a loop closure module as from my tests, it seems that there is no loop closure.

For the map, is there a way to save a generated map in pcd? Also, can i extract the path to be used with evo evaluation tool?

Support for MID-40？

Thanks for releasing the code! I'm wondering if Traj-LO supports Livox MID-40? We tried to run Traj-LO with our bags. However, we encountered with Segmentation fault.

Can Traj-LO run in real-time?

I printed the time taken for each iteration of the while (true) loop on line 67 in the odometry.cpp file. Below is a part of the output:
dometry computation took 70.6064 milliseconds.
odometry computation took 108.871 milliseconds.
odometry computation took 87.9254 milliseconds.
odometry computation took 99.0804 milliseconds.
odometry computation took 85.6526 milliseconds.
odometry computation took 84.4505 milliseconds.
odometry computation took 111.856 milliseconds.
odometry computation took 106.236 milliseconds.
odometry computation took 79.5079 milliseconds.
odometry computation took 70.2243 milliseconds.
odometry computation took 101.104 milliseconds.
odometry computation took 102.121 milliseconds.
odometry computation took 80.6436 milliseconds.
odometry computation took 71.5117 milliseconds.
odometry computation took 104.166 milliseconds.
odometry computation took 75.5676 milliseconds.
odometry computation took 126.864 milliseconds.
odometry computation took 119.489 milliseconds.
odometry computation took 88.3249 milliseconds.
odometry computation took 69.8241 milliseconds.

Be aware of License Terms 😉

Hello, thanks for opening the source code for the benefit of the community.

I'm very happy to see many patterns we've developed with much love and effort for KISS-ICP. It definitely makes your implementation stand against the usual ones we see in LiDAR odometry.

Although I would recommend being a bit more conservative when it comes to copying code from other projects, even when you don't pay for it, there are licensing terms:
https://github.com/PRBonn/kiss-icp/blob/11c7d97d156a7ff72fefdacc3105db4a99805e0e/LICENSE#L13-L14

Just to name a few

Traj-LO/include/trajlo/core/map_manager.h

Lines 42 to 59 in f04d15a

	struct VoxelBlock {
	// buffer of points with a max limit of n_points
	std::vector<Eigen::Vector3d> points;
	int num_points_;
	inline void AddPoint(const Eigen::Vector3d &point) {
	if (points.size() < static_cast<size_t>(num_points_))
	points.push_back(point);
	}
	};
	struct VoxelHash {
	size_t operator()(const Voxel &voxel) const {
	const uint32_t *vec = reinterpret_cast<const uint32_t *>(voxel.data());
	return ((1 << 20) - 1) &
	(vec[0] * 73856093 ^ vec[1] * 19349663 ^ vec[2] * 83492791);
	}
	};

Looks WAY too similar too
https://github.com/PRBonn/kiss-icp/blob/11c7d97d156a7ff72fefdacc3105db4a99805e0e/cpp/kiss_icp/core/VoxelHashMap.hpp#L36-L51

I don't mind at ALL that you reuse some code. Just be mindful of not claiming it as yours, as you claim in your license.

Inquiry about running NTU-VIRAL dataset with config_ntu.yaml

Thank you for your excellent open-source lidar odometry.

I evaluated the TRAJ-LO on the NTU-VIRAL dataset with the ./trajlo ../data/config_ntu.yaml, but could not get consistent ATE results with the paper published on RA-L. The ATE is getting from the official evaluation python script from NTU-VIRAL dataset . Could you give me some advice about how to get a consistent ATE with the results illustrated in your paper?
Moreover, the ./trajlo ../data/config_ntu.yaml seems useful for single lidar odometry, but does not work for the multi-lidar odometry. How can I run TRAJ-LO with multiple lidar?

Looking forward to your reply.

Is there any schedule for code release

Nice work! Any idea when the code will be released ? Thanks