Add node marginalization support

hidmic commented 2 years ago

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	Second half of #407
Primary OS tested on	Ubuntu
Robotic platform tested on	CPR's Jackal gazebo simulation [*]

[*] See gist for instructions on how to replicate it.

Description of contribution in a few bullet points

Exposed Ceres solver information matrix for covariance estimation of marginalized distributions.
Added node marginalization support, based on a Chow-Liu tree approximation to Karto's graph.
Modified experimental lifelong mapping node to optionally use node marginalization (instead of plain removal).

Description of documentation updates required from your changes

Added new parameter, so I probably need to add that to the documentation page.

Future work that may be required in bullet points

This patch essentially reverts https://github.com/SteveMacenski/slam_toolbox/pull/445. I haven't run into issues myself, but I'm not entirely sure I follow what the failure mode was.
Testing, lots of testing. This has been manually tested on a simulation only. It must be validated on a real mobile robot.
Port to ROS 2. The only reason this patch targets ROS 1 Noetic is third-party simulation availability.

SteveMacenski commented 2 years ago

This is frankly so much code, I cannot reasonably review it. I'm not really sure what I can promise to be done here. There's enough going on and I don't have the time to do the background research to understand what you've done to critically review this. Is there someone that could take a look at this that would better understand and be able to provide technique and software reviews?

I've long since moved on from 2D SLAM into navigation world so this is also unfamiliar to me at this moment.

hidmic commented 2 years ago

This is frankly so much code, I cannot reasonably review it. I'm not really sure what I can promise to be done here.

Assuming that LOC count is the problem here, I think it would be possible to split this. At least to some extent.

Is there someone that could take a look at this that would better understand and be able to provide technique and software reviews?

Hmm, good question. Perhaps I can ask folks over at Nav2's Slack? Or anybody at the Nav2 WG?

I've long since moved on from 2D SLAM into navigation world so this is also unfamiliar to me at this moment.

That's fair. It is a bit intricate, that's true.

SteveMacenski commented 2 years ago

It's not just LOC but also what's happening in those LOC. Though the length certainly doesn't help :laughing:

No one that I can think of, there's not many people familiar with this codebase or the intricacies of SLAM, unfortunately.

hidmic commented 2 years ago

@SteveMacenski I've reached out to @Shokman to help review this contribution. He's a former co-worker and a friend with expertise in the area and no ties to Ekumen Inc. as of today. He has kindly accepted to do so.

If you're onboard, I'll close this PR, split it in 2 or 3 smaller PRs, and kick start the review.

SteveMacenski / slam_toolbox