Wheel Odometry Integration

[manomitbal]

Firstly, thank you for an amazing algorithm!

For a wheeled robot, what would be the best way to integrate wheel odometry instead of GPS in order to provide a better initial guess either imuPreintegration or another factor graph instead of lidar-visual-odometry for situations like in narrow corridors? Is that a valid idea? I was toying with providing a ukf generated nav::Odometry message with imu and wheel encoder data fused to maintain a factor graph. Since I have covariances for wheel encoder information, is there a way to extract Lidar Odometry covariances so that data could be fused? I look forward to your suggestions.

Thanks!

[narutojxl]

Maybe A Closed-form Estimate of 3D ICP Covariance, chapter 2 Covariance of a Minimization Algorithm could help to calculate laser odom given by scan-to-scan match, as in lego_loam. The analytical jacobians are all available in lego_loam, although they are not express with eigen matrix type. This is my personal idea, hope to useful. Look forward to author's reply.

[manomitbal]

Hi @narutojxl, I actually did dabble a bit with Normal Distributed Transforms where the algorithm provided hessian matrix, which when inversed provided covariances. This however is not an accurate representation of noise in scan matching and thus is unusable when trying to integrate within a tightly coupled filter.

[narutojxl]

Hi @manomitbal, if we calculate NDT correcttly, when the cost function tends to zero, the approximate hessian matrix should converge to the true hessian. Here are some my guess.

• Did you implement ndt yourself based on ndt's paper, or directlly use PCL ndt? If you implement it, there is a typo in the paper when calculating hessian, see here, but the PCL's version is correct.
• Is the number of iterations of the algorithm too few?
• Did there occur degenracy of the solution, if yes, we should update the result delta_x with paper On degeneracy of optimization-based state estimation problems.

[TixiaoShan]

If you have good wheel odometry, then you can replace imuPreintegration.cpp with your odometry. The purpose of imu preintegration is to provide a scan-matching initial guess. Then the initial guess is extracted here. There will be a lot of coding needs to be done to incorporate wheel odometry, but I think it will work well.

[manomitbal]

Hi @narutojxl thanks for your suggestions! I will surely try that approach. Previously, there were instances where the cloud containing too many floor points would converge incorrectly and get stuck in local minima during alignment, causing incorrect hessian and fitness score. This proved to be erroneous. I used PCL's correct version of NDT.

Thank you @TixiaoShan for your advice, I like the idea of tightly coupled IMU but simply having high rate of wheel odometry fused with IMU would be a good critical factor to add to the graph, I will try this out and see if it works out by replacing imupreintegration. Speaking of NDT, is there a benefit to add NDT based convergence and alignment for loop closure instead of ICP?

[narutojxl]

Hi @manomitbal, I am glad you found the cause of the problem. Thanks for your information :)

[stale[bot]]

This issue has been automatically marked as stale because it has not had recent activity. It will be closed if no further activity occurs. Thank you for your contributions.