Gaussian Blief Propagation on Planning

Implementation of factor graph is highly referred to fglib, but also add support to multivariate gaussians and update-able factor node, etc.

Motion planning code is an re-implementation for the paper Distributing Collaborative Multi-Robot Planning with Gaussian Belief Propagation. (However NOT finetuned.)

Sample for test_motion.py

Sample for test_motion_cross.py

References

• Papers

  • Distributing Collaborative Multi-Robot Planning with Gaussian Belief Propagation
  • FutureMapping 2: Gaussian Belief Propagation for Spatial AI

• Blogs (Tutorials)

  • A visual introduction to Gaussian Belief Propagation
  • 確率伝播アルゴリズムとは

• Other

  • Multiplying multivariate Gaussian with a univariate Gaussia