search

cartographer-project / cartographer

Cartographer is a system that provides real-time simultaneous localization and mapping (SLAM) in 2D and 3D across multiple platforms and sensor configurations.
Apache License 2.0
7.18k stars 2.25k forks source link

High-frequency optimized trajectories #1151

Open ojura opened 6 years ago

ojura commented 6 years ago

Hello everyone. I have been tasked with obtaining a high-frequency optimized trajectory from Cartographer - preferably with as high frequency as IMU measurements. Currently, cartographer_ros publishes high frequency poses in PublishTrajectoryStates, but these are just the local SLAM poses multiplied by the latest GlobalToLocal transform at the time of publishing. Accumulating these would give a high-frequency trajectory, but not fully optimized.

So the question comes down how to do interpolation of the trajectory. @pifon2a interpolates between trajectory nodes, but using only slerp for orientation, and not IMU measurements. We believe that better results would be obtained by using IMU measurements as well.

One approach that came to my mind (but am not really happy with it) is to have an additional optimization step. In this scheme, first the regular optimization would be run as usual. Then, a second run of optimization could be performed, where the regular nodes resulting from the first optimization would be frozen, and lots of additional nodes (corresponding to IMU measurements) would be inserted inbetween. These can be initialized to slerp poses, and then constrained using Ceres to follow IMU measurements, similarly to how regular nodes in regular optimization are constrained like so here.

Does anyone have any thoughts on this? Perhaps I should write an RFC?

gaschler commented 6 years ago

My first impression is that the additional complexity is probably not worth the potential benefit. I don't see how optimization on only the current nodes does little more than regularizing/smoothing the trajectory. If you want to make better use of IMU measurements, I think the most promising approach would be to do some optimization along these lines in the PoseExtrapolator. (However, implementation in local SLAM is risky because it easily delays the main thread.) BTW, all changes like these can have unexpected side effects so they need to done with great care.

ojura commented 6 years ago

Optimizing the nodes (which define the "low-frequency" trajectory) is what Cartographer currently does, and I don't want to make invasive changes to that. I was thinking of adding something like GetDenseTrajectoryNodePoses to PoseGraph, and it would take the existing optimized trajectory and give you the denser version which would make use of IMU measurements. A potential user for this would be the assets writer.

I also don't want to change how local SLAM works, so I don't think this should go into the PoseExtrapolator. Perhaps PoseInterpolator?