Factor Graph - SLAM

Factor Graph

Forney Factor Graph with representation of message passing

Factor graphs are one of the probabilistic graphical inference models, well suited for modelling and inferencing complex **estimation/optimization** problems like Simultaneous Localization and Mapping (SLAM) or Structure from Motion (SFM). Formally, Factor graphs are bi-partite graphs with two nodes,

• Variable nodes, represents unknown quantities in the problem
• Factor nodes, defines the relationship between variables

SLAM problem solves two parts, Localisation (knowing where you are in the environmnet) and Mapping (How the environment looks). Factor graphs are one of the ways to solve the SLAM problem by modelling the unknown locations as nodes and their relationship between varibales or with environemnt objects (landmarks) as factors. Consider the simple SLAM problem below,

Simple SLAM example

x1, x2 and x3 are nodes which are unknown locations l1 and l2 are the location of landmarks (which will be extracted from the environment using sensors) f1, f2, f3 are factors which defines the relationship between nodes and landmarks

Required libraries

• GTSAM
• aruco detect

Building GTSAM libraries

git clone https://github.com/borglab/gtsam.git

Then, in the root folder of the library, execute the following commands

#!bash
$ mkdir build
$ cd build
$ cmake ..
$ make check (optional, runs unit tests)
$ make install

Pre-requisite libraries: Boost, CMake

If not, install it with the following commands

$ sudo apt-get install libboost-all-dev
$ sudo apt-get install cmake

Building aruco detect library

sudo apt-get install ros-noetic-aruco-detect

Planar SLAM implementation

Using the GTSAM library, planar SLAM is implemented. This uses the data extracted from the robot's ros topics such as '/odom', '/tf'. From the topic '/odom', the robot's position can be found. From the topic '/tf', the aruco marker distance and orientation from the robot's baselink can be found. These are the two data required by the planar SLAM to estimate the robot's Position. The documentation can be found here

ROS node for SLAM using GTSAM

A ROS node for SLAM is implemented using the GTSAM library and its source code and documentation can be found here

Forney factor graph (FFG)

The generation and inferencing from the FFG is implemented in python. This allows the user to create a FFG with Boolean random variables and infer any variable from the created graph

The modules (GTSAM not required) needed and the sample code for forney factor graph generation and inferecing can be found here