hku-mars / std

A 3D point cloud descriptor for place recognition

STD: A Stable Triangle Descriptor for 3D place recognition

1. Introduction

STD_detector is a global descriptor for 3D place recognition. For a triangle, its shape is uniquely determined by the length of the sides or included angles. Moreover, the shape of triangles is completely invariant to rigid transformations. Based on this property, we first design an algorithm to efficiently extract local key points from the 3D point cloud and encode these key points into triangular descriptors. Then, place recognition is achieved by matching the side lengths (and some other information) of the descriptors between point clouds. The point correspondence obtained from the descriptor matching pair can be further used in geometric verification, which greatly improves the accuracy of place recognition.

A typical place recognition case with STD. These two frames of point clouds are collected by a small FOV LiDAR (Livox Avia) moving in opposite directions, resulting in a low point cloud overlap and drastic viewpoint change.

1.1. Developers:

The codes of this repo are contributed by: Chongjian Yuan (袁崇健), Jiarong Lin (林家荣) and dustier

1.2. Related paper

Our paper has been accepted to ICRA2023, and our preprint version is now available on arxiv:
STD: Stable Triangle Descriptor for 3D place recognition

1.3. Related video

Our accompanying video is now available on YouTube.

2. Prerequisites

2.1 Ubuntu and ROS

We tested our code on Ubuntu18.04 with ros melodic and Ubuntu20.04 with noetic. Additional ROS package is required:

sudo apt-get install ros-xxx-pcl-conversions

2.2 Eigen

Following the official Eigen installation, or directly install Eigen by:

sudo apt-get install libeigen3-dev

2.3. ceres-solver (version>=2.1)

Please kindly install ceres-solver by following the guide on ceres Installation. Notice that the version of ceres-solver should higher than ceres-solver 2.1.0

2.4. GTSAM

Following the official GTSAM installation, or directly install GTSAM 4.x stable release by:

# Add PPA
sudo add-apt-repository ppa:borglab/gtsam-release-4.0
sudo apt update  # not necessary since Bionic
# Install:
sudo apt install libgtsam-dev libgtsam-unstable-dev

!! IMPORTANT !!: Please do not install the GTSAM of develop branch, which are not compatible with our code! We are still figuring out this issue.

2.5 Prepare for the data

Since this repo does not implement any method (i.e., LOAM, LIO, etc) for solving the pose for registering the LiDAR scan. So, you have to prepare two set of data for reproducing our results, include: 1) the LiDAR point cloud data. 2) the point cloud registration pose.

2.5.1. Download our Example data

Departure from the purpose of convenience, we provide two sets of data for your fast evaluation, which can be downloaded from OneDrive and BaiduNetDisk(百度网盘)

2.5.2. LiDAR Point cloud data

• For the Kitti dataset (i.e., our Example-1), we read the raw scan data with suffix ".bin". These raw LiDAR scan data can be downloaded from the Kitti Odometry benchmark website.
• For the solid-state LiDAR dataset (i.e., our Example-2), we read the undistort scan data from the recorded rosbag files, whose bag file contains undistort LiDAR scan data in rostopic: "/cloud_undistort"

2.5.3. Point cloud registration pose

In the poses file, the poses for LiDAR point cloud registration are given in the following data format:

Timestamp pos_x pos_y pos_z quat_x quat_y quat_z quat_w

where, Timestamp is the correspond sampling time stamp of a LiDAR scan, pose_{x,y,z} and quad_{x,y,z,w} are the translation and rotation (expressed used quaternion) of pose.

3. Examples

This reposity contains implementations of Stable Triangle Descriptor, as well as demos for place recognition and loop closure correction. For the complete pipline of online LiDAR SLAM, we will release this code along with the release of the extended version.

3.1. Example-1: place recognition with KITTI Odometry dataset

To run Example-1, you need to first download the poses file we provide.

Then, you should modify the demo_kitti.launch file

• Set the lidar_path to your local path
• Set the pose_path to your local path

