Issue with pose in own datasets

[EloyAldao]

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

[ChongjianYUAN]

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

Thank you for your attention and testing. I have checked the pose file and in_STD.bag. It seems like the timestamp of the point cloud recorded by the bag is inconsistent with the timestamp of the pose.

To make sure the timestamps are correct, you can refer to my approach, I directly recorded the point cloud and save the pose by modifying the code of FAST-LIO2.(see the following pic)

I
My modified file is in the appendix
laserMappinp.txt
, if you need to use this function, you can search the ‘‘write_rosbag‘‘’’ keyword for more details

[EloyAldao]

Thank you very much!!
You are very kind for answering my question.

This worked well.
After replacing the laserMapping.cpp and some tweaking on the code (there were some params not included in the current FAST-LIO release, as the localization toogle, and I had the LZ4_streamDecode error) I succeded to compile and test with the original bag.

Back to STD, with the bag and poses created with the new FAST-LIO (they are in the same Google Drive folder as my previous tests under the "FAST-LIO_Proccesed" folder )the map builds well.
I tweaked the params in "config-livox.yaml" to get the maximum effect from STD.

Here are my params:

# pre process
ds_size: 0.05
maximum_corner_num: 5000

# key points
plane_detection_thre: 0.001
plane_merge_normal_thre: 0.1
voxel_size: 1
voxel_init_num: 10
proj_image_resolution: 0.25
proj_dis_min: 0
proj_dis_max: 5
corner_thre: 10

# std descriptor
descriptor_near_num: 500
descriptor_min_len: 0.2
descriptor_max_len: 100
non_max_suppression_radius: 2
std_side_resolution: 0.2

# candidate search
skip_near_num: 1
candidate_num: 500
sub_frame_num: 100
vertex_diff_threshold: 0.8
rough_dis_threshold: 0.015
normal_threshold: 0.2
dis_threshold: 0.5
icp_threshold: 0.2

It finds enough candidates and it's matching well, but once the proccess adds the last scan in the bag, the matching proccess stops, as in the next image:

The white triangles are from the last scan, while the greens are still matching from a previous zone. The red points are the acumulated KeyPoints.

Is it correct that the matching proccess stops as soon as the last scan is added?
This means that in large and complex datasets and with lots of keypoints to match the matching proccess is never completed.
Am I correct?

Thanks in advance

[ChongjianYUAN]

Thank you very much!! You are very kind for answering my question.

This worked well. After replacing the laserMapping.cpp and some tweaking on the code (there were some params not included in the current FAST-LIO release, as the localization toogle, and I had the LZ4_streamDecode error) I succeded to compile and test with the original bag.

Back to STD, with the bag and poses created with the new FAST-LIO (they are in the same Google Drive folder as my previous tests under the "FAST-LIO_Proccesed" folder )the map builds well. I tweaked the params in "config-livox.yaml" to get the maximum effect from STD.

Here are my params:

# pre process
ds_size: 0.05
maximum_corner_num: 5000

# key points
plane_detection_thre: 0.001
plane_merge_normal_thre: 0.1
voxel_size: 1
voxel_init_num: 10
proj_image_resolution: 0.25
proj_dis_min: 0
proj_dis_max: 5
corner_thre: 10

# std descriptor
descriptor_near_num: 500
descriptor_min_len: 0.2
descriptor_max_len: 100
non_max_suppression_radius: 2
std_side_resolution: 0.2

# candidate search
skip_near_num: 1
candidate_num: 500
sub_frame_num: 100
vertex_diff_threshold: 0.8
rough_dis_threshold: 0.015
normal_threshold: 0.2
dis_threshold: 0.5
icp_threshold: 0.2

It finds enough candidates and it's matching well, but once the proccess adds the last scan in the bag, the matching proccess stops, as in the next image: The white triangles are from the last scan, while the greens are still matching from a previous zone. The red points are the acumulated KeyPoints.

Is it correct that the matching proccess stops as soon as the last scan is added? This means that in large and complex datasets and with lots of keypoints to match the matching proccess is never completed. Am I correct?

Thanks in advance

A match is made each time this function "std_manager->SearchLoop(stds_vec, search_result, loop_transform, loop_std_pair)" is called. By the way, The descriptor_near_num value(500) in your config is too large, resulting in too many STDs generated, it is recommended to set it to 10--20. Besides, skip_near_num is recommended to be set to 30--50, otherwise the adjacent frame will be matched as a loop frame.

[hr2894235132]

想请问一下，rosbag包的雷达话题必须是"sensor_msgs/PointCloud2"吗？“livox_ros_driver/CustomMsg”话题貌似不能成功加载（我把这个types.push_back(std::string("sensor_msgs/PointCloud2"));改成types.push_back(std::string("livox_ros_driver/CustomMsg"));也没用）

[hr2894235132]

想请问一下，rosbag包的雷达话题必须是"sensor_msgs/PointCloud2"吗？“livox_ros_driver/CustomMsg”话题貌似不能成功加载（我把这个types.push_back(std::string("sensor_msgs/PointCloud2"));改成types.push_back(std::string("livox_ros_driver/CustomMsg"));也没用）

按照袁博您上面提供的方法保存了我的bag文件以及位姿文件。出现了下面的问题，好像并没有有效点云，袁博有空的时候可以帮我解答一下吗？

下面是我的bag文件以及位姿文件信息：

刚又用较大的数据集测试了下，将proj_image_resolution改为0.5，检测到一些回环，但是还是不是很多，猜想是数据集小或者点云数量过少的问题吗？