Question about the IMU Propagation using Robocentric formulation

[Gatsby23]

Dear Researcher: I have detailed the code about《Robocentric Visual-Inertial Odometry》recently. I really appreciate the idea that using robocentric formulation to avoid the inconsistent problem and can avoid the initialization failure. With curiosity, I have detailed the code under the simulation gazebo environment. I use the husky-simulator to get the simulation of the IMU sensor. In order to understand the propagation in the robocentric formulation. I collect the true value of the IMU sensor without the disturbing of imu bias. Like below:
Use this setting, I have collect simulation data as below: 链接: https://pan.baidu.com/s/11zdWTpG0oJ1bfPKes9F8wQ 提取码: snjw --来自百度网盘超级会员v2的分享 （If you have problem with baidu disk, you can also download the data from:https://1drv.ms/u/s!Av8TDx3WXbi_gm4juVcof1GeClpE?e=OX4WsI ） Using the data, I have test the LINS and Fast-LIO algorithm with update closed and the result is fine. The Fast-LIO Result:

The LINS Result:

However, when I use the R-VIO code without the visual update, the result is weird:

I closed the update like follows:

I don't know why the imu pose drifts quickly under the R-VIO IMU propagation. Could you please give me some advice? I have also consider the gravity effect, but when I stable the gravity orientation, the result is not good too.

I'm very appreciate for your help, thank you very much. Yours Qi Wu

[huaizheng]

Here if the robot in the simulation is moving on the ground, then an easy way for targeting the issue is to first print out the robot acceleration in {G} (by rotating the accelerometer measurements, a, from {I} to {G}). Ideally, because of no vertical motions the accelerations in the z-axis of {G} should be constant, for example -9.8m/s^2. If not, then it means {G} is not aligned with gravity.

[Gatsby23]

Here if the robot in the simulation is moving on the ground, then an easy way for targeting the issue is to first print out the robot acceleration in {G} (by rotating the accelerometer measurements, a, from {I} to {G}). Ideally, because of no vertical motions the accelerations in the z-axis of {G} should be constant, for example -9.8m/s^2. If not, then it means {G} is not aligned with gravity.

Thank you for your replay. Follow your advice, I have checked that the gravity is aligned, the result is as follows:

[Gatsby23]

Dear Researcher: I have detailed the code about《Robocentric Visual-Inertial Odometry》recently. I really appreciate the idea that using robocentric formulation to avoid the inconsistent problem and can avoid the initialization failure. With curiosity, I have detailed the code under the simulation gazebo environment. I use the husky-simulator to get the simulation of the IMU sensor. In order to understand the propagation in the robocentric formulation. I collect the true value of the IMU sensor without the disturbing of imu bias. Like below: Use this setting, I have collect simulation data as below: 链接: https://pan.baidu.com/s/11zdWTpG0oJ1bfPKes9F8wQ 提取码: snjw --来自百度网盘超级会员v2的分享 （If you have problem with baidu disk, you can also download the data from:https://1drv.ms/u/s!Av8TDx3WXbi_gm4juVcof1GeClpE?e=OX4WsI ） Using the data, I have test the LINS and Fast-LIO algorithm with update closed and the result is fine. The Fast-LIO Result:

The LINS Result:

However, when I use the R-VIO code without the visual update, the result is weird:

I closed the update like follows:

I don't know why the imu pose drifts quickly under the R-VIO IMU propagation. Could you please give me some advice? I have also consider the gravity effect, but when I stable the gravity orientation, the result is not good too.

I'm very appreciate for your help, thank you very much. Yours Qi Wu

I also checked the bias again, I really sorry for bothering your time. But I don't know what's wrong with it. Could you please test the data ? Begging for your help

[huaizheng]

Did you run with the same data by enabling the update? What the result look like?

[Gatsby23]

Did you run with the same data by enabling the update? What the result look like? It's the same data[捂脸], and the result is not correct as well[捂脸]

[Gatsby23]

Did you run with the same data by enabling the update? What the result look like?

The thing bothering me is that why the clean imu data need update?

[huaizheng]

I just checked the data you sent me and I can find that the IMU data (in topic /imu/data) is not clean (corrupted by the noise). So, if you used it to do pure propagation, then the drift in your result is reasonable.

