vectr-ucla / direct_lidar_inertial_odometry Goto Github PK

Hi, i recently read the papers of your group and i got some questions.
The first is in the paper "Direct LiDAR-Inertial Odometry and Mapping: Perceptive and Connective SLAM". I wonder that how did you add that connectivity factor into your factor graph? did you just calculated a relative pose using the result from front-end odometer when two these two non-adjacent keyframes having sufficient overlap and used your 3D Jaccard index calculated a noise?
The second is in the paper "Joint On-Manifold Gravity and Accelerometer Intrinsics Estimation for Inertially Aligned Mapping". Did the gravity factor you calculated was only used in the mapping stage and not added into the factor graph you mentioned before? And i also curious that you projected the gravity and covariance calculated in factor graph in inertial frame to body frame, and projected the fixed gravity in inertial frame to body frame, then used these two gravity as error function, why don't you just calculated the error in the inertial frame?
Thank you for your contribution of open-sourceing your code to the community. Looking forward to your reply.

Hey! Firstly, thanks for great paper and code!
I used this tool for intrinsics and extrinsics: https://github.com/hku-mars/LiDAR_IMU_Init
I need help on understanding how to implement those values for DLIO:

Initialization result:
Rotation LiDAR to IMU (degree) = -0.084620 -0.649203 -1.151916
Translation LiDAR to IMU (meter) = -0.056023 -0.021736 0.026451
Time Lag IMU to LiDAR (second) = -0.010057
Bias of Gyroscope (rad/s) = -0.007985 -0.012948 0.020327
Bias of Accelerometer (meters/s^2) = 0.010086 0.009814 0.010098
Gravity in World Frame(meters/s^2) = -0.027753 0.030412 -9.809914

Homogeneous Transformation Matrix from LiDAR to IMU:
0.999734 0.020119 -0.011298 -0.056023
-0.020101 0.999797 0.001704 -0.021736
0.011330 -0.001477 0.999935 0.026451
0.000000 0.000000 0.000000 1.000000

Refinement result:
Rotation LiDAR to IMU (degree) = -0.014112 -0.341810 -1.171677
Translation LiDAR to IMU (meter) = -0.042382 -0.019593 0.020312
Time Lag IMU to LiDAR (second) = -0.010057
Bias of Gyroscope (rad/s) = -0.008959 0.001204 0.013092
Bias of Accelerometer (meters/s^2) = 0.013954 0.009286 -0.020293
Gravity in World Frame(meters/s^2) = -0.029866 0.030184 -9.791937

Homogeneous Transformation Matrix from LiDAR to IMU:
0.999773 0.020448 -0.005959 -0.042382
-0.020446 0.999791 0.000368 -0.019593
0.005965 -0.000246 0.999982 0.020312
0.000000 0.000000 0.000000 1.000000

Btw, using livox mid-360

Thanks a lot for opening source such a great work, but when I run the code, I found that the odometry output pose has always been the origin and has not changed. I have checked the data topics of lidar and imu, and there is nothing wrong. The frequency of lidar is 10Hz, and the frequency of imu is 200Hz. What are the possible reasons for the above case? Looking forward to your reply~

The geometric observer in your paper is derived from contraction analysis, which is beyond my comprehension. As a mediocre SLAMer, I know a bit about calculus, linear algebra, probability, optimization and lie theory. However, when I asked chatgpt how to learn contraction analysis, it started talking about ODE, functional analysis, dynamic systems and control theory. What is the minimal knowledge I need from these areas to understand your paper A Contracting Hierarchical Observer for Pose-Inertial Fusion? What tutorials would you recommend?

Under the livox branch: when I run rosservice call /robot/dlio_map/save_pcd 0.1 ~/pcd/campus2, the following error occurs:

Hello,

First of all thanks for a great paper and code, very easy to use and produced great results!
I just wondered where I can set it to use a constant number of cpu cores? When downsampling the pointcloud I of course got lower computation time but the number of cpu cores used and cpu load also decreased, to further decrease computation time I would like to increase the number of cpu cores used. I tried changing the num_threads variable but that didn't change anything in the debug output, any idea of where I could change this or if this even is an option?

