Information-theoretic lifelong mapping

[hidmic]

Feature description

As an alternative to SLAM Toolbox's lifelong mapping support (still experimental, as #101 suggests), implement:

H. Kretzschmar, C. Stachniss and G. Grisetti, "Efficient information-theoretic graph pruning for graph-based SLAM with laser range finders," 2011 IEEE/RSJ International Conference on Intelligent Robots and Systems, San Francisco, CA, USA, 2011, pp. 865-871, DOI: 10.1109/IROS.2011.6094414.

which, in practice, implies:

1. Scoring nodes (i.e. laser scans) based on an estimate of their contribution to the reduction in the overall uncertainty of the resulting occupancy grid.
2. Finding nodes in excess to a configurable graph size that score the lowest in uncertainty reduction (i.e. information gain).
3. Marginalizing nodes using a Chow-Liu tree approximation to keep the sparse structure of the graph.

(1.) can coexist with the current geometrical score, and (2.) is one possible answer to: https://github.com/SteveMacenski/slam_toolbox/blob/6fce7fa0cf64a21720e4a3a76c68fcb7f27604c1/slam_toolbox/src/experimental/slam_toolbox_lifelong.cpp#L307

If published results hold ground, it may be an interesting step towards enabling lifelong mapping in production.

Implementation considerations

One possible path forward would be:

• [ ] Track occupancy grid alongside graph, caching mutual information
  • On node addition/removal, for each cell within scan range:
    • [ ] Recompute occupancy probability P(C | Z) (using discrete bayesian updates)
    • [ ] Recompute measurement outcome mass probability P(Z) (using Algorithm 1 in paper)
    • [ ] Recompute conditional entropy H(C | Z) and mutual information I(C; Z).
• [ ] Track graph size (total, per area), and once it exceeds a configured limit:
  • [ ] Compute mutual information I(M; Z) by summarizing all I(C; Z)
  • [ ] Select k candidates for removal (all in graph, oldest k nodes, LRU k nodes)
    • Must be efficient and deal with the real world breaking the static map assumption.
  • [ ] Compute mutual information I(M; Z\{Zk}) by simulating candidate removal
    • Using information deltas may help avoid re-computation and occupancy grid copies.
  • [ ] Find node with the minimum information gain I(M; Z) - I(M; Z\{Zk})
  • [ ] Marginalize the node in the graph
    • [ ] Compute mutual information between neighbor nodes as edge weights, which IIUC can be derived from marginalized covariances as estimated by Ceres.
    • [ ] Compute maximum-weight spanning tree using Kruskal's algorithm.
    • [ ] Remove node from graph and update constraints between neighbor nodes to match the tree.
  • [ ] Repeat until the graph size limit is satisfied.

However, note that node marginalization can be done in parallel and leveraged immediately by the current lifelong mapping support. I also wonder whether having a two-stage pipeline with coarse and fine grid resolutions would help speed. But I haven't thought it through yet.

[hidmic]

An efficient implementation of this wonderful piece of research may be an interesting addition and a solution to past issue reports (such as https://github.com/SteveMacenski/slam_toolbox/issues/309).

Would you be willing to accept such a contribution @SteveMacenski?

[SteveMacenski]

I'd also make sure you consider the program of losing "key" frames. There are sometimes in SLAM sessions "key" nodes that regardless of the information it supplies, is the "backbone" of a loop closure or important tie-in that removing will substantially decompose the map. I've seen that happen in practice and one of the main reasons I didn't continue working on the problem (definitely solvable, just didn't have the time at that point). I can't tell you all the situations of how a "key" node forms, but certainly any node pair that solves a loop closure shouldn't be decayed, but some checking on entropy might reveal that implicitly.

That plan seems reasonable to me and I'd be open to a PR that were to replace the existing lifelong mapping mode for this one if it is able to function better to be "really" used vs a tech demo like mine! I'd thrown that out as an experimental demo hoping it would spark someone's creativity / interest to use the toolset I've provided to do better!

[hidmic]

There are sometimes in SLAM sessions "key" nodes that regardless of the information it supplies, is the "backbone" of a loop closure or important tie-in that removing will substantially decompose the map.

Yeah, intuitively I can see that too. I'd expect the information gain of such nodes to be high as a result, but we won't know for a fact until it is implemented.

That plan seems reasonable to me and I'd be open to a PR that were to replace the existing lifelong mapping mode for this one if it is able to function better to be "really" used vs a tech demo like mine!

Right. I personally foresee both approaches coexisting, for some time at least. No matter how much we work in a comprehensive enough demonstration of the system capabilities, it is no substitute for all the real world experience that fellow ROS users can bring. Time will tell if either approach should be dropped entirely. Having said that, I do believe this paper has potential.

I'd thrown that out as an experimental demo hoping it would spark someone's creativity / interest to use the toolset I've provided to do better!

And it worked? :smiley: