peterwon / d-liom Goto Github PK

Tightly-coupled Direct LiDAR-Inertial Odometry and Mapping Based on Cartographer3D.

Starlark 0.79% CMake 2.19% C++ 91.76% C 0.03% Lua 2.30% Python 1.73% Shell 1.12% GLSL 0.07%

d-liom's Introduction

D-LIOM: Tightly-coupled Direct LiDAR-Inertial Odometry and Mapping

Our paper: we have corrected some typos and errors in Section III-D of the previous version of the paper, the revised version can be accessed here. When amending our paper, I would like to say thanks to Sky Shaw, who has found my errors and warmly provided his suggestions.

Related video: A running demo can be found at https://youtu.be/21J2QLUQbno or at https://www.bilibili.com/video/BV14y4y157du.

Sensor suite supported

6-axis IMU + Velodyne/Ouster/Robosense LiDAR (Livox LiDAR can be also supported if the data of which is tansformed to the format of velodyne's, i.e., PointXYZRT.)

Prerequisites

Install depends follow the instructions of "cartographer" and "cartographer_ros" respectively.
Build OpenCV 3.3.1 (other version may work fine if it includes implementation of SURF keypoint detector) from source with "opencv_contrib"
Build GTSAM 4.0.2 with compile options as follows:

cmake -DGTSAM_USE_SYSTEM_EIGEN=ON -DGTSAM_WITH_TBB=OFF -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF -DGTSAM_COMPILE_OPTIONS_PUBLIC="" ..
Build PCL 1.8.0 with compile options as follows:

cmake -DPCL_ENABLE_SSE=OFF ..

Build

Clone this repository in your ROS workspace
Set CMake variable "OpenCV_DIR" in the root CMakeLists.txt of "cartographer".
Return to the root folder of your ROS workspace (which has the same directory as this "README" file) and run:
```
catkin_make_isolated
```

Run an online demo

TONGJI Dataset

Some demo sequences can be downloaded via:
- TONGJI dataset: link, pwd: hfrl
roslaunch dlio demo_dlio_tongji.launch bag_filename:=BAG_FILE_PATH where BAG_FILE_PATH is the full path of the downloaded bag file in your device
Some results on the self-collected TONGJI dataset.

NTU-VIRAL

The dataset is available online at NTU-VIRAL
```
```roslaunch dlio demo_dlio_viral.launch bag_filename:=BAG_FILE_PATH```
```
where BAG_FILE_PATH is the full path of the downloaded bag file in your device
```
Results on VIRAL
```

KAIST-Complex Urban Dataset

The dataset is available online at KAIST
Play KAIST bag follow the instruction: file_player
roslaunch dlio demo_dlio_kaist.launch
Results on KAIST "Urban-09" and "Urban-10"

Your own data

Write a urdf file of your device and save it to folder 'urdf'
Write a configuration file of your data and save it to 'config'
Write a launch file of your data and save it to 'launch'
Run your bag as above examples

Run offline and view mapping result

Launch dlio offline

roslaunch dlio dlio_offline_tongji bag_filenames:=BAG_FILE_PATH

View mapping result

roslaunch dlio dlio_map_viewer_rectified urdf_filename:=tongji pose_graph_pb_filename:=PG_FILENAME range_data_pb_filename:=RD_FILENAME rate:=100

where PG_FILENAME and RD_FILENAME are binary pbstream files saved to disk when launch the offline script above.

Acknowledgements

The authors of cartographer
The authors of cartographer_ros
The authors of LIO-SAM
The authors of VINS-Mono
The authors of VIO-Doc

d-liom's People

Contributors

Stargazers

Watchers

d-liom's Issues

gravity online estimation

Hey, @peterWon
Thank for your interesting work !
I see that you adopt gravity factor from cartographer 3D, i have read the paper as well as the code. I have tested some SOTA framework with gravity factor and without gravity factor, and the output odometry is better with gravity factor. But I can not figure out what exactly gravity factor affect odometry, what is the true purpose of it . If you have time please give me some answers.
This problem bother me for a week.
Thanks

pcl registration.h crash

hi, what's your version of pcl

/usr/bin/ld: 找不到 -lFLANN::FLANN

Hi :
在ubuntu20.04上编译运行的时候，遇到：

 58%] Linking CXX executable /home/chuanxu/dliom_ws/devel_isolated/cartographer_ros/lib/cartographer_ros/cartographer_read_constraints_from_pbsteam