When dlio::OdomNode::publishCloud(published_cloud, T_cloud) is called with published_cloud having a width of 0, the odometry node will crash due to a propagated division by zero error. This happens because publishCloud calls the following method with the copy_all_fields argument set to true (by default):

pcl::transformPointCloud (const pcl::PointCloud<PointT> &cloud_in,
                     pcl::PointCloud<PointT> &cloud_out,
                     const Eigen::Matrix<Scalar, 4, 4> &transform,
                     bool copy_all_fields)
{
...
  if (copy_all_fields)
        cloud_out.assign (cloud_in.begin (), cloud_in.end (), cloud_in.width);
      else
        cloud_out.resize (cloud_in.width, cloud_in.height);
...
}

This method furthermore calls the following assign method:

template <class InputIterator>
inline void
assign(InputIterator first, InputIterator last, index_t new_width)
{
  points.assign(std::move(first), std::move(last));
  width = new_width;
  height = size() / width;
  if (width * height != size()) {
    PCL_WARN("Mismatch in assignment. Requested width (%zu) doesn't divide "
             "provided size (%zu) cleanly. Setting height to 1\n",
             static_cast<std::size_t>(width),
             static_cast<std::size_t>(size()));
    width = size();
    height = 1;
  }
}

The division by width results in an uncaught division by zero error. This is easily fixed by calling pcl::transformPointCloud with copy_all_fields set to false.

Just a quick question: which app/lib/package was used to create the point cloud visualizations? These look very nice.

Hi,thank you for publishing a so good lio framework!
I have some questions about the timestamp in function integrateImu().I think the start_time means the first point's timestamp, but you use prev_scan_stamp (the medium timestamp of last scan) in function deskewPointcloud(). Could you please explain?

Hey @kennyjchen,

Thank you for your work on DLIO, which I have recently tested and found to be highly effective. However, I've noticed that DLIO lacks the "save_traj" service available in DLO. This functionality is essential for my current project. Could you please let me know why it's missing in DLIO and how I can add it to the platform? Your help is greatly appreciated.

Hi, dear author!
Thank you for your work! I have tested the program with your example data, it runs very well.
I am interesting in the situation when spinning lidar is in a very aggressive motion. So I tested your program with another data with aggressive motion in staircase. Here it is. https://drive.google.com/drive/folders/1f-VQOORs1TA5pT-OO_7-rG0kW5F5UoGG
I just tested the 2022-08-30-20-33-52_0.bag. The odometry has a very great drift at the very beginning.
I believe there's something wrong with the extrinsic parameters, so I try serveral times. It turns out that this parameter may be approximate as the picture shows below.

But, still it gets drift after some seconds when walking into a very narrow space.
Should I set the accurate extrinsic parameters, or something is wrong with this aggressive motion in narrow space?
By the way, how do you define the extrinsics baselink2imu and baselink2lidar? Do you mean baselink2lidar as P_L = R*P_b2l + t?

Hello author,
I am delighted to see that you have open-sourced this work. I tested some data right away and it worked very well. I am interested in the positive constants in the geometric observer. How is it set up? Does it relate to the usage scenario?
Thanks again for your outsanding contribution!

For example, let's focus on this->imu_stamp:

Its value is set in callbackImu here:

And then it is used as timestamp for published messages in the following callbacks:

• in publishPose (timer callback)
• in callbackCloud (lidar sensor data callback)

Since everything runs in parallel in multiple thread, you get a race condition. For example, by the time the latest LIDAR scan has been processed and is ready to be published, many IMU messages may have been received already, thus causing the published LIDAR stamp look further in time than it actually is.

The consequence is that some of the data DLIO publishes is not reliable.

Hi! I have tested your algorithm using Dataset 5: HK-Data20190117 from the UrbanLoco dataset. However, I noticed significant cumulative errors when the vehicle returns to the starting point. Below are the parameters I used. Could you please advise on how to adjust them for better performance? Thank you.