[Gatsby23]

I just checked the data you sent me and I can find that the IMU data (in topic /imu/data) is not clean (corrupted by the noise). So, if you used it to do pure propagation, then the drift in your result is reasonable.

I checked from the topic /imu/data/bias that the bias is 0.

This is the fast-lio pure propagation result without the lidar correction  .

[Gatsby23]

I just checked the data you sent me and I can find that the IMU data (in topic /imu/data) is not clean (corrupted by the noise). So, if you used it to do pure propagation, then the drift in your result is reasonable.

I also upload the data again and could you please test it again: 链接: https://pan.baidu.com/s/1HZtXcsjWpEiD3mc3cM29ug 提取码: m4p5 --来自百度网盘超级会员v2的分享

[huaizheng]

First of all, if the IMU measurements are corrupted by the noise, then it will definitely cause drift after pure propagation. If the robot is running on an ideal flat plane in the simulation, then its acceleration in z-axis should be constant, however, it is not in your data. IMU measurements are usually corrupted by sensor noise and biases, so you need to make sure that the noise is also zero when generating the data, or you will not have the "clean" IMU measurements. Please double check those things.

[Gatsby23]

First of all, if the IMU measurements are corrupted by the noise, then it will definitely cause drift after pure propagation. If the robot is running on an ideal flat plane in the simulation, then its acceleration in z-axis should be constant, however, it is not in your data. IMU measurements are usually corrupted by sensor noise and biases, so you need to make sure that the noise is also zero when generating the data, or you will not have the "clean" IMU measurements. Please double check those things.

The gazebo environment I have used is https://github.com/TurtleZhong/LVIO-SAM.git. The data collected is from the husky simulation tools. It's not an idea flat plane, because the mesh noise. I also check the paper that it doesn't claim that the collection platform should be on an ideal flat plane. I have checked the data again and again to make sure that the imu data bias is clean from the gazebo collection(Like the same group work cpi [https://github.com/rpng/cpi]). To make sure that the data is right, I have use the data sent into different IMU-based system to prove that the pure propagation on these data is fine.

To tell the truth. I really like the robocentric vio, because it's can start from arbitrary pose. From last years, I put all the effort to follow the paper and the code. I check all the issue and have talk with different people. But the system I developed is always drift. So I want to use the simulation to solve the bug. However, this time, I don't know what's really problem is. Maybe there have some things that can't explained. The pure propagation is always not right.

Sorry for bothering your time, thanks for your advice.

[Gatsby23]

I just checked the data you sent me and I can find that the IMU data (in topic /imu/data) is not clean (corrupted by the noise). So, if you used it to do pure propagation, then the drift in your result is reasonable.

I also upload the data again and could you please test it again: 链接: https://pan.baidu.com/s/1HZtXcsjWpEiD3mc3cM29ug 提取码: m4p5 --来自百度网盘超级会员v2的分享

I just checked the data you sent me and I can find that the IMU data (in topic /imu/data) is not clean (corrupted by the noise). So, if you used it to do pure propagation, then the drift in your result is reasonable.

I checked from the topic /imu/data/bias that the bias is 0. This is the fast-lio pure propagation result without the lidar correction  .

Sorry for wrong picture. Forgive me with long time no sleep, the test on fast-lio without update is like this:

[huaizheng]

Here if your simulated IMU outputs true (or totally clean) measurements, then by integrating them the estimator will output a trajectory close to the ground truth. I may have no time to investigate the other two works you presented here, but for my work the propagation only gives you the current relative pose estimate and the composition step will then use this result to recover global pose estimate. As we did simulation in our paper, we also used the generated true IMU measurements to verify our propagation+composition logic without enabling update. So, if you think your data is okay, then there is one thing that may still cause the drift which is about the magnitude of gravity in your simulation environment. You can find that in the config file the magnitude of gravity is 9.8082 which is for the EuRoC data, but when you test your own data you should change it to the value you simulated, otherwise such difference in the magnitude of gravity will introduce propagation errors.

[Gatsby23]

e