/usr/bin/ld: 找不到 -lFLANN::FLANN
collect2: error: ld returned 1 exit status
make[2]: *** [cartographer_ros/CMakeFiles/cartographer_dev_rosbag_publisher.dir/build.make:401：/home/chuanxu/dliom_ws/devel_isolated/cartographer_ros/lib/cartographer_ros/cartographer_dev_rosbag_publisher] 错误 1
make[1]: *** [CMakeFiles/Makefile2:3173：cartographer_ros/CMakeFiles/cartographer_dev_rosbag_publisher.dir/all] 错误 2
make[1]: *** 正在等待未完成的任务....
/usr/bin/ld: 找不到 -lFLANN::FLANN
collect2: error: ld returned 1 exit status
make[2]: *** [cartographer_ros/CMakeFiles/cartographer_rosbag_validate.dir/build.make:401：/home/chuanxu/dliom_ws/devel_isolated/cartographer_ros/lib/cartographer_ros/cartographer_rosbag_validate] 错误 1
make[1]: *** [CMakeFiles/Makefile2:3335：cartographer_ros/CMakeFiles/cartographer_rosbag_validate.dir/all] 错误 2

编译问题，还请帮忙看一下。
谢谢

map update？

请问，该算法支持地图更新吗？在原有地图基础上更新变化了的点云地图

Initialization failed! Perform re-initialization...

 hi. 我在使用 Cartographer的公开数据集(b3-2016-04-05-14-14-00.bag)测试这个项目时, 无法正常的初始化(TJ数据集正常).

经过检查与TJ数据集的主要区别在于Cartographer的lidar数据是单包发送(一个lidar累积38包左右为一圈). 在尝试调整 frames_for_dynamic_initialization, num_accumulated_range_data等参数后, 也无法正常运行.

[FATAL] [1667358347.917461118]

[FATAL] [1667358347.917461118]: F1102 11:05:47.000000 123013 lua_parameter_dictionary.cc:83] Check failed: status == 0 (2 vs. 0) [string "/home/renxiao/D-LIOM/src/dlio/config/basic_co..."]:81: attempt to index field 'imu' (a nil value)

[cartographer_node-2] process has died [pid 123013, exit code -6, cmd /home/renxiao/D-LIOM/devel_isolated/cartographer_ros/lib/cartographer_ros/cartographer_node -configuration_directory /home/renxiao/D-LIOM/src/dlio/config -configuration_basename tongji.lua imu:=/zed2/zed_node/imu/data points2:=/rslidar_points __name:=cartographer_node __log:=/home/renxiao/.ros/log/ebe0e900-5a4b-11ed-ad62-c1e389bf42f9/cartographer_node-2.log].
log file: /home/renxiao/.ros/log/ebe0e900-5a4b-11ed-ad62-c1e389bf42f9/cartographer_node-2*.log

在Ubuntu20.04 下运行D-LIOM一直报错，这是啥原因啊？

[cartographer_node-3] process has died

您好，我严格安装了您要求的各个依赖包版本，配置cartographer可以成功运行，最终也成功编译了D-LIOM，但是在运行：
roslaunch dlio demo_dlio_tongji.launch bag_filename:=/home/panda/rosbag/D-LIOM/TJ-6.bag
过程中会碰到节点死掉的问题：
[cartographer_node-3] process has died [pid 20819, exit code -11, cmd /home/panda/cartographer_ws/devel_isolated/cartographer_ros/lib/cartographer_ros/cartographer_node -configuration_directory /home/panda/cartographer_ws/src/dlio/config -configuration_basename tongji.lua imu:=/zed2/zed_node/imu/data points2:=/rslidar_points __name:=cartographer_node __log:=/home/panda/.ros/log/5296ca3e-c568-11ed-b1dc-d08e7902e389/cartographer_node-3.log].
log file: /home/panda/.ros/log/5296ca3e-c568-11ed-b1dc-d08e7902e389/cartographer_node-3*.log

