hku-mars / voxelmap Goto Github PK

[RA-L 2022] An efficient and probabilistic adaptive voxel mapping method for LiDAR odometry

CMake 1.79% C++ 98.21%

voxelmap's Introduction

VoxelMap

Introduction

VoxelMap is an efficient and probabilistic adaptive(coarse-to-fine) voxel mapping method for 3D LiDAR. Unlike the point cloud map, VoxelMap uses planes as representation units. A scan of LiDAR data will generate or update the plane. Each plane contains its own plane parameters and uncertainties that need to be estimated. This repo shows how to integrate VoxelMap into a LiDAR odometry.

The plane map constructed by VoxelMap on KITTI Odometry sequence 00.

Developers:

Chongjian Yuan 袁崇健， Wei Xu 徐威

Related paper

Related paper available on arxiv:
Efficient and Probabilistic Adaptive Voxel Mapping for Accurate Online LiDAR Odometry

1. Prerequisites

1.1. PCL && Eigen

PCL>= 1.8, Follow PCL Installation.

Eigen>= 3.3.4, Follow Eigen Installation.

1.2. livox_ros_driver

Follow livox_ros_driver Installation.

2. Build

Clone the repository and catkin_make:

    cd ~/$A_ROS_DIR$/src
    git clone https://github.com/hku-mars/VoxelMap.git
    cd ..
    catkin_make
    source devel/setup.bash

Remember to source the livox_ros_driver before build (follow 1.2 livox_ros_driver)

3. Run on Dataset

Current version of VoxelMap does not support IMU and requires undistorted point cloud.

3.1 Run on KITTI Odometry dataset

Step A: Setup before run Edit config/velodyne.yaml to set the below parameters:

LiDAR point cloud topic name: lid_topic
If you want to show the voxel map, set pub_voxel_map to true
If you want to show the accumulated point cloud map, set pub_point_cloud to true

Step B: Run below

    cd ~/$VOXEL_MAP_ROS_DIR$
    source devel/setup.bash
    roslaunch voxel_map mapping_velodyne.launch

Step C: Play rosbag.

If want to save the trajectory result (camera pose), set the write_kitti_log to true and change the result_path to your own path.

3.2 Run on L515 dataset

Step A: Download our bags here: Voxel Map L515 Datasets

Then the same step as 3.1

4.Acknowledgments

Thanks for Fast-LIO2 (Fast Direct LiDAR-inertial Odometry)

5. License

The source code is released under GPLv2 license.For any technical issues, please contact us via email [email protected]. For commercial use, please contact Dr. Fu Zhang [email protected].

voxelmap's People

Contributors

Stargazers

Watchers

voxelmap's Issues

Mountain Dataset Release

Hi,
are there any updates on the release of the mountain dataset mentioned in the paper?
I'd be very interested in using it for my research :)

计算世界系点云协方差的时候貌似存在bug

袁博，抱歉请教几个问题！
1：在创建地图，更新地图，点云匹配时都会计算世界系下的协方差，代码中的多项式的第二项貌似少了旋转，请问这是一个bug么？
2：此处的point_crossmat是body系下的点，而非lidar系下的点。与论文不太相符，请问这是为什么呢？

是否如下代码表示才是更符合论文原理的实现呢？

HELP

I found a problem with the IMU information of the 00 and 02 sequences in KITTI raw, how did you solve it. Or is the KITTI odometry dataset being used. Can you share your rosbag for 00 sequence?

about Degeneration

Thank you very much for your open source code. I just finished learning it, and I found out if degradation is a benefit issue
int minRow, minCol;
if(V.minCoeff(&minRow, &minCol) < 1.0f) //if (0) //
{
VD(6) V = H_T_H.block<6, 6>(0, 0).eigenvalues().real();
cout << "!!!!!! Degeneration Happend, eigen values: "
<< V.transpose() << endl;
EKF_stop_flg = true;
solution.block<6, 1>(9, 0).setZero();
}

There seems to be no assignment to minRow and minCol, and I don't understand the usage of the V function. I don't quite understand this block

About state covariance update

Great thanks to your project and I have a question about state covariance. At each period of GN iteration. After state update in j-th iteration, shouldn't the state covariance be updated? I think the initial cov of the state in j=0 and of the j-th are different in my mind. Hope for your answer.

No moving in my dataset!!! But work normal in other LIO&LO algorithms

voxel_mapping_odom node not found/located

Hello @ChongjianYUAN

