Issue of trajectory drifting at U-Turn
radeonwu opened this issue · 6 comments
Hi, kailaili and other users,
I ran into the issue of trajectory drifting at U-Turn situations. What happens can be illustrated by the example below, where the white dotted line is /trajectory topic from BackendFusion, and the green dotted line is the odom /path topic from LidarOodmetry. So after the 180 degree of U-turn on the road, the white trajectory starts to drift away from the actual path(meanwhile, the lidar pointcloud's orientation shown in rviz also shift away from the trajectory), while the odom path is still roughly stays.
I tried to trace a bit in the BackendFusion code, and the /trajectory topic is from the variable "pose_each_frame", which is further updated in function buildLocalPoseGraph() and optimizeLocalGraph().
I collected quite a few dataset using horizon, and this drift happens at most of U-turn cases. Appreciate any help on the reason analysis and possible solutions.
I suppose that you use the internal IMU? As discussed before, lili-om has been developed with the external Xsens IMU, which gives more accurate inertial measurements. Also, we are unsure about how you exactly modified lili-om to run your own recorded data sets.
The Horizon itself has a limited FoV, thus the overlapping landmarks between frames could be insufficient if your u-turns were very fast and the IMU readings were not enough accurate. You could try to tune some parameters, e.g., those regarding feature extraction, downsampling or local map width, etc. There might be a chance of better results if more landmarks are used in the backend fusion in your case.
@kailaili,
Thanks for your reply.
I am using an external 6-axis IMU instead, outputting at 100Hz. Code wise, I didn't change except remapping the imu topic. The same setting works mostly for testing without U-turn. A rqt_graph screenshot is attached for reference.
The most curious part to me is that the /trajectory from BackendFusion drifts, but the odom /path from LidarOodmetry doesn't. Please see the full screenshot below from the same testing case.
You definitely need to tune the parameter as you used a different IMU and also had a different scene. There's nothing strange with the nonconvergence in the backend fusion. It was just that the optimization didn't converge given your setup when fusing with IMU readings. The lidar odometry from the frontend provides the odometry estimates fast. It's less accurate but usually stable as the optimization less nonlinear compared with the case fusing with the inertial measurements.
hi, kailaili,
Thank you for the points. Yeah, I will still try tuning parameters to see if lucky enough.
For the lidar odometry, I am just wondering, if we combine it with loop closure? will this be a more straightforward solution for lidar mapping?
You can do that to have some pure lidar odometry system of course. But that is then some lego-loam-like system, which is according to our experience, not as good as tightly-coupled lidar-inertial odometry system.
Hi @radeonwu , did you manage to tune your parameters such that it works? I also encounter this issue where the optimisation diverges at a u-turn. If it works, do you mind sharing the parameters?