<aside> 💡
This blog is extensively built on the work of RRC seniors Shubodh Sai and Udit Singh Parihar, with added content including a hands-on tutorial on GTSAM. Checkout the original blog post here https://shubodhs.ai/blog/
</aside>
$$ P (X_{t}, M | U_{t-1}, Z_{t}) $$
$$ X_t \rightarrow \text{Poses}; M \rightarrow \text{Map};\\ U_{t-1} \rightarrow \text{Control Inputs}; Z_t \rightarrow \text{Observations} $$
Given robot's control inputs and observations, we want to estimate the probability distribution of robot's path and the map.
<aside> 💡 🐤 and 🥚 problem. We get the correct poses(trajectory) from the correct map which requires correct poses? We optimise for both.
</aside>
Over here, dark blue dots refer to the control inputs and light blue dots refer to the observations. $\mathbf{l}$stands for landmarks and $\mathbf{x}$ stands for poses.
<aside> 💡 Least Squares in General : Computing a solution for an overdetermined system by minimizing sum of least sqaures
</aside>
$$ P (X_{t}| U_{t-1}) $$
Pose graph SLAM requires two types of constraints. The first is odometry constraints, which link consecutive states $\mathbf{x}{i}$ and $\mathbf{x}{i+1}$ using a motion model. Additionally, to perform loop closure, the robot must recognize previously visited locations. This place recognition is part of the front-end and provides the second type of constraint, known as loop closure constraints, which connect two non-consecutive poses $\mathbf{x}{i}$ and $\mathbf{x}{j}$.
Example of a Factor Graph consisting of nodes representing poses, blue edges representing odometry measurements and green edges representing loop closure measurements
Odometry constraints/edges $u_i$ between $x_i$ and $x_{i+1}$ : As denoted by blue 🥶 circle
$$ x_{i+1} \: \sim \: \mathcal{N}(f(x_{i}, u_{i}), \Sigma_{i}) $$
Loop closure constraints/edges $u_{ij}$ between $x_i$ and $x_j$ : As denoted by green 🎾 circle
$$ x_{j} \: \sim \: \mathcal{N}(f(x_{i}, u_{ij}), \Lambda_{ij}) $$
<aside> 💡 Both of these are actually what makes it an optimization problem!
</aside>