I have Ubuntu 18.04 and ROS melodic and I run the command in the terminal
$ roslaunch voxel_map mapping_velodyne.launch
and the result/output in the terminal is as following:

SUMMARY

PARAMETERS

/Result/result_path: /home/ycj/catkin_...
/Result/write_kitti_log: False
/common/imu_topic: /imu_data
/common/lid_topic: /Navtech/Filtered...
/imu/extrinsic_R: [1, 0, 0, 0, 1, 0...
/imu/extrinsic_T: [0, 0, 0]
/imu/imu_en: False
/mapping/down_sample_size: 0.5
/mapping/layer_point_size: [5, 5, 5, 5, 5]
/mapping/max_cov_points_size: 1000
/mapping/max_iteration: 3
/mapping/max_layer: 4
/mapping/max_points_size: 1000
/mapping/plannar_threshold: 0.01
/mapping/voxel_size: 3.0
/noise_model/acc_cov_scale: 1.0
/noise_model/angle_cov: 0.1
/noise_model/gyr_cov_scale: 0.5
/noise_model/ranging_cov: 0.04
/preprocess/blind: 1
/preprocess/calib_laser: True
/preprocess/lidar_type: 2
/preprocess/point_filter_num: 1
/preprocess/scan_line: 64
/rosdistro: melodic
/rosversion: 1.14.13
/visualization/dense_map_enable: False
/visualization/pub_point_cloud: True
/visualization/pub_point_cloud_skip: 5
/visualization/pub_voxel_map: True
/visualization/publish_max_voxel_layer: 1

NODES
/
voxel_mapping_odom (voxel_map/voxel_mapping_odom)

ROS_MASTER_URI=http://localhost:11311

ERROR: cannot launch node of type [voxel_map/voxel_mapping_odom]: Cannot locate node of type [voxel_mapping_odom] in package [voxel_map]. Make sure file exists in package path and permission is set to executable (chmod +x)
No processes to monitor
shutting down processing monitor...
... shutting down processing monitor complete

What is the problem?
I cannt find the file voxel_mapping_odom...

Best,
PM

运行avia数据漂移

您好！感谢您出色的工作！我有一个问题想请教您，目前我运行自己的avia采集的数据，立马漂移，请问需要如何修改您的代码才能正常运行，或者我是不是哪里参数设置的不对？期待您的答复

Nan bug

VoxelMap/include/voxel_map_util.hpp

Line 641 in d787ee8

float range_dis = sqrt(dis_to_center - dis_to_plane * dis_to_plane);

这里有一个bug，sqrt里可能是负值。

顺便请教一下，这里的range_dis具体含义是什么？

No TF data

请问是否方便公开Unstructured Environment Test的数据集呢？

非常感谢作者的作品，有效的平面评估使得我们的研究思路受益匪浅，尤其在解决退化场景时，有很好的效果。
请问您在论文中关于Mountain, Park的 Avia数据集方便公开么？这种高空单向的山地飞行数据很珍贵，也很难找到类似这么好的数据了=-=

请问支持ouster激光雷达吗

about point covariance

According to the paper, covariance of point in the world:

but in the codes, it seems missing W_R_L in the second iterm, see codes:
cov = state.rot_end * cov * state.rot_end.transpose() +
(-point_crossmat) * state.cov.block<3, 3>(0, 0) *
(-point_crossmat).transpose() +
state.cov.block<3, 3>(3, 3);

So, is it a bug in the codes?

thanks!

哈希索引的代码问题

袁博您好，抱歉请教个问题：

for (int j = 0; j < 3; ++j) {
            loc_xyz[j] = p_v.point[j] / voxel_size;
            if (loc_xyz[j] < 0) loc_xyz[j] -= 1.0; // todo: ???
}

在计算哈希索引的时候，为什么为负的值都要减1操作呢？可能是我太菜了，但是我真的没有想明白，希望您能在百忙中帮我解答一下，万分感谢！

后续是否会更新imu版本？

袁博和林博都好高产，看到了你们的ImMesh。想问下ImMesh论文被接收后这个VoxelMap的IMU版本是否会更新？

about state

L515 Failed to find match for field 'intensity'

i launch mapping_l515.launch and play l5152.bag. it prompts
Failed to find match for field 'intensity'.
Failed to find match for field 'time'.
Failed to find match for field 'ring'.
and nothing can be seen in rviz.
by the way, how to make my own bag with l515

How to build and maintain a consistent voxel map?

Hi, @ChongjianYUAN, thanks for your sharing! I'm puzzled about the buildVoxelMap() and updateVoxelMap() in both of which you the local coordinate of each laser point is used to determine the voxel key. From my understanding, the world coordinate instead of local coordinate should be used to find the key of each point, to maintain the consistency of the voxel map. Do you have any other consideration for this issue?

初始化不鲁棒

VoxelMap总体性能很好，但是UAV在初始化时如果飞机一直在运动则并不鲁棒非常容易发散，请问这是为什么呢？演示视频中UAV也是从中途平飞静止时启动的算法，请问有没有什么好的解决方法呢？

error: invalid conversion from ‘const char*’ to ‘int’ [-fpermissive]

您好，感谢您的分享，这里有个问题想要请教，在我尝试编译的时候，出现了以下错误：
~/catkin_ws/src/VoxelMap-master/include/voxel_map_util.hpp: In function ‘void BuildResidualListOMP(const std::unordered_map<VOXEL_LOC, OctoTree*>&, double, double, int, const std::vector&, std::vector&, std::vector<Eigen::Matrix<double, 3, 1> >&)’:
error: invalid conversion from ‘const char*’ to ‘int’ [-fpermissive]
请问您知道这里是什么原因吗，谢谢！

请问为什么kitti要进行高度角校正？

Close range points

Hi!!

First of all, congratulations for your work!

I'm testing the code and it seems to work well.
I'm testing it with Livox Horizon datasets, and I need to avoid too close points.

Is there any easy way to crop too near points?

Thanks in advance

更新方程为什么没有先验的雅可比

作者您好，感谢您的开源工作，这里有一个疑惑，想要向您请教：

根据fast_lio2的论文，更新的方程如下：

在这个项目中，代码中求解方程如下：
solution = K * meas_vec + vec - K * Hsub * vec.block<6, 1>(0, 0);
相比fast_lio2没有Jk预测方程的雅可比矩阵，请问这里是有什么不一样吗，为什么这里没有Jk。

谢谢！

There is no such file or directory 12 | #include <voxel_ map/Pose6D.h>

Hello, I couldn't find this header file while running the program

fatal error: voxel_ Map/Pose6D. h: There is no such file or directory

12 | #include <voxel_ map/Pose6D.h>

I searched the internet a lot and couldn't find how to install this header file. This package also doesn't contain this header file. How should I install it?

如何实现l515彩色点云mapping？

您好！我看文章中有补充材料实现了L515 RGB点云的地图构建，但是目前好像只能进行激光点云的地图构建。请问我需要如何修改才能实现RGB点云的地图构建？

执行catkin_make操作出现问题

有关pcl::PointXYZINormal点云intensity，normal_x(y,z)，以及曲率更新的问题

袁博您好，抱歉打扰，想请教一下在计算测量的雅可比矩阵H的过程中，计算完点的协方差之后，代码中做了一步更新laserCloudOri中点的一些属性值（intensity，normal_x， normal_y， normal_z， curvature），更新的具体公式我不是很明白，在论文中也没有找到对应的部分，可以麻烦您在百忙中抽空给我解答一下吗？非常感谢。具体代码部分如下：

laserCloudOri->points[i].intensity = sqrt(R_inv(i));
laserCloudOri->points[i].normal_x = corr_normvect->points[i].intensity; // point-to-plane dist
laserCloudOri->points[i].normal_y = sqrt(sigma_l);
laserCloudOri->points[i].normal_z = sqrt(norm_vec.transpose() * cov * norm_vec);
laserCloudOri->points[i].curvature = sqrt(sigma_l + norm_vec.transpose() * cov * norm_vec);

只运行几帧就报错了

[ERROR] [1675417375.513728154]: Ignoring transform for child_frame_id "aft_mapped" from authority "unknown_publisher" because of a nan value in the transform (-nan -nan -nan) (0.000000 0.000000 0.000000 1.000000)

roslaunch failed

Thanks for your contribution!

I think there is a bug in the roslaunch file that prevents the code from working properly.

<node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find voxel_mapping)/rviz_cfg/voxel_mapping.rviz" />

should be:
<node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find voxel_map)/rviz_cfg/voxel_mapping.rviz" />

Otherwise，there will be a error when running the launch file as follows:

Resource not found: voxel_mapping
ROS path [0]=/opt/ros/noetic/share/ros
ROS path [1]=/home/xxx/voxel_map_ws/src
ROS path [2]=/opt/ros/noetic/share
The traceback for the exception was written to the log file