dlio:

  frames:
    odom: odom
    baselink: base_link
    lidar: lidar
    imu: imu

  map:
    waitUntilMove: true
    dense:
      filtered: false
    sparse:
      frequency: 1.0
      leafSize: 0.25

  odom:

    gravity: 9.80665

    imu:
      approximateGravity: false
      calibration:
        gyro: true
        accel: true
        time: 3
      bufferSize: 5000

    preprocessing:
      cropBoxFilter:
        size: 1.0
      voxelFilter:
        res: 0.25

    keyframe:
      threshD: 1.0
      threshR: 45.0

    submap:
      useJaccard: true
      keyframe:
        knn: 10
        kcv: 10
        kcc: 10
    gicp:
      minNumPoints: 64
      kCorrespondences: 16
      maxCorrespondenceDistance: 0.5
      maxIterations: 32
      transformationEpsilon: 0.01
      rotationEpsilon: 0.01
      initLambdaFactor: 1e-9

    geo:
      Kp: 4.5
      Kv: 11.25
      Kq: 4.0
      Kab: 2.25
      Kgb: 1.0
      abias_max: 5.0
      gbias_max: 0.5

we want to use dlio on agv,and we have stable encoder odom,how to apply it on dlio?

Hello author,
Thanks for sharing your excellent work! I have some trival questions.
Q1: After firstly process imu after this->imu_calibrated = true;, it seems this->prev_imu_stamp is equal to imu->header.stamp.toSec(), not the default zero value ?

Q2: ros odometry linear velocity is in some body frame, not in a world frame this->state.v.lin.b?

Hey there,
when I compile dlio (and dlo) with clang, the average computation time more than triples in comparison to using gcc ( ~100ms instead of ~30ms for an Ouster OS0 128).
I tried this on a AMD Ryzen 7 5800H with ubuntu 20.04 using clang 12.0 and gcc 9.4.
With clang, the CPU was running at full core utilization (x16) and a CPU load around 99% while dropping around 30% of the scans.
With gcc, the CPU runs at around 8% and slightly above 1 CPU utilized.
Any idea were this problem may come from?

Best regards, Jan

Hello author, first of all, thank you very much for such an excellent work！ I am currently testing high-speed sports scenes when there is a large drift, I use MID360, the trajectory is about 700m, and the speed is about 12m/s.Previously, It used to run well in slow motion.Don't know if you want to adjust some parameters? Hope you can give me some advice.

best wishes!

Would you please update the download link？Thanks a lot!

great job! thanks for sharing!

1. when i read the paper, i found the part of loop closure, but not found during the code . will it be released in future?
2. if add gps positon as prior factor, the way of lio-sam based on gtsam will be better?

In DLIO, local gravity value is a fixed parameter and can be used to estimate the initial orientation/bias of the robot.

However, most IMU sensors do measure and report gravity, for example something like this: acc={x=0.1, y=0.2, z=9.76}. Thus the gravity value can be calibrated automatically like so: gravity = (avg_acc - initial_bias).norm()), and it is not necessary to set the gravity value anymore

What do you think?

Hello author, thank you for your open source. Can DLIO be implemented using livox+px4?

If you play a rosbag, pause it for a few seconds, then resume it, the robot pose will suddenly jump away, as if the robot had been moving during the pause.

Or at least it is the case with the feature/ros2 branch.

Hello, Kenny!

Thank you for your remarkable work. I am currently working on adapting HESAI (Pandar) LiDAR and encountering difficulties progressing with the point cloud distortion correction. Can you please provide some guidance or suggestions on how to resolve this issue?

The terminal log is as follows.

I have added some log statements to pinpoint the problem, and it seems like the LIDAR timestamp is less than the latest timestamp in the IMU buffer. My IMU is a 6-axis one operating at 100Hz, and I understand the characteristics of the HESAI point cloud timestamp. Regarding the parameters, I have only modified the extrinsic parameters and the related topics.

Could you please provide some insights or suggestions on how to address this issue? Any help would be greatly appreciated.

hello,good lifes for yours!

this->T = this->T_corr * this->T_prior

why matrix T_corr need mutiply current T prior? T_corr is the relative transformation to submap, T_corr is not current global pose?