我重装了很多遍，也换了好几版opencv的版本，该问题一直没有解决，请问您知道可能导致该问题的原因吗？

gtsam

Hello, thank you for open-sourcing this project for my benefit. Currently at the moment I'm using your code to add IMU pre-integration to a 2d cartographer, and in the LocalTrajectoryBuilder2D::WindowOptimize function gtsam::PriorFactorgtsam::Pose2 priorPose(X(0) , prev_pose_, prior_pose_noise_); Occurs: use of undeclared identifier 'X'. I'm using gtsam for the first time and can't get started with this, can you suggest some modifications, thanks in advance for your reply.

process has died ：pid 17818, exit code -11

process has died [pid 17818, exit code -11, cmd /home/Desktop/D-LIOM-master/devel_isolated/cartographer_ros/lib/cartographer_ros/cartographer_node -configuration_directory /home/Desktop/D-LIOM-master/src/dlio/config -configuration_basename tongji.lua imu:=/zed2/zed_node/imu/data points2:=/rslidar_points __name:=cartographer_node __log:=/home/.ros/log/1ee3f080-130d-11ed-ab28-02423fbede56/cartographer_node-3.log].
出现进程崩掉，通过定位发现这里有问题：
local_trajectory_builder = common::make_unique(
trajectory_options.trajectory_builder_3d_options(),
SelectRangeSensorIds(expected_sensor_ids));

请大佬解决一下，效果一直没有办法验证

The corresponding paper

Hi, friend,

Thanks for your great work.

I'm interesting in your work, but unable to download your paper for some reason.

Can you upload the corresponding paper?

[cartographer_node-3] process has died

你好，非常感谢你们的分享，我在Run an online demo遇到了这个问题，可以请你看看应该怎么办吗？
我是这样操作的：
新建终端，source devel_isolated/setup.bash，然后roslaunch dlio demo_dlio_tongji.launch bag_filename:=/home/xwcd/Dataset/D-LIOM/TJ-2.bag
终端显示了如下信息：
... logging to /home/xwcd/.ros/log/17d6f370-0997-11ed-ba16-b0416f0553d8/roslaunch-xwcd-9099.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://xwcd:45417/

SUMMARY

PARAMETERS

/robot_description: <robot name="TONG...
/rosdistro: melodic
/rosversion: 1.14.13
/use_sim_time: True

NODES
/
cartographer_node (cartographer_ros/cartographer_node)
cartographer_occupancy_grid_node (cartographer_ros/cartographer_occupancy_grid_node)
playbag (rosbag/play)
robot_state_publisher (robot_state_publisher/robot_state_publisher)
rviz (rviz/rviz)