cd $STD_ROS_DIR
source deve/setup.bash
roslaunch std_detector demo_kitti.launch

3.2. Example-2: place recognition with Livox LiDAR dataset

To run Example-2, you need to first download the rosbag file and poses file we provide. Then, you should modify the demo_livox.launch file

• Set the bag_path to your local path
• Set the pose_path to your local path

cd $STD_ROS_DIR
source deve/setup.bash
roslaunch std_detector demo_livox.launch

3.3. Example-3: loop closure correction on the KITTI Odometry dataset

The point cloud map and trajectory before and after correction by STD.

To run Example-3, you need to first download the poses file we provide or create your own pose file on the KITTI Odometry dataset with a LiDAR odom following the format: timestamp x y z qx qy qz qw Then, you should modify the demo_pgo.launch file

• Set the lidar_path to your local path
• Set the pose_path to your local path

cd $STD_ROS_DIR
source deve/setup.bash
roslaunch std_detector demo_pgo.launch

3.4. Example-4: online loop closure correction with FAST-LIO2 integrated

To run Example-4, you need to install and configure FAST-LIO2 first. You can try the data building_slower_motino_avia.bag here(provided by zlwang7), which is outdoor scan data with no loop closure other than the one between the starting point and the endpoint. Therefore, relying solely on the fast-lio algorithm results in obvious Z-axis drift, with STD loop detection and graph optimization, there will be a noticeable correction to the drift.

# termianl 1: run FAST-LIO2
roslaunch fast_lio mapping_avia.launch

# terminal 2: run std online demo
roslaunch std_detector demo_online.launch

# terminal 3: play data
rosbag play building_slower_motion_avia.bag

Acknowledgments

In the development of STD_detector, we stand on the shoulders of the following repositories:

• Scan Context: An Egocentric Spatial Descriptor for Place Recognition within {3D} Point Cloud Map
• FAST-LIO: A computationally efficient and robust LiDAR-inertial odometry package.
• VoxelMap: An efficient and probabilistic adaptive(coarse-to-fine) voxel mapping method for 3D LiDAR.
• R3LIVE: A Robust, Real-time, RGB-colored, LiDAR-Inertial-Visual tightly-coupled state Estimation and mapping package

Contact Us

We are still working on improving the performance and reliability of our codes. For any technical issues, please contact us via email Chongjian Yuan < ycj1ATconnect.hku.hk >, Jiarong Lin < ziv.lin.ljrATgmail.com >.

For commercial use, please contact Dr. Fu Zhang < [email protected] >

License

The source code of this package is released under GPLv2 license. We only allow it free for personal and academic usage. For commercial use, please contact us to negotiate a different license.

We are still working on improving the performance and reliability of our codes. For any technical issues, please contact contact us via email Chongjian Yuan < ycj1ATconnect.hku.hk >, Jiarong Lin < ziv.lin.ljrATgmail.com >.

If you use any code of this repo in your academic research, please cite at least one of our papers:

[1] Yuan, C., Lin, J., Zou, Z., Hong, X., & Zhang, F.. "STD: Stable Triangle Descriptor for 3D place recognition."
[2] Xu, W., Cai, Y., He, D., Lin, J., & Zhang, F. "Fast-lio2: Fast direct lidar-inertial odometry."
[3] Yuan, C., Xu, W., Liu, X., Hong, X., & Zhang, F. "Efficient and probabilistic adaptive voxel mapping for accurate online lidar odometry."

std's Issues

Dose this project support mid-360?

Hi, thank you for your great job about STD. Dose this project support mid-360 lidar?

LTA-OM会开源嘛

Replace the kd-tree in PCL with nanoflann kd-tree

Whether using nanoflann kd-tree instead of PCL library kd-tree in code will reduce the nearest neighbor search time and memory consumption.

有关角点提取一致性的问题

袁博您好，你们的工作很棒！我想请教一下有关角点提取一致性的问题？