Hi,
Thank you for your work!
I found that a guess transformation matrix was passed in for the s2m gicp align function in DLO, while DLIO transforms each new scan pointcloud into global frame for GICP. Does this have any impact on the accuracy or efficiency of GICP solving? Also, are the GICP used by these two algorithms exactly the same?:D

First, Thank you for this amazing work.
Please I didn't understand the leaf_size var in the rosservice command that download the pcd file,
If I want to save the dense map, what is the number of leaf_size to put in the command ? I tried 0.01 and the pcd file downloaded isn't like the dense map in rviz I don't know why.
Thank you again.

Great work! While studying your code, I noticed that the submap generated by performing ICP with each frame of point cloud seems to be generated based on the estimated state from the previous frame, rather than being generated based on the predicted state of the current frame. Specifically, it appears that the "getNextPose" function is executed before the "buildKeyframesAndSubmap" function. This seems to deviate from what is described in the paper. I would like to kindly ask if my understanding is correct. If so, what are the advantages of doing it this way? Thank you very much in advance for any response you can provide.

Hi! I was trying to test DLIO (ROS2 branch) algorithm with a TurtleBot3 Burger in a simulated scenario for educational purpose. I am using Ubuntu 20.04 and ROS2 Foxy as ROS2 distro. However, I got into some troubles and I am currently trying to solve them.

1. If I understood correctly, the extrinsic parameters (i.e. baselink2imu and baselink2lidar) should be equivalent to the base_link->imu_link and base_link->base_scan transforms that are defined in the .xacro file. Am I right?

2. What I wish to obtain is a reference frame map as in the tf tree below (to do so, I used a static transform publisher but other packages such as Cartographer provide a better transform, hence I tried both). That is, I wish my fixed frame to be map instead of odom. My idea is to change the reference frame point clouds are published to, from odom to map.
   
   However, if I launch DLIO I not only get a big disalignment between the built map and the actual scan. It also sometimes happens that reference frames "jump". Here I took some screens from RViz and Gazebo.
   
   
   (The purple point cloud is the current scan, while the red one represents the map)

3. A last problem concerns the position that is computed with respect to the origin. I see that, even if the robot does not move, the position somehow oscillates. Furthermore, the trajectory is badly computed. Is this related to IMU calibration or is it related to the reference frame issue?
   EDIT: Terminal shows that the position significantly changes but the traveled distance is still 0. However, if I move around the robot and then I stop it, the position keeps changing and the traveled distance strangely increases.

I may test with a real copy of TurtleBot3 Burger tomorrow and check if I encounter the same problems.

Hi,
thanks for open-sourcing this great work :)
I'm trying to run your approach on the 2021 Hilti Challenge Dataset using the livox lidar. I tried different sequences, the both IMUs (/alphasense/imu & /alphasense/imu_adis) as well as different extrinsics (z up, z down), but it always seems to diverge instantly, despite the sensor being static in the beginning of the scenes. I'm using the default parameters of DLIO, except of the adapted extrinsics. I also tried disabling deskewing, but it didnt show any effect.

I would appreciate any suggestions on what has to be changed to make it work.

Hi, and thanks for open sourcing your work!

I assume your extrinsics are tailored to the OS1-64 and taking from the datasheet/driver, correct? Does base_link, your robot frame, refer to the sensor frame of the ouster driver p. 14p?

I think this code produces NaNs or undefined behavior, if the time difference is 0.

From the reference:

If the second operand is zero, the behavior is undefined, except that if floating-point division is taking place and the type supports IEEE floating-point arithmetic (see std::numeric_limits::is_iec559), then:
- if one operand is NaN, the result is NaN
- dividing a non-zero number by ±0.0 gives the correctly-signed infinity and FE_DIVBYZERO is raised
- dividing 0.0 by 0.0 gives NaN and FE_INVALID is raised

The straightforward solution would be to check for 0 before adjusting imu_rates. I am not sure if that would break some other parts of the code that depend on imu_rates.

I can create a PR and test with the provided data if you agree this could be an issue.