Is there something wrong I modify the code ? I only comment the code show above: the update process and the augmented process. If you have time, could you please test it on your laptop? I would appreciate it, If you could show me some analysis about which information is incorrect. About the gravity magnitude. The result on the above I think is accurate enough？I don't know how to change it at all（increase or decrease？because the velocity is increasing all the time）. Sorry for my stupid question.

[Gatsby23]

Here if your simulated IMU outputs true (or totally clean) measurements, then by integrating them the estimator will output a trajectory close to the ground truth. I may have no time to investigate the other two works you presented here, but for my work the propagation only gives you the current relative pose estimate and the composition step will then use this result to recover global pose estimate. As we did simulation in our paper, we also used the generated true IMU measurements to verify our propagation+composition logic without enabling update. So, if you think your data is okay, then there is one thing that may still cause the drift which is about the magnitude of gravity in your simulation environment. You can find that in the config file the magnitude of gravity is 9.8082 which is for the EuRoC data, but when you test your own data you should change it to the value you simulated, otherwise such difference in the magnitude of gravity will introduce propagation errors.

The two code I used is here:https://github.com/hku-mars/FAST_LIO.git and https://github.com/ChaoqinRobotics/LINS---LiDAR-inertial-SLAM. Only close the update and without parameters change

[goldbattle]

Please remember this is research code. We won't debug your own implementation. As mentioned before, it looks like you do not have the additive 9.81 in your accelerometer readings, which means the IMU readings are incorrect. I suggest you correct your simulation and then move on to test from there.

[Gatsby23]

goldbattle

I'm sorry, I haven't reimplment anything. I use the original R-VIO code and only close the update process on the system.cc. Like above. I'm sorry, but I don't know what is the additive 9.81 meaning? I notice that CPI（I really like the work, by the way）also collect the IMU data from the gazebo? Sorry to bother you, but what have you set to make sure that the imu is clean and useful for the test?

[Gatsby23]

Please remember this is research code. We won't debug your own implementation. As mentioned before, it looks like you do not have the additive 9.81 in your accelerometer readings, which means the IMU readings are incorrect. I suggest you correct your simulation and then move on to test from there.

I really thank you for all your help. I just can't figure out why and want to solve this problem.

[goldbattle]

You can see that in the CPI simulation, the IMU has 9.81 in it but with zero noise. If I remember correctly, you need to explicitly set gravity in the simulator. It has been years since then, so I don't remember the details on how we did this.

I'm sorry, I haven't reimplment anything. I use the original R-VIO code and only close the update process on the system.cc

Yes, but you are using your own data, and we have pointed out it is incorrect. Please try to be understanding.

[Gatsby23]

gravity in the simulator

Thank you very much, I'll set the gravity in the simulator and test it again. Thank you very much. Really sorry for bothering your time.

[Gatsby23]

You can see that in the CPI simulation, the IMU has 9.81 in it but with zero noise. If I remember correctly, you need to explicitly set gravity in the simulator. It has been years since then, so I don't remember the details on how we did this.

I'm sorry, I haven't reimplment anything. I use the original R-VIO code and only close the update process on the system.cc

Yes, but you are using your own data, and we have pointed out it is incorrect. Please try to be understanding.

Here is my parameter used in gazebo. I think maybe it's fine ? I use rostopic echo /gazebo/parameter_descriptions to figure out the parameter

[Gatsby23]

You can see that in the CPI simulation, the IMU has 9.81 in it but with zero noise. If I remember correctly, you need to explicitly set gravity in the simulator. It has been years since then, so I don't remember the details on how we did this.

I'm sorry, I haven't reimplment anything. I use the original R-VIO code and only close the update process on the system.cc

Yes, but you are using your own data, and we have pointed out it is incorrect. Please try to be understanding.

Dear Dr.Paul: Thank you for your advice. I have checked the data again. I found that the acc value from the IMU is always jumping. Maybe you think that is noise. However, the simulation environment is like this: The jumping value from the IMU acc maybe caused by the mesh. So I change the environment to an garage environemnt and collect the data again. This time the value is look fine and stable. Sorry I don't know how to upload the video to the github. But this time R-VIO is work-fine without drift. But last time data is really the clean data, maybe only have the "mesh noise". The gazebo simulation about IMU I used is the hector_gazebo_imu, so it's already have the gravity value. All in all, thank you for your advice for help me. Yours Qi WU