背景是我现在在使用您的代码做一件另外的事情：不同时间的场景识别。主要任务是想要实现：用第一个时间采集的一个场景中的全部点云帧的STD描述子加入到数据库中，然后用另一个时间采集的同一场景的帧去与之前的数据库对比，实现场景重识别。

但我在测试中出现了一个现象，表现为对某一帧点云增加小幅度偏置后提取到的角点出现较大差异，比如把点云帧沿x方向整体偏移一个0.05米再重新提取角点，和未加偏置的提取角点结果差异较大，但当我把偏置设为1.0米或者1.0米的整数倍时，角点提取和未加偏置是一致的，不知道是否与参数voxel_size设置为1.0米有关吗，想请教一下您觉得原因可能是什么呢？

非常感谢您的工作！

The gradient calculation has a fetal bug.

Or it is my mistake

Question about the codes

I am a Master's student from Korea, and I would like to express my appreciation for sharing the code with the community. I have carefully studied the HKU-MARS studies, and I have a question regarding the extraction of corner points.

I would like to inquire about two aspects of the code:

1. In lines 780 to 805, I am not sure if the variables in this section are being utilized, as it seems that the code is using the maximum of img_count_array[x_index][y_index] instead of the gradient (line 807 to 834). Can you please confirm if this section of the code is indeed being used?

2. In lines 837 to 884, it appears that the code is filtering lines in the image from the voxel, and the values of neighboring pixels should be similar if a line is present. The condition (img_count_array[p1[0]][p1[1]] >= threshold && img_count_array[p2[0]][p2[1]] >= threshold) is used to find lines, not corner points. However, it seems that the code is repurposing the line filtering step to identify corner points. Therefore, I would like to ask if uncommenting the line in 856, which is "is_add = false," would be the right approach to filter out lines and retain only the corner points.

Thank you for your kind attention, and I look forward to your response.

Best regards,
Minwoo Jung

distort points

Hello, I would like to ask if this point cloud data has already been distorted before processing

错误配准

作者你好，我使用16线激光雷达跑STD，并绘制处闭环对的连线，发现会产生较多的错误闭环，如图蓝色的连线，想问一下，对于16线机械雷达是不是得将voxel size设置小一点，平面重合阈值icp_threshold设置大一些？

STD结果的PRcurve如何绘制？

place_recognition_kitti.cpp中将每帧点云都作为关键帧，输出的结果中还是存在部分帧并没有找到一个回环结果，难以用于prcurve的绘制，请问作者是如何绘制的呢？能否提供代码呢？

有关固态激光雷达PGO_demo的问题

袁博您好，你们的工作真是太棒了！我想请教一下pgo_demo.launch有livox版本的吗？我尝试修改了一下bag路径以及读取点云的方式，但是运行到后面会挂，里程计只有修正前的。想问一下会提供固态激光雷达的图优化版本吗？