auto-starting new master
process[master]: started with pid [9117]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to 17d6f370-0997-11ed-ba16-b0416f0553d8
process[rosout-1]: started with pid [9135]
started core service [/rosout]
process[robot_state_publisher-2]: started with pid [9138]
process[cartographer_node-3]: started with pid [9139]
process[playbag-4]: started with pid [9140]
process[cartographer_occupancy_grid_node-5]: started with pid [9141]
process[rviz-6]: started with pid [9142]
[ INFO] [1658478007.163407526]: I0722 16:20:07.000000 9139 configuration_file_resolver.cc:41] Found '/home/xwcd/Project/D-LIOM-master/src/dlio/config/tongji.lua' for 'tongji.lua'.
[ INFO] [1658478007.164334948]: I0722 16:20:07.000000 9139 configuration_file_resolver.cc:41] Found '/home/xwcd/Project/D-LIOM-master/src/dlio/config/basic_config_3d.lua' for 'basic_config_3d.lua'.
[ INFO] [1658478007.164363531]: I0722 16:20:07.000000 9139 configuration_file_resolver.cc:41] Found '/home/xwcd/Project/D-LIOM-master/src/dlio/config/basic_config_3d.lua' for 'basic_config_3d.lua'.
[ INFO] [1658478007.165019573]: I0722 16:20:07.000000 9139 configuration_file_resolver.cc:41] Found '/home/xwcd/Project/D-LIOM-master/devel_isolated/cartographer/share/cartographer/configuration_files/map_builder.lua' for 'map_builder.lua'.
[ INFO] [1658478007.165099847]: I0722 16:20:07.000000 9139 configuration_file_resolver.cc:41] Found '/home/xwcd/Project/D-LIOM-master/devel_isolated/cartographer/share/cartographer/configuration_files/map_builder.lua' for 'map_builder.lua'.
[ INFO] [1658478007.165334917]: I0722 16:20:07.000000 9139 configuration_file_resolver.cc:41] Found '/home/xwcd/Project/D-LIOM-master/devel_isolated/cartographer/share/cartographer/configuration_files/pose_graph.lua' for 'pose_graph.lua'.
[ INFO] [1658478007.165385219]: I0722 16:20:07.000000 9139 configuration_file_resolver.cc:41] Found '/home/xwcd/Project/D-LIOM-master/devel_isolated/cartographer/share/cartographer/configuration_files/pose_graph.lua' for 'pose_graph.lua'.
[ INFO] [1658478007.165673538]: I0722 16:20:07.000000 9139 configuration_file_resolver.cc:41] Found '/home/xwcd/Project/D-LIOM-master/devel_isolated/cartographer/share/cartographer/configuration_files/trajectory_builder.lua' for 'trajectory_builder.lua'.
[ INFO] [1658478007.165721010]: I0722 16:20:07.000000 9139 configuration_file_resolver.cc:41] Found '/home/xwcd/Project/D-LIOM-master/devel_isolated/cartographer/share/cartographer/configuration_files/trajectory_builder.lua' for 'trajectory_builder.lua'.
[ INFO] [1658478007.165871252]: I0722 16:20:07.000000 9139 configuration_file_resolver.cc:41] Found '/home/xwcd/Project/D-LIOM-master/devel_isolated/cartographer/share/cartographer/configuration_files/trajectory_builder_2d.lua' for 'trajectory_builder_2d.lua'.
[ INFO] [1658478007.165955588]: I0722 16:20:07.000000 9139 configuration_file_resolver.cc:41] Found '/home/xwcd/Project/D-LIOM-master/devel_isolated/cartographer/share/cartographer/configuration_files/trajectory_builder_2d.lua' for 'trajectory_builder_2d.lua'.
[ INFO] [1658478007.166161172]: I0722 16:20:07.000000 9139 configuration_file_resolver.cc:41] Found '/home/xwcd/Project/D-LIOM-master/devel_isolated/cartographer/share/cartographer/configuration_files/trajectory_builder_3d.lua' for 'trajectory_builder_3d.lua'.
[ INFO] [1658478007.166206889]: I0722 16:20:07.000000 9139 configuration_file_resolver.cc:41] Found '/home/xwcd/Project/D-LIOM-master/devel_isolated/cartographer/share/cartographer/configuration_files/trajectory_builder_3d.lua' for 'trajectory_builder_3d.lua'.
[ INFO] [1658478007.193933971]: I0722 16:20:07.000000 9139 submap_3d.cc:325] Added submap 1
[cartographer_node-3] process has died [pid 9139, exit code -11, cmd /home/xwcd/Project/D-LIOM-master/devel_isolated/cartographer_ros/lib/cartographer_ros/cartographer_node -configuration_directory /home/xwcd/Project/D-LIOM-master/src/dlio/config -configuration_basename tongji.lua imu:=/zed2/zed_node/imu/data points2:=/rslidar_points __name:=cartographer_node __log:=/home/xwcd/.ros/log/17d6f370-0997-11ed-ba16-b0416f0553d8/cartographer_node-3.log].
log file: /home/xwcd/.ros/log/17d6f370-0997-11ed-ba16-b0416f0553d8/cartographer_node-3*.log