I got a memory corruption while running it in ros2 humble for around 30 mnitutes.
The output is below
[dlio_odom_node-4] double free or corruption (!prev)

I have never faced this issue so not sure if is reproducible.
I will post it here one I face the same issue again.

I have some questions about this section of updateKeyframes():

To my understanding, there are some possible issues here:

rotational exploration keyframing

// rotational exploration keyframing 
if (abs(dd) <= this->keyframe_thresh_dist_ && abs(theta_deg) > this->keyframe_thresh_rot_ && num_nearby <= 1) { 
  newKeyframe = true; 
} 

is always overwritten by

 if (abs(dd) <= this->keyframe_thresh_dist_) { 
   newKeyframe = false;
}

num_nearby has no effect

If this statement

 if (... || abs(theta_deg) > this->keyframe_thresh_rot_) { 
   newKeyframe = true; 
 } 

is true (from the second condition),

// rotational exploration keyframing 
if (abs(dd) <= this->keyframe_thresh_dist_ && abs(theta_deg) > this->keyframe_thresh_rot_ && num_nearby <= 1) { 
  newKeyframe = true; 
} 

this statement doesn't change newKeyFrame, even if num_nearby is larger than 1. At this point, abs(dd) <= this->keyframe_thresh_dist_ is always true, given that abs(dd) > this->keyframe_thresh_dist_ was false in the condition before.

Intended logic

What is the intended logic here? I couldn't find much about this in your papers, except the adaptive thresholding distance to account for tight spaces.

Without the rotational exploration keyframing, the logic is clear to me (roughly):

if abs(dd) <= this->keyframe_thresh_dist or abs(dd) > 0.5 -> return false
else if abs(dd) > this->keyframe_thresh_dist -> return true

Now, I don't quite understand why the keyframe are updated when a large rotation was registered. In a scenario where, let's say, the robot stays in the same spot, but turns 180deg. Why do I have to update keyframes? The map is rotation invariant, so to speak, so I have gained no new information about the environment just be turning. I guess this is what was meant with the following statement in the DLO-paper?

Keyframe nodes are commonly dropped using fixed thresholds (e.g., every 1m or 10◦ of translational or rotational change)

Thanks for you help, happy to discuss

The propagateState is called every time an IMU message is received, while updateState update the state using Geometric Observer after scan to map. In Geometric Observer, errors between propagated and measured state are computed to perform state correction.

The timestamp of propagated state is at latest IMU timestamp(1678822355.936), while measured state is at Lidar timestamp(1678822355.870) if no motion correction, or median time of the scan(1678822355.870+0.025).

I think propagated state this->state and measured state pin/qin should describe state at the same timestamp.

Despites their name, both baselink2imu and baselink2lidar are used as imu->baselink and lidar->baselink transformations in the code.

For example, this line:

converts IMU angular velocity measurement into baselink frame.

Another example:

Here, the original LIDAR scan (so in lidar frame) is first transformed by baselink2lidar, as if it was "lidar->baselink".

Not only it makes the code confusing, but it is also easy to miswrite the transforms in the dlio.yaml file. I think it would be better to rename these transforms to lidar2baselink and imu2baselink:

$ grep -rIl baselink2lidar | xargs sed -i 's/baselink2lidar/lidar2baselink/g'
$ grep -rIl baselink2imu | xargs sed -i 's/baselink2imu/imu2baselink/g'

?

What is the point of the branching here:

I ran DLIO on some sample data and I noticed kf.second->points.size() == kf.second->width * kf.second->height is always true. So is there any need for a branching?

i have tried my best to find a solution, but it doesn't work.
It seems that the problem occurs only in foxy, but i need foxy not humble.
Did you meet this problem?

Hello Guys!
Where to get the ground truth of your provided datasets?

Hi,
thanks for sharing your great work!

I wanted to download the sample data but it seems that those files have been moved. Would you be so kind and provide the updated URLs to download the data?

Thank you!

Two bugs within the following code:
if (!std::filesystem::is_directory(p)) {
std::cout << "Could not find directory " << p << std::endl;
res->success = false; // should be res.success = false;
return;// should be return false;
}