有大佬知道为什么跑第四个例子时候，运行online_demo和fast-lio2,再play bag包时候出现process has died [pid 229204, exit code -11,进程崩了

有大佬知道为什么跑第四个例子时候，运行online_demo和fast-lio2,再play bag包时候出现process has died [pid 229204, exit code -11,进程崩了

When to open source

I am very looking forward to，thanks

Launch crash between PCL and GTSAM

Hello, thank you very much for your excellent open source work. I have read your paper, run through the three demo programs provided, and tested my own data set, and the results are great. So I want to implement the "loop closure correction on the Livox LiDAR dataset" demo like Example-3, but I meet a bug as follows.

Since I need to convert the point cloud from ros msg to pcl type, I tried to use the following code

pcl::fromPCLPointCloud2(pcl_pc, cloud);
pcl::fromROSMsg(*cloud_ptr, cloud);

But process has died [pid 6718, exit code -11, ......]appeared when launching node, and after debug I found that it happened in this line of code related to gtsam.
gtsam::noiseModel::Diagonal::Variances(Vector6);

After I remove the relevant code of pcl::fromROSMsg, the node can be launched normally, so I am very confused, I don’t know if it is an environmental problem, my environment is ros-melodic, ceres-solver-2.1.0, gtsam-release -4.1.1 as readme file suggested.

Thanks again.

Best regards :)

How to use STD with LIO-SAM?

Hi, thank you for your great job about STD. I am trying to use STD with LIO-SAM. Cloud you give me some advice. I put the odometry and keycloud in lio-sam to STD online detector. But the STD shows something wrong in RVIZ. The point cloud is drift.

Can you opensource bin files?

Thank you for sharing your code.
I'm trying to evaluate your method on avia datasets(park1 and park2), however, rosbags are not convenient to test for place recognition tasks. And when reading the messages and turned them to bin files, I found that messages were easily lost so it was hard to match the frames.
Is it possible that you could share the bin fiiles you used for testing STD on park1 and park2 datasets? I believe it would be useful for the society.

PR曲线-召回率较低

作者你好，我测试了你给的开源livox数据集，并且将关键帧位姿作为真值，运行后得到的精准率-召回率中精准率很高，接近1，但是召回率即使在score（平面重合百分比阈值）给0时也只有20-35之间，请问论文中的召回率时如何评测出来的。

数据集和launch文件中路径匹配问题

你好，请问一下demo_kitti.launch中的数据集是百度网盘里的那个数据集吗？刚接触不是很了解。

数据库哈希键

代码中的哈希键并没有用上论文中的角度，能说一下为什么吗？同时我发现边长排序算法有一定的代码错误，只能保证c是最长边

Ground-Truth of KA Urban East Dataset

Thank you for sharing your newest research! I've been following your works for a while, and I really admire your previous achievements.
I noticed that KA Urban East, the dataset used for testing your algorithm on Livox Horizon LiDAR, doesn't contain the ground truth poses according to its site(https://isas-server.iar.kit.edu/lidardataset/). It maybe silly to ask, but how did you generate the gt data?
Looking forward to your reply, and hope your paper be accepted soon!

In demo_pgo.cpp, line 122: cloud_vec.push_back(current_cloud), it will consume a lot of memory, So better to downsample it.

Issue with No Corner

Dear hku-mars team:

Thanks for your wonderfull work.

I'm trying to test STD on my own datasets with a velodyne16 lidar, and I meet a issue that I'm not achieving to resolve.
This is what I do:

1. I run a laserodm like floam, and publish the odom pose per scan.
2. Subscribe the laser data and odom, sync the laser and odom and cache the lidar in global frame as a keyframe(5m for a dense temp cloud) like the example.
   Then generate STDescs with the function GenerateSTDescs, but I find that the stds size: 0.

The cache keyFrame

search loop

Hello, thank you very much for your open source code. I studied it yesterday and found that it accumulates point searches every 500 frames, and every time it matches a point cloud, it matches ten frames. Isn't it supposed to match with the current frame point cloud? Why is the last ten frame point clouds used here? I don't understand. Thank you

在重定位中出现误匹配

第一次执行结果

第二次执行结果

地图（target）

当前局部地图（source）

使用局部在线地图与离线地图进行重定位，定位结果与实际不符，产生误匹配

使用STD重定位效果不佳

感谢开源优秀的工作，我在尝试使用STD进行重定位时候，在室内效果不佳，我猜测是因为我的地图是只有关键帧了，关键帧相比较实时建图，没有非关键帧的信息，导致提取角点和平面点的效果不好吗，是这个原因吗

Config parameters for benchmark

Hello @ChongjianYUAN,

I'm Minwoo Jung, a PhD student at Seoul National University.

We're currently working on a project that involves collecting LiDAR datasets and evaluating them using various LiDAR place recognition techniques. Upon reviewing the configuration in your repository, I've noticed some discrepancies. Some of the parameters differ from those presented in the paper, and there are several parameters not discussed in the paper.

To ensure a fair comparison, could you please provide the YAML file that was used in the paper? This would be immensely helpful.

Thank you!

Single frame registration problem！

Thank you very much for your excellent work！
I am trying to use STC to match the current scanned point cloud with a part of the point cloud established by Fastlio, achieving rapid map repositioning.

I extracted a point cloud within a range of 50 meters around a center point on the map and used one frame of the point cloud from that center point for operation.

The first step is to perform a self set translation and rotation transformation on a frame of point cloud, and then downsample the scanned point cloud and some point clouds:
down_sampling_voxel (* center_cloud, config_setting. ds_size_);
down_sampling_voxel (* onescand_cloud dev, config_setting. ds_size_);

Step 2, I generated a local point cloud descriptor and added it to STDescs:
std:: vectorstds_vec;
std_manager ->GenerateSTDescs (temp_cloud, stds vec);
std_manager ->AddSTDescs (stds vec);

Step 3: I will scan the point cloud to generate a descriptor
std:: vectorstds_vec_;
std_manager ->GenerateSTDescs (onescanc_cloud dev, stds vec_);

Finally, I attempted to use SearchLoop to obtain translation rotation and print
std_manager ->SearchLoop (stdsvec_, search_result, loop_transform, loop_std_pair);
std::cout << ANSI_COLOR_RED << "loop_Translation: " << loop_transform.first << std::endl;
std::cout << ANSI_COLOR_RED << "loop_rotation: " << loop_transform.second << std::endl;
std:: cout<<std:: endl;

I found that the translation is 0 0 0 0 and the rotation is the unit matrix.

More importantly, I performed translation and rotation transformations on the local point cloud and matched the results with itself, but still failed

How can I set parameters to increase the number of key points in a point cloud to improve registration accuracy? Or is my usage incorrect?

Looking forward to your reply!

Doesn't work for indoor dataset

I have some indoor data collected with Livox Lidar. I don't know why, but STD shows not good results here. I tried to play with parameters, but don't have any improvements.
You can see how it performs in a video below.
1RlhuzwF.webm

I launched demo_livox.launch with default parameters.
Modified place_recognition_livox.cpp to reproduce this result:
place_recognition_livox.zip

Issue with pose in own datasets

Dear hku-mars team:

First of all: congratulations for your wonderfull work.

I'm trying to test STD on my own datasets recorded with a Livox Horizon, and I found a issue that I'm not achieving to resolve.
To get the undistorted cloud and the poses I'm trying to use FAST-LIO2.

This is what I do:

• I run FAST-LIO2 having the tag scan_bodyframe_pub_en setted to true in the horizon.yaml config file
• I start recording a new bag file (out_FAST-LIO.bag) which stores all the topics using the -a tag.
• I then play the bag I had on my dataset collection (with the --clock tag enabled)

Finished the proccess I have the whole FAST-LIO2 topics in a bag file (I know it's not an efficient way to do it)
Then I need to proccess the next topics:

• "/cloud_registered_body" that is the undistorted cloud in "body" frame
• "Odometry" that containds the poses to extract the Time, p.x, p.y, p.z, o.x, o.y, o.z, o.w

So, to rename "/cloud_registered_body" to "/cloud_undistort" and set the frame to "camera_init" as in the example datasets, and extract the poses I did the next python script:

#! /usr/bin/env python

import sys
import os
import csv
import rospy
import rosbag
from nav_msgs.msg import Odometry
from rosgraph_msgs.msg import Clock

input_bag = '/home/developer/Downloads/out_FAST-LIO.bag'
bag_file = '/home/developer/Downloads/in_STD.bag'
outposes = '/home/developer/Downloads/outposes.txt'

def correct_topics():
    with rosbag.Bag(bag_file, 'w') as Y:
      for topic, msg, t in rosbag.Bag(input_bag):
          if topic == '/cloud_registered_body':
              new_msg = msg
              new_msg.header.frame_id = "camera_init"
              new_msg.header.seq = 0
              Y.write('/cloud_undistort', new_msg, t)
          # if topic in ['/Odometry', '/tf', '/tf_static', '/clock']:
              # Y.write(topic, msg, t)
          # Y.write(topic, msg, t)
    Y.close()


def log_poses():
  bag = rosbag.Bag(bag_file)

  with open(outposes, mode='w') as data_file:
    data_writer = csv.writer(data_file, delimiter=' ', quotechar='"', quoting=csv.QUOTE_MINIMAL)
    
    for topic, msg, t in bag.read_messages(topics=['/Odometry']):
      if msg.pose is not None:
        time = float("{0}.{1}".format(t.secs, t.nsecs))
        p = msg.pose.pose.position
        o = msg.pose.pose.orientation
        data_writer.writerow([time, p.x, p.y, p.z, o.x, o.y, o.z, o.w])

  bag.close()

print "Correcting the topics..."
correct_topics()
print "Correction Done"

print "Logging the poses..."
log_poses()
print "Logging done"
print "Finished"

This ouputs a correct poses file with some lines like the next:

1678546402.612144 -0.002096549697542764 -0.009372931544699327 0.002765338194950498 -0.0018071946941900683 -0.0006340698193831235 0.00020620311920631487 0.9999981447398163
1678546402.71254 -0.0006095704600163874 -0.027139210193813337 0.010113429629662413 -0.0011872611441451524 -0.0003852258571645211 0.00016939503815105857 0.9999992066583528
1678546402.8192487 -0.002305266949169554 -0.03239809492705823 0.01163492051121122 -0.0015048833301887408 -0.00069737258071356 0.00047112203218456196 0.9999985135197332
1678546402.9206886 -0.0035820286084963064 -0.026384161958860963 0.007936787840409036 -0.0033966968062800994 -0.0008083309116593871 -0.00019888650198328063 0.999993884729353
1678546403.1825767 -0.0017142880467184538 -0.030422082115569463 -0.001904878395690949 -0.002388874864515384 0.00010240077266197415 3.4486938168033673e-06 0.9999971413854485
1678546403.114134 -0.0053641625621179195 -0.027541924784114904 -0.003207783273881632 -0.0036762553729212054 -0.0008392987505604607 -0.0002636734540402738 0.9999928555746532
1678546403.2157943 -0.004732212569894099 -0.033894880123071056 -0.008441601047098044 -0.003305924280544573 5.1068253914301856e-05 0.00024885669105765716 0.9999945031484081
1678546403.3215632 -0.003170633947118756 -0.03906678157931448 -0.007492368391314982 -0.0031717868945264262 -7.919361677011604e-05 3.316393042852856e-05 0.99999496618554
...

I then run roslaunch std_detector demo_livox.launch configured to use the in_STD.bag and the outposes.txt obtained with the python script.
No errors, and everything in console looks fine, but on rviz the poses are not being applyed to the pointcloud, overlapping all scans without movement. Even it finds matches:

I don't know the cause and I'm loosing the perspective...

You can find the bags, the poses and the python script in the next GoogleDrive folder:
https://drive.google.com/drive/folders/1LZQAjIAIoE35cfyi_7Z5IxWl1W1ilotM?usp=sharing

If anybody can optimize the script or the workflow it shoud be great.

Thanks in advance

Update maybe?:)

Can you maybe update whole code to get work with our own dataset which we take from lidar avia and fastlio or r3live algorithm?:)

Best regards,
Antun Jakopec

pose file

Hello Dr Yuan, how is the pose file calculated for solid state LIDAR?

Save STD descriptors

Thanks for sharing the good work. I am wondering whether it is possible to add the way to save STD descriptors to files in the codes, and for another place recognition, reload STD descriptors from the files.

3d目标检测

Ouf of memory allocating 180908767704 bytes

When the config file is changed, fatal error occurs sometime. The content of error is that:
libgomp: Out of memory allocation Ouf of memory allocating 180908767704 bytes

It seems that some error happen with OpenMP. Anyone has advises to fix it?

About octree leaves

Hi，there

This work is excellent with detailed notes on codes as well. I wonder why you dont use the recut process which means splitting the octree root until it becomes a plane or reach the max depth. Is it useless and time-consuming? Are the fixed-resolution voxels enough for this task?

Thanks in advance.

能否提供一份参数说明？