LIO_SAM in TJ dataset

Thanks for your excellent job! How do I run LIO_SAM on the TJ dataset? Can you provide the relevant config file?

opencv 3.4.18: undefined reference error in cartographer

D-LIOM-master/devel_isolated/cartographer/lib/libcartographer.a(submap_3d.cc.o): In function cartographer::mapping::ProjectToCvMat(cartographer::mapping::HybridGrid const*, cartographer::transform::Rigid3<double> const&, double&, double&, double&)': submap_3d.cc:(.text+0x14a2): undefined reference to cv::Mat::Mat(int, int, int, cv::Scalar_ const&)'

gtsam::Rot3::quaternion() vs gtsam::Rot3::toQuaternion()

Hello, I encountered a bug here because gtsam::Rot3::quaternion() does not behave as expected (the w, x, y, z are in the wrong order). This is probably due to the specific version of GTSAM I'm using, as the behavior has not always been the same (see
borglab/gtsam#1219). It may be a good idea to switch to toQuaternion() which should always be correct.

Current code:

D-LIOM/src/cartographer/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc

Lines 1107 to 1111 in 82063e0

 Eigen::Quaterniond rot = Eigen::Quaterniond( 

 prev_pose_.rotation().quaternion().w(), 

 prev_pose_.rotation().quaternion().x(), 

 prev_pose_.rotation().quaternion().y(), 

 prev_pose_.rotation().quaternion().z());

Proposed change:

  Eigen::Quaterniond rot = Eigen::Quaterniond(
    prev_pose_.rotation().toQuaternion().w(),
    prev_pose_.rotation().toQuaternion().x(),
    prev_pose_.rotation().toQuaternion().y(),
    prev_pose_.rotation().toQuaternion().z());

Missing source files

Hi,

Thank you for releasing D-LIOM. I found the approach described in the paper to be very elegant and with exceptionally good reported performance, so I wanted to see if I can maybe reproduce some of the results on the provided datasets.

Unfortunately, I found a sizable portion of core source files in this repo to be missing. Based on my count these are:

cartographer/common/tic_toc.h
cartographer/mapping/internal/3d/initialization/rigid3d_with_preintegrator.h
cartographer/mapping/internal/3d/initialization/imu_lidar_initializer.h
cartographer/mapping/internal/3d/gravity_factor/gravity_estimator.h
cartographer/mapping/internal/3d/gravity_factor/gravity_factor.h
cartographer/mapping/internal/3d/range_data_synchronizer.h
cartographer/mapping/proto/imu_options.proto
cartographer/mapping/proto/local_slam_range_data.proto
cartographer_ros/cartographer_ros/pbstream_3d_map_publisher_main.cc
cartographer_ros/cartographer_ros/kitti_trajectory_from_pbstream.cc
cartographer_ros/cartographer_ros/kitti_urdf_param_caculator.cc
cartographer_ros/cartographer_ros/gen_ground_truth_by_ndt_match.cc
cartographer_ros/cartographer_ros/dlio_eval_node.cc
cartographer_ros/cartographer_ros/read_constraints_from_pbsteam.cc
cartographer_ros/cartographer_ros/pb_range_data_to_ros_cloud_main.cc
cartographer_ros/cartographer_ros/trajectory_info_from_pbstream.cc

Could these perhaps be uploaded as well, so the repo would be in a complete and usable state?

	Eigen::Quaterniond rot = Eigen::Quaterniond(
	prev_pose_.rotation().quaternion().w(),
	prev_pose_.rotation().quaternion().x(),
	prev_pose_.rotation().quaternion().y(),
	prev_pose_.rotation().quaternion().z());