github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-04-22 03:00:22 -04:00
Code Issues Packages Projects Releases Wiki Activity

clean up SLAM docs (#572)

Browse Source 
* clean up SLAM docs

* clean up SLAM docs

* clean up SLAM docs

* clean up SLAM docs

* clean up SLAM docs

* clean up SLAM docs

* clean up SLAM docs

* clean up SLAM docs

* clean up SLAM docs

* clean up SLAM docs

* clean up SLAM docs

* clean up SLAM docs

* clean up SLAM docs

* clean up SLAM docs
This commit is contained in:
Atsushi Sakai
2021-11-21 22:39:18 +09:00
committed by GitHub
parent 1f4e5ed8c3
commit d34f3ca7fa
35 changed files with 1617 additions and 3355 deletions
Download Patch File Download Diff File Expand all files Collapse all files

0
docs/modules/aerial_navigation/aerial_navigation.rst → docs/modules/aerial_navigation/aerial_navigation_main.rst
View File

0
docs/modules/appendix/appendix.rst → docs/modules/appendix/appendix_main.rst
View File

0
docs/modules/arm_navigation/arm_navigation.rst → docs/modules/arm_navigation/arm_navigation_main.rst
View File

0
docs/modules/introduction.rst → docs/modules/introduction_main.rst
View File

0
docs/modules/localization/localization.rst → docs/modules/localization/localization_main.rst
View File

0
docs/modules/mapping/mapping.rst → docs/modules/mapping/mapping_main.rst
View File

0
docs/modules/path_planning/path_planning.rst → docs/modules/path_planning/path_planning_main.rst
View File

0
docs/modules/path_tracking/path_tracking.rst → docs/modules/path_tracking/path_tracking_main.rst
View File

17
docs/modules/slam/FastSLAM1.rst
View File

@@ -2,14 +2,6 @@
FastSLAM1.0
-----------
.. code-block:: ipython3
from IPython.display import Image
Image(filename="animation.png",width=600)
.. image:: FastSLAM1_files/FastSLAM1_1_0.png
:width: 600px
@@ -31,8 +23,6 @@ positions by FastSLAM.
.. figure:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/SLAM/FastSLAM1/animation.gif
:alt: gif
gif
Introduction
~~~~~~~~~~~~
@@ -547,6 +537,9 @@ indices
References
~~~~~~~~~~
http://www.probabilistic-robotics.org/
- `PROBABILISTIC ROBOTICS`_
http://ais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/slam10-fastslam.pdf
- `FastSLAM Lecture`_
.. _PROBABILISTIC ROBOTICS: http://www.probabilistic-robotics.org/
.. _FastSLAM Lecture: http://ais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/slam10-fastslam.pdf

593
docs/modules/slam/ekf_slam.rst Normal file
View File

@@ -0,0 +1,593 @@
EKF SLAM
--------
This is an Extended Kalman Filter based SLAM example.
The blue line is ground truth, the black line is dead reckoning, the red
line is the estimated trajectory with EKF SLAM.
The green crosses are estimated landmarks.
|4|
Simulation
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
This is a simulation of EKF SLAM.
- Black stars: landmarks
- Green crosses: estimates of landmark positions
- Black line: dead reckoning
- Blue line: ground truth
- Red line: EKF SLAM position estimation
Introduction
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
EKF SLAM models the SLAM problem in a single EKF where the modeled state
is both the pose :math:`(x, y, \theta)` and an array of landmarks
:math:`[(x_1, y_1), (x_2, x_y), ... , (x_n, y_n)]` for :math:`n`
landmarks. The covariance between each of the positions and landmarks
are also tracked.
:math:`\begin{equation} X = \begin{bmatrix} x \\ y \\ \theta \\ x_1 \\ y_1 \\ x_2 \\ y_2 \\ \dots \\ x_n \\ y_n \end{bmatrix} \end{equation}`
:math:`\begin{equation} P = \begin{bmatrix} \sigma_{xx} & \sigma_{xy} & \sigma_{x\theta} & \sigma_{xx_1} & \sigma_{xy_1} & \sigma_{xx_2} & \sigma_{xy_2} & \dots & \sigma_{xx_n} & \sigma_{xy_n} \\ \sigma_{yx} & \sigma_{yy} & \sigma_{y\theta} & \sigma_{yx_1} & \sigma_{yy_1} & \sigma_{yx_2} & \sigma_{yy_2} & \dots & \sigma_{yx_n} & \sigma_{yy_n} \\ & & & & \vdots & & & & & \\ \sigma_{x_nx} & \sigma_{x_ny} & \sigma_{x_n\theta} & \sigma_{x_nx_1} & \sigma_{x_ny_1} & \sigma_{x_nx_2} & \sigma_{x_ny_2} & \dots & \sigma_{x_nx_n} & \sigma_{x_ny_n} \end{bmatrix} \end{equation}`
A single estimate of the pose is tracked over time, while the confidence
in the pose is tracked by the covariance matrix :math:`P`. :math:`P` is
a symmetric square matrix which each element in the matrix corresponding
to the covariance between two parts of the system. For example,
:math:`\sigma_{xy}` represents the covariance between the belief of
:math:`x` and :math:`y` and is equal to :math:`\sigma_{yx}`.
The state can be represented more concisely as follows.
:math:`\begin{equation} X = \begin{bmatrix} x \\ m \end{bmatrix} \end{equation}`
:math:`\begin{equation} P = \begin{bmatrix} \Sigma_{xx} & \Sigma_{xm}\\ \Sigma_{mx} & \Sigma_{mm}\\ \end{bmatrix} \end{equation}`
Here the state simplifies to a combination of pose (:math:`x`) and map
(:math:`m`). The covariance matrix becomes easier to understand and
simply reads as the uncertainty of the robots pose
(:math:`\Sigma_{xx}`), the uncertainty of the map (:math:`\Sigma_{mm}`),
and the uncertainty of the robots pose with respect to the map and vice
versa (:math:`\Sigma_{xm}`, :math:`\Sigma_{mx}`).
Take care to note the difference between :math:`X` (state) and :math:`x`
(pose).
.. code:: ipython3
"""
Extended Kalman Filter SLAM example
original author: Atsushi Sakai (@Atsushi_twi)
notebook author: Andrew Tu (drewtu2)
"""
import math
import numpy as np
%matplotlib notebook
import matplotlib.pyplot as plt
# EKF state covariance
Cx = np.diag([0.5, 0.5, np.deg2rad(30.0)])**2 # Change in covariance
# Simulation parameter
Qsim = np.diag([0.2, np.deg2rad(1.0)])**2 # Sensor Noise
Rsim = np.diag([1.0, np.deg2rad(10.0)])**2 # Process Noise
DT = 0.1 # time tick [s]
SIM_TIME = 50.0 # simulation time [s]
MAX_RANGE = 20.0 # maximum observation range
M_DIST_TH = 2.0 # Threshold of Mahalanobis distance for data association.
STATE_SIZE = 3 # State size [x,y,yaw]
LM_SIZE = 2 # LM state size [x,y]
show_animation = True
Algorithm Walk through
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
At each time step, the following is done. - predict the new state using
the control functions - update the belief in landmark positions based on
the estimated state and measurements
.. code:: ipython3
def ekf_slam(xEst, PEst, u, z):
"""
Performs an iteration of EKF SLAM from the available information.
:param xEst: the belief in last position
:param PEst: the uncertainty in last position
:param u: the control function applied to the last position
:param z: measurements at this step
:returns: the next estimated position and associated covariance
"""
S = STATE_SIZE
# Predict
xEst, PEst, G, Fx = predict(xEst, PEst, u)
initP = np.eye(2)
# Update
xEst, PEst = update(xEst, PEst, u, z, initP)
return xEst, PEst
1- Predict
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
**Predict State update:** The following equations describe the predicted
motion model of the robot in case we provide only the control
:math:`(v,w)`, which are the linear and angular velocity respectively.
:math:`\begin{equation*} F= \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{bmatrix} \end{equation*}`
:math:`\begin{equation*} B= \begin{bmatrix} \Delta t cos(\theta) & 0\\ \Delta t sin(\theta) & 0\\ 0 & \Delta t \end{bmatrix} \end{equation*}`
:math:`\begin{equation*} U= \begin{bmatrix} v_t\\ w_t\\ \end{bmatrix} \end{equation*}`
:math:`\begin{equation*} X = FX + BU \end{equation*}`
:math:`\begin{equation*} \begin{bmatrix} x_{t+1} \\ y_{t+1} \\ \theta_{t+1} \end{bmatrix}= \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{bmatrix}\begin{bmatrix} x_{t} \\ y_{t} \\ \theta_{t} \end{bmatrix}+ \begin{bmatrix} \Delta t cos(\theta) & 0\\ \Delta t sin(\theta) & 0\\ 0 & \Delta t \end{bmatrix} \begin{bmatrix} v_{t} + \sigma_v\\ w_{t} + \sigma_w\\ \end{bmatrix} \end{equation*}`
Notice that while :math:`U` is only defined by :math:`v_t` and
:math:`w_t`, in the actual calculations, a :math:`+\sigma_v` and
:math:`+\sigma_w` appear. These values represent the error between the
given control inputs and the actual control inputs.
As a result, the simulation is set up as the following. :math:`R`
represents the process noise which is added to the control inputs to
simulate noise experienced in the real world. A set of truth values are
computed from the raw control values while the values dead reckoning
values incorporate the error into the estimation.
:math:`\begin{equation*} R= \begin{bmatrix} \sigma_v\\ \sigma_w\\ \end{bmatrix} \end{equation*}`
:math:`\begin{equation*} X_{true} = FX + B(U) \end{equation*}`
:math:`\begin{equation*} X_{DR} = FX + B(U + R) \end{equation*}`
The implementation of the motion model prediciton code is shown in
``motion_model``. The ``observation`` function shows how the simulation
uses (or doesn’t use) the process noise ``Rsim`` to the find the ground
truth and dead reckoning estimates of the pose.
**Predict covariance:** Add the state covariance to the the current
uncertainty of the EKF. At each time step, the uncertainty in the system
grows by the covariance of the pose, :math:`Cx`.
:math:`P = G^TPG + Cx`
Notice this uncertainty is only growing with respect to the pose, not
the landmarks.
.. code:: ipython3
def predict(xEst, PEst, u):
"""
Performs the prediction step of EKF SLAM
:param xEst: nx1 state vector
:param PEst: nxn covariance matrix
:param u: 2x1 control vector
:returns: predicted state vector, predicted covariance, jacobian of control vector, transition fx
"""
S = STATE_SIZE
G, Fx = jacob_motion(xEst[0:S], u)
xEst[0:S] = motion_model(xEst[0:S], u)
# Fx is an an identity matrix of size (STATE_SIZE)
# sigma = G*sigma*G.T + Noise
PEst[0:S, 0:S] = G.T @ PEst[0:S, 0:S] @ G + Fx.T @ Cx @ Fx
return xEst, PEst, G, Fx
.. code:: ipython3
def motion_model(x, u):
"""
Computes the motion model based on current state and input function.
:param x: 3x1 pose estimation
:param u: 2x1 control input [v; w]
:returns: the resulting state after the control function is applied
"""
F = np.array([[1.0, 0, 0],
[0, 1.0, 0],
[0, 0, 1.0]])
B = np.array([[DT * math.cos(x[2, 0]), 0],
[DT * math.sin(x[2, 0]), 0],
[0.0, DT]])
x = (F @ x) + (B @ u)
return x
2 - Update
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
In the update phase, the observations of nearby landmarks are used to
correct the location estimate.
For every landmark observed, it is associated to a particular landmark
in the known map. If no landmark exists in the position surrounding the
landmark, it is taken as a NEW landmark. The distance threshold for how
far a landmark must be from the next known landmark before its
considered to be a new landmark is set by ``M_DIST_TH``.
With an observation associated to the appropriate landmark, the
**innovation** can be calculated. Innovation (:math:`y`) is the
difference between the observation and the observation that *should*
have been made if the observation were made from the pose predicted in
the predict stage.
:math:`y = z_t - h(X)`
With the innovation calculated, the question becomes which to trust more
- the observations or the predictions? To determine this, we calculate
the Kalman Gain - a percent of how much of the innovation to add to the
prediction based on the uncertainty in the predict step and the update
step.
:math:`K = \bar{P_t}H_t^T(H_t\bar{P_t}H_t^T + Q_t)^{-1}`
In these equations, :math:`H` is the jacobian of the
measurement function. The multiplications by :math:`H^T` and :math:`H`
represent the application of the delta to the measurement covariance.
Intuitively, this equation is applying the following from the single
variate Kalman equation but in the multivariate form, i.e. finding the
ratio of the uncertainty of the process compared the measurement.
K = :math:`\frac{\bar{P_t}}{\bar{P_t} + Q_t}`
If :math:`Q_t << \bar{P_t}`, (i.e. the measurement covariance is low
relative to the current estimate), then the Kalman gain will be
:math:`~1`. This results in adding all of the innovation to the estimate
– and therefore completely believing the measurement.
However, if :math:`Q_t >> \bar{P_t}` then the Kalman gain will go to 0,
signaling a high trust in the process and little trust in the
measurement.
The update is captured in the following.
:math:`xUpdate = xEst + (K * y)`
Of course, the covariance must also be updated as well to account for
the changing uncertainty.
:math:`P_{t} = (I-K_tH_t)\bar{P_t}`
.. code:: ipython3
def update(xEst, PEst, u, z, initP):
"""
Performs the update step of EKF SLAM
:param xEst: nx1 the predicted pose of the system and the pose of the landmarks
:param PEst: nxn the predicted covariance
:param u: 2x1 the control function
:param z: the measurements read at new position
:param initP: 2x2 an identity matrix acting as the initial covariance
:returns: the updated state and covariance for the system
"""
for iz in range(len(z[:, 0])): # for each observation
minid = search_correspond_LM_ID(xEst, PEst, z[iz, 0:2]) # associate to a known landmark
nLM = calc_n_LM(xEst) # number of landmarks we currently know about
if minid == nLM: # Landmark is a NEW landmark
print("New LM")
# Extend state and covariance matrix
xAug = np.vstack((xEst, calc_LM_Pos(xEst, z[iz, :])))
PAug = np.vstack((np.hstack((PEst, np.zeros((len(xEst), LM_SIZE)))),
np.hstack((np.zeros((LM_SIZE, len(xEst))), initP))))
xEst = xAug
PEst = PAug
lm = get_LM_Pos_from_state(xEst, minid)
y, S, H = calc_innovation(lm, xEst, PEst, z[iz, 0:2], minid)
K = (PEst @ H.T) @ np.linalg.inv(S) # Calculate Kalman Gain
xEst = xEst + (K @ y)
PEst = (np.eye(len(xEst)) - (K @ H)) @ PEst
xEst[2] = pi_2_pi(xEst[2])
return xEst, PEst
.. code:: ipython3
def calc_innovation(lm, xEst, PEst, z, LMid):
"""
Calculates the innovation based on expected position and landmark position
:param lm: landmark position
:param xEst: estimated position/state
:param PEst: estimated covariance
:param z: read measurements
:param LMid: landmark id
:returns: returns the innovation y, and the jacobian H, and S, used to calculate the Kalman Gain
"""
delta = lm - xEst[0:2]
q = (delta.T @ delta)[0, 0]
zangle = math.atan2(delta[1, 0], delta[0, 0]) - xEst[2, 0]
zp = np.array([[math.sqrt(q), pi_2_pi(zangle)]])
# zp is the expected measurement based on xEst and the expected landmark position
y = (z - zp).T # y = innovation
y[1] = pi_2_pi(y[1])
H = jacobH(q, delta, xEst, LMid + 1)
S = H @ PEst @ H.T + Cx[0:2, 0:2]
return y, S, H
def jacobH(q, delta, x, i):
"""
Calculates the jacobian of the measurement function
:param q: the range from the system pose to the landmark
:param delta: the difference between a landmark position and the estimated system position
:param x: the state, including the estimated system position
:param i: landmark id + 1
:returns: the jacobian H
"""
sq = math.sqrt(q)
G = np.array([[-sq * delta[0, 0], - sq * delta[1, 0], 0, sq * delta[0, 0], sq * delta[1, 0]],
[delta[1, 0], - delta[0, 0], - q, - delta[1, 0], delta[0, 0]]])
G = G / q
nLM = calc_n_LM(x)
F1 = np.hstack((np.eye(3), np.zeros((3, 2 * nLM))))
F2 = np.hstack((np.zeros((2, 3)), np.zeros((2, 2 * (i - 1))),
np.eye(2), np.zeros((2, 2 * nLM - 2 * i))))
F = np.vstack((F1, F2))
H = G @ F
return H
Observation Step
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
The observation step described here is outside the main EKF SLAM process
and is primarily used as a method of driving the simulation. The
observations function is in charge of calculating how the poses of the
robots change and accumulate error over time, and the theoretical
measurements that are expected as a result of each measurement.
Observations are based on the TRUE position of the robot. Error in dead
reckoning and control functions are passed along here as well.
.. code:: ipython3
def observation(xTrue, xd, u, RFID):
"""
:param xTrue: the true pose of the system
:param xd: the current noisy estimate of the system
:param u: the current control input
:param RFID: the true position of the landmarks
:returns: Computes the true position, observations, dead reckoning (noisy) position,
and noisy control function
"""
xTrue = motion_model(xTrue, u)
# add noise to gps x-y
z = np.zeros((0, 3))
for i in range(len(RFID[:, 0])): # Test all beacons, only add the ones we can see (within MAX_RANGE)
dx = RFID[i, 0] - xTrue[0, 0]
dy = RFID[i, 1] - xTrue[1, 0]
d = math.sqrt(dx**2 + dy**2)
angle = pi_2_pi(math.atan2(dy, dx) - xTrue[2, 0])
if d <= MAX_RANGE:
dn = d + np.random.randn() * Qsim[0, 0] # add noise
anglen = angle + np.random.randn() * Qsim[1, 1] # add noise
zi = np.array([dn, anglen, i])
z = np.vstack((z, zi))
# add noise to input
ud = np.array([[
u[0, 0] + np.random.randn() * Rsim[0, 0],
u[1, 0] + np.random.randn() * Rsim[1, 1]]]).T
xd = motion_model(xd, ud)
return xTrue, z, xd, ud
.. code:: ipython3
def calc_n_LM(x):
"""
Calculates the number of landmarks currently tracked in the state
:param x: the state
:returns: the number of landmarks n
"""
n = int((len(x) - STATE_SIZE) / LM_SIZE)
return n
def jacob_motion(x, u):
"""
Calculates the jacobian of motion model.
:param x: The state, including the estimated position of the system
:param u: The control function
:returns: G: Jacobian
Fx: STATE_SIZE x (STATE_SIZE + 2 * num_landmarks) matrix where the left side is an identity matrix
"""
# [eye(3) [0 x y; 0 x y; 0 x y]]
Fx = np.hstack((np.eye(STATE_SIZE), np.zeros(
(STATE_SIZE, LM_SIZE * calc_n_LM(x)))))
jF = np.array([[0.0, 0.0, -DT * u[0] * math.sin(x[2, 0])],
[0.0, 0.0, DT * u[0] * math.cos(x[2, 0])],
[0.0, 0.0, 0.0]],dtype=object)
G = np.eye(STATE_SIZE) + Fx.T @ jF @ Fx
if calc_n_LM(x) > 0:
print(Fx.shape)
return G, Fx,
.. code:: ipython3
def calc_LM_Pos(x, z):
"""
Calculates the pose in the world coordinate frame of a landmark at the given measurement.
:param x: [x; y; theta]
:param z: [range; bearing]
:returns: [x; y] for given measurement
"""
zp = np.zeros((2, 1))
zp[0, 0] = x[0, 0] + z[0] * math.cos(x[2, 0] + z[1])
zp[1, 0] = x[1, 0] + z[0] * math.sin(x[2, 0] + z[1])
#zp[0, 0] = x[0, 0] + z[0, 0] * math.cos(x[2, 0] + z[0, 1])
#zp[1, 0] = x[1, 0] + z[0, 0] * math.sin(x[2, 0] + z[0, 1])
return zp
def get_LM_Pos_from_state(x, ind):
"""
Returns the position of a given landmark
:param x: The state containing all landmark positions
:param ind: landmark id
:returns: The position of the landmark
"""
lm = x[STATE_SIZE + LM_SIZE * ind: STATE_SIZE + LM_SIZE * (ind + 1), :]
return lm
def search_correspond_LM_ID(xAug, PAug, zi):
"""
Landmark association with Mahalanobis distance.
If this landmark is at least M_DIST_TH units away from all known landmarks,
it is a NEW landmark.
:param xAug: The estimated state
:param PAug: The estimated covariance
:param zi: the read measurements of specific landmark
:returns: landmark id
"""
nLM = calc_n_LM(xAug)
mdist = []
for i in range(nLM):
lm = get_LM_Pos_from_state(xAug, i)
y, S, H = calc_innovation(lm, xAug, PAug, zi, i)
mdist.append(y.T @ np.linalg.inv(S) @ y)
mdist.append(M_DIST_TH) # new landmark
minid = mdist.index(min(mdist))
return minid
def calc_input():
v = 1.0 # [m/s]
yawrate = 0.1 # [rad/s]
u = np.array([[v, yawrate]]).T
return u
def pi_2_pi(angle):
return (angle + math.pi) % (2 * math.pi) - math.pi
.. code:: ipython3
def main():
print(" start!!")
time = 0.0
# RFID positions [x, y]
RFID = np.array([[10.0, -2.0],
[15.0, 10.0],
[3.0, 15.0],
[-5.0, 20.0]])
# State Vector [x y yaw v]'
xEst = np.zeros((STATE_SIZE, 1))
xTrue = np.zeros((STATE_SIZE, 1))
PEst = np.eye(STATE_SIZE)
xDR = np.zeros((STATE_SIZE, 1)) # Dead reckoning
# history
hxEst = xEst
hxTrue = xTrue
hxDR = xTrue
while SIM_TIME >= time:
time += DT
u = calc_input()
xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFID)
xEst, PEst = ekf_slam(xEst, PEst, ud, z)
x_state = xEst[0:STATE_SIZE]
# store data history
hxEst = np.hstack((hxEst, x_state))
hxDR = np.hstack((hxDR, xDR))
hxTrue = np.hstack((hxTrue, xTrue))
if show_animation: # pragma: no cover
plt.cla()
plt.plot(RFID[:, 0], RFID[:, 1], "*k")
plt.plot(xEst[0], xEst[1], ".r")
# plot landmark
for i in range(calc_n_LM(xEst)):
plt.plot(xEst[STATE_SIZE + i * 2],
xEst[STATE_SIZE + i * 2 + 1], "xg")
plt.plot(hxTrue[0, :],
hxTrue[1, :], "-b")
plt.plot(hxDR[0, :],
hxDR[1, :], "-k")
plt.plot(hxEst[0, :],
hxEst[1, :], "-r")
plt.axis("equal")
plt.grid(True)
plt.pause(0.001)
.. code:: ipython3
%matplotlib notebook
main()
.. parsed-literal::
start!!
New LM
New LM
New LM
.. image:: ekf_slam_files/ekf_slam_1_0.png
References:
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- `PROBABILISTIC ROBOTICS`_
.. _PROBABILISTIC ROBOTICS: http://www.probabilistic-robotics.org/
.. |4| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/SLAM/EKFSLAM/animation.gif

BIN
docs/modules/slam/ekf_slam_files/ekf_slam_1_0.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 111 KiB

209
docs/modules/slam/graphSLAM_SE2_example.rst Normal file
View File

@@ -0,0 +1,209 @@
Graph SLAM for a real-world SE(2) dataset
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
.. code:: ipython3
from graphslam.graph import Graph
from graphslam.load import load_g2o_se2
Introduction
^^^^^^^^^^^^
For a complete derivation of the Graph SLAM algorithm, please see
`Graph SLAM Formulation`_.
This notebook illustrates the iterative optimization of a real-world
:math:`SE(2)` dataset. The code can be found in the ``graphslam``
folder. For simplicity, numerical differentiation is used in lieu of
analytic Jacobians. This code originated from the
`python-graphslam <https://github.com/JeffLIrion/python-graphslam>`__
repo, which is a full-featured Graph SLAM solver. The dataset in this
example is used with permission from Luca Carlone and was downloaded
from his `website <https://lucacarlone.mit.edu/datasets/>`__.
The Dataset
^^^^^^^^^^^^
.. code:: ipython3
g = load_g2o_se2("data/input_INTEL.g2o")
print("Number of edges: {}".format(len(g._edges)))
print("Number of vertices: {}".format(len(g._vertices)))
.. parsed-literal::
Number of edges: 1483
Number of vertices: 1228
.. code:: ipython3
g.plot(title=r"Original ($\chi^2 = {:.0f}$)".format(g.calc_chi2()))
.. image:: graphSLAM_SE2_example_files/graphSLAM_SE2_example_4_0.png
Each edge in this dataset is a constraint that compares the measured
:math:`SE(2)` transformation between two poses to the expected
:math:`SE(2)` transformation between them, as computed using the current
pose estimates. These edges can be classified into two categories:
1. Odometry edges constrain two consecutive vertices, and the
measurement for the :math:`SE(2)` transformation comes directly from
odometry data.
2. Scan-matching edges constrain two non-consecutive vertices. These
scan matches can be computed using, for example, 2-D LiDAR data or
landmarks; the details of how these constraints are determined is
beyond the scope of this example. This is often referred to as *loop
closure* in the Graph SLAM literature.
We can easily parse out the two different types of edges present in this
dataset and plot them.
.. code:: ipython3
def parse_edges(g):
"""Split the graph `g` into two graphs, one with only odometry edges and the other with only scan-matching edges.
Parameters
----------
g : graphslam.graph.Graph
The input graph
Returns
-------
g_odom : graphslam.graph.Graph
A graph consisting of the vertices and odometry edges from `g`
g_scan : graphslam.graph.Graph
A graph consisting of the vertices and scan-matching edges from `g`
"""
edges_odom = []
edges_scan = []
for e in g._edges:
if abs(e.vertex_ids[1] - e.vertex_ids[0]) == 1:
edges_odom.append(e)
else:
edges_scan.append(e)
g_odom = Graph(edges_odom, g._vertices)
g_scan = Graph(edges_scan, g._vertices)
return g_odom, g_scan
.. code:: ipython3
g_odom, g_scan = parse_edges(g)
print("Number of odometry edges: {:4d}".format(len(g_odom._edges)))
print("Number of scan-matching edges: {:4d}".format(len(g_scan._edges)))
print("\nχ^2 error from odometry edges: {:11.3f}".format(g_odom.calc_chi2()))
print("χ^2 error from scan-matching edges: {:11.3f}".format(g_scan.calc_chi2()))
.. parsed-literal::
Number of odometry edges: 1227
Number of scan-matching edges: 256
χ^2 error from odometry edges: 0.232
χ^2 error from scan-matching edges: 7191686.151
.. code:: ipython3
g_odom.plot(title="Odometry edges")
.. image:: graphSLAM_SE2_example_files/graphSLAM_SE2_example_8_0.png
.. code:: ipython3
g_scan.plot(title="Scan-matching edges")
.. image:: graphSLAM_SE2_example_files/graphSLAM_SE2_example_9_0.png
Optimization
^^^^^^^^^^^^
Initially, the pose estimates are consistent with the collected odometry
measurements, and the odometry edges contribute almost zero towards the
:math:`\chi^2` error. However, there are large discrepancies between the
scan-matching constraints and the initial pose estimates. This is not
surprising, since small errors in odometry readings that are propagated
over time can lead to large errors in the robot’s trajectory. What makes
Graph SLAM effective is that it allows incorporation of multiple
different data sources into a single optimization problem.
.. code:: ipython3
g.optimize()
.. parsed-literal::
Iteration chi^2 rel. change
--------- ----- -----------
0 7191686.3825
1 320031728.8624 43.500234
2 125083004.3299 -0.609154
3 338155.9074 -0.997297
4 735.1344 -0.997826
5 215.8405 -0.706393
6 215.8405 -0.000000
.. figure:: graphSLAM_SE2_example_files/Graph_SLAM_optimization.gif
:alt: Graph_SLAM_optimization.gif
.. code:: ipython3
g.plot(title="Optimized")
.. image:: graphSLAM_SE2_example_files/graphSLAM_SE2_example_13_0.png
.. code:: ipython3
print("\nχ^2 error from odometry edges: {:7.3f}".format(g_odom.calc_chi2()))
print("χ^2 error from scan-matching edges: {:7.3f}".format(g_scan.calc_chi2()))
.. parsed-literal::
χ^2 error from odometry edges: 142.189
χ^2 error from scan-matching edges: 73.652
.. code:: ipython3
g_odom.plot(title="Odometry edges")
.. image:: graphSLAM_SE2_example_files/graphSLAM_SE2_example_15_0.png
.. code:: ipython3
g_scan.plot(title="Scan-matching edges")
.. image:: graphSLAM_SE2_example_files/graphSLAM_SE2_example_16_0.png

BIN
docs/modules/slam/graphSLAM_SE2_example_files/Graph_SLAM_optimization.gif Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 112 KiB

BIN
docs/modules/slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_13_0.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 32 KiB

BIN
docs/modules/slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_15_0.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 29 KiB

BIN
docs/modules/slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_16_0.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 29 KiB

BIN
docs/modules/slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_4_0.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 51 KiB

BIN
docs/modules/slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_8_0.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 30 KiB

BIN
docs/modules/slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_9_0.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 48 KiB

557
docs/modules/slam/graphSLAM_doc.rst Normal file
View File

@@ -0,0 +1,557 @@
Graph SLAM
~~~~~~~~~~~~
.. code:: ipython3
import copy
import math
import itertools
import numpy as np
import matplotlib.pyplot as plt
from graph_based_slam import calc_rotational_matrix, calc_jacobian, cal_observation_sigma, \
calc_input, observation, motion_model, Edge, pi_2_pi
%matplotlib inline
np.set_printoptions(precision=3, suppress=True)
np.random.seed(0)
Introduction
^^^^^^^^^^^^
In contrast to the probabilistic approaches for solving SLAM, such as
EKF, UKF, particle filters, and so on, the graph technique formulates
the SLAM as an optimization problem. It is mostly used to solve the full
SLAM problem in an offline fashion, i.e. optimize all the poses of the
robot after the path has been traversed. However, some variants are
available that uses graph-based approaches to perform online estimation
or to solve for a subset of the poses.
GraphSLAM uses the motion information as well as the observations of the
environment to create least square problem that can be solved using
standard optimization techniques.
Minimal Example
^^^^^^^^^^^^^^^
The following example illustrates the main idea behind graphSLAM. A
simple case of a 1D robot is considered that can only move in 1
direction. The robot is commanded to move forward with a control input
:math:`u_t=1`, however, the motion is not perfect and the measured
odometry will deviate from the true path. At each time step the robot can
observe its environment, for this simple case as well, there is only a
single landmark at coordinates :math:`x=3`. The measured observations
are the range between the robot and landmark. These measurements are
also subjected to noise. No bearing information is required since this
is a 1D problem.
To solve this, graphSLAM creates what is called as the system
information matrix :math:`\Omega` also referred to as :math:`H` and the
information vector :math:`\xi` also known as :math:`b`. The entries are
created based on the information of the motion and the observation.
.. code:: ipython3
R = 0.2
Q = 0.2
N = 3
graphics_radius = 0.1
odom = np.empty((N,1))
obs = np.empty((N,1))
x_true = np.empty((N,1))
landmark = 3
# Simulated readings of odometry and observations
x_true[0], odom[0], obs[0] = 0.0, 0.0, 2.9
x_true[1], odom[1], obs[1] = 1.0, 1.5, 2.0
x_true[2], odom[2], obs[2] = 2.0, 2.4, 1.0
hxDR = copy.deepcopy(odom)
# Visualization
plt.plot(landmark,0, '*k', markersize=30)
for i in range(N):
plt.plot(odom[i], 0, '.', markersize=50, alpha=0.8, color='steelblue')
plt.plot([odom[i], odom[i] + graphics_radius],
[0,0], 'r')
plt.text(odom[i], 0.02, "X_{}".format(i), fontsize=12)
plt.plot(obs[i]+odom[i],0,'.', markersize=25, color='brown')
plt.plot(x_true[i],0,'.g', markersize=20)
plt.grid()
plt.show()
# Defined as a function to facilitate iteration
def get_H_b(odom, obs):
"""
Create the information matrix and information vector. This implementation is
based on the concept of virtual measurement i.e. the observations of the
landmarks are converted into constraints (edges) between the nodes that
have observed this landmark.
"""
measure_constraints = {}
omegas = {}
zids = list(itertools.combinations(range(N),2))
H = np.zeros((N,N))
b = np.zeros((N,1))
for (t1, t2) in zids:
x1 = odom[t1]
x2 = odom[t2]
z1 = obs[t1]
z2 = obs[t2]
# Adding virtual measurement constraint
measure_constraints[(t1,t2)] = (x2-x1-z1+z2)
omegas[(t1,t2)] = (1 / (2*Q))
# populate system's information matrix and vector
H[t1,t1] += omegas[(t1,t2)]
H[t2,t2] += omegas[(t1,t2)]
H[t2,t1] -= omegas[(t1,t2)]
H[t1,t2] -= omegas[(t1,t2)]
b[t1] += omegas[(t1,t2)] * measure_constraints[(t1,t2)]
b[t2] -= omegas[(t1,t2)] * measure_constraints[(t1,t2)]
return H, b
H, b = get_H_b(odom, obs)
print("The determinant of H: ", np.linalg.det(H))
H[0,0] += 1 # np.inf ?
print("The determinant of H after anchoring constraint: ", np.linalg.det(H))
for i in range(5):
H, b = get_H_b(odom, obs)
H[(0,0)] += 1
# Recover the posterior over the path
dx = np.linalg.inv(H) @ b
odom += dx
# repeat till convergence
print("Running graphSLAM ...")
print("Odometry values after optimzation: \n", odom)
plt.figure()
plt.plot(x_true, np.zeros(x_true.shape), '.', markersize=20, label='Ground truth')
plt.plot(odom, np.zeros(x_true.shape), '.', markersize=20, label='Estimation')
plt.plot(hxDR, np.zeros(x_true.shape), '.', markersize=20, label='Odom')
plt.legend()
plt.grid()
plt.show()
.. image:: graphSLAM_doc_files/graphSLAM_doc_2_0.png
.. parsed-literal::
The determinant of H: 0.0
The determinant of H after anchoring constraint: 18.750000000000007
Running graphSLAM ...
Odometry values after optimzation:
[[-0. ]
[ 0.9]
[ 1.9]]
.. image:: graphSLAM_doc_files/graphSLAM_doc_2_2.png
In particular, the tasks are split into 2 parts, graph construction, and
graph optimization. ### Graph Construction
Firstly the nodes are defined
:math:`\boldsymbol{x} = \boldsymbol{x}_{1:n}` such that each node is the
pose of the robot at time :math:`t_i` Secondly, the edges i.e. the
constraints, are constructed according to the following conditions:
- robot moves from :math:`\boldsymbol{x}_i` to
:math:`\boldsymbol{x}_j`. This edge corresponds to the odometry
measurement. Relative motion constraints (Not included in the
previous minimal example).
- Measurement constraints, this can be done in two ways:
- The information matrix is set in such a way that it includes the
landmarks in the map as well. Then the constraints can be entered
in a straightforward fashion between a node
:math:`\boldsymbol{x}_i` and some landmark :math:`m_k`
- Through creating a virtual measurement among all the node that
have observed the same landmark. More concretely, robot observes
the same landmark from :math:`\boldsymbol{x}_i` and
:math:`\boldsymbol{x}_j`. Relative measurement constraint. The
“virtual measurement” :math:`\boldsymbol{z}_{ij}`, which is the
estimated pose of :math:`\boldsymbol{x}_j` as seen from the node
:math:`\boldsymbol{x}_i`. The virtual measurement can then be
entered in the information matrix and vector in a similar fashion
to the motion constraints.
An edge is fully characterized by the values of the error (entry to
information vector) and the local information matrix (entry to the
system’s information vector). The larger the local information matrix
(lower :math:`Q` or :math:`R`) the values that this edge will contribute
with.
Important Notes:
- The addition to the information matrix and vector are added to the
earlier values.
- In the case of 2D robot, the constraints will be non-linear,
therefore, a Jacobian of the error w.r.t the states is needed when
updated :math:`H` and :math:`b`.
- The anchoring constraint is needed in order to avoid having a
singular information matri.
Graph Optimization
^^^^^^^^^^^^^^^^^^
The result from this formulation yields an overdetermined system of
equations. The goal after constructing the graph is to find:
:math:`x^*=\underset{x}{\mathrm{argmin}}~\underset{ij}\Sigma~f(e_{ij})`,
where :math:`f` is some error function that depends on the edges between
to related nodes :math:`i` and :math:`j`. The derivation in the references
arrive at the solution for :math:`x^* = H^{-1}b`
Planar Example:
^^^^^^^^^^^^^^^
Now we will go through an example with a more realistic case of a 2D
robot with 3DoF, namely, :math:`[x, y, \theta]^T`
.. code:: ipython3
# Simulation parameter
Qsim = np.diag([0.01, np.deg2rad(0.010)])**2 # error added to range and bearing
Rsim = np.diag([0.1, np.deg2rad(1.0)])**2 # error added to [v, w]
DT = 2.0 # time tick [s]
SIM_TIME = 100.0 # simulation time [s]
MAX_RANGE = 30.0 # maximum observation range
STATE_SIZE = 3 # State size [x,y,yaw]
# TODO: Why not use Qsim ?
# Covariance parameter of Graph Based SLAM
C_SIGMA1 = 0.1
C_SIGMA2 = 0.1
C_SIGMA3 = np.deg2rad(1.0)
MAX_ITR = 20 # Maximum iteration during optimization
timesteps = 1
# consider only 2 landmarks for simplicity
# RFID positions [x, y, yaw]
RFID = np.array([[10.0, -2.0, 0.0],
# [15.0, 10.0, 0.0],
# [3.0, 15.0, 0.0],
# [-5.0, 20.0, 0.0],
# [-5.0, 5.0, 0.0]
])
# State Vector [x y yaw v]'
xTrue = np.zeros((STATE_SIZE, 1))
xDR = np.zeros((STATE_SIZE, 1)) # Dead reckoning
xTrue[2] = np.deg2rad(45)
xDR[2] = np.deg2rad(45)
# history initial values
hxTrue = xTrue
hxDR = xTrue
_, z, _, _ = observation(xTrue, xDR, np.array([[0,0]]).T, RFID)
hz = [z]
for i in range(timesteps):
u = calc_input()
xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFID)
hxDR = np.hstack((hxDR, xDR))
hxTrue = np.hstack((hxTrue, xTrue))
hz.append(z)
# visualize
graphics_radius = 0.3
plt.plot(RFID[:, 0], RFID[:, 1], "*k", markersize=20)
plt.plot(hxDR[0, :], hxDR[1, :], '.', markersize=50, alpha=0.8, label='Odom')
plt.plot(hxTrue[0, :], hxTrue[1, :], '.', markersize=20, alpha=0.6, label='X_true')
for i in range(hxDR.shape[1]):
x = hxDR[0, i]
y = hxDR[1, i]
yaw = hxDR[2, i]
plt.plot([x, x + graphics_radius * np.cos(yaw)],
[y, y + graphics_radius * np.sin(yaw)], 'r')
d = hz[i][:, 0]
angle = hz[i][:, 1]
plt.plot([x + d * np.cos(angle + yaw)], [y + d * np.sin(angle + yaw)], '.',
markersize=20, alpha=0.7)
plt.legend()
plt.grid()
plt.show()
.. image:: graphSLAM_doc_files/graphSLAM_doc_4_0.png
.. code:: ipython3
# Copy the data to have a consistent naming with the .py file
zlist = copy.deepcopy(hz)
x_opt = copy.deepcopy(hxDR)
xlist = copy.deepcopy(hxDR)
number_of_nodes = x_opt.shape[1]
n = number_of_nodes * STATE_SIZE
After the data has been saved, the graph will be constructed by looking
at each pair for nodes. The virtual measurement is only created if two
nodes have observed the same landmark at different points in time. The
next cells are a walk through for a single iteration of graph
construction -> optimization -> estimate update.
.. code:: ipython3
# get all the possible combination of the different node
zids = list(itertools.combinations(range(len(zlist)), 2))
print("Node combinations: \n", zids)
for i in range(xlist.shape[1]):
print("Node {} observed landmark with ID {}".format(i, zlist[i][0, 3]))
.. parsed-literal::
Node combinations:
[(0, 1)]
Node 0 observed landmark with ID 0.0
Node 1 observed landmark with ID 0.0
In the following code snippet the error based on the virtual measurement
between node 0 and 1 will be created. The equations for the error is as follows:
:math:`e_{ij}^x = x_j + d_j cos(\psi_j +\theta_j) - x_i - d_i cos(\psi_i + \theta_i)`
:math:`e_{ij}^y = y_j + d_j sin(\psi_j + \theta_j) - y_i - d_i sin(\psi_i + \theta_i)`
:math:`e_{ij}^x = \psi_j + \theta_j - \psi_i - \theta_i`
Where :math:`[x_i, y_i, \psi_i]` is the pose for node :math:`i` and
similarly for node :math:`j`, :math:`d` is the measured distance at
nodes :math:`i` and :math:`j`, and :math:`\theta` is the measured
bearing to the landmark. The difference is visualized with the figure in
the next cell.
In case of perfect motion and perfect measurement the error shall be
zero since :math:`x_j + d_j cos(\psi_j + \theta_j)` should equal
:math:`x_i + d_i cos(\psi_i + \theta_i)`
.. code:: ipython3
# Initialize edges list
edges = []
# Go through all the different combinations
for (t1, t2) in zids:
x1, y1, yaw1 = xlist[0, t1], xlist[1, t1], xlist[2, t1]
x2, y2, yaw2 = xlist[0, t2], xlist[1, t2], xlist[2, t2]
# All nodes have valid observation with ID=0, therefore, no data association condition
iz1 = 0
iz2 = 0
d1 = zlist[t1][iz1, 0]
angle1, phi1 = zlist[t1][iz1, 1], zlist[t1][iz1, 2]
d2 = zlist[t2][iz2, 0]
angle2, phi2 = zlist[t2][iz2, 1], zlist[t2][iz2, 2]
# find angle between observation and horizontal
tangle1 = pi_2_pi(yaw1 + angle1)
tangle2 = pi_2_pi(yaw2 + angle2)
# project the observations
tmp1 = d1 * math.cos(tangle1)
tmp2 = d2 * math.cos(tangle2)
tmp3 = d1 * math.sin(tangle1)
tmp4 = d2 * math.sin(tangle2)
edge = Edge()
print(y1,y2, tmp3, tmp4)
# calculate the error of the virtual measurement
# node 1 as seen from node 2 throught the observations 1,2
edge.e[0, 0] = x2 - x1 - tmp1 + tmp2
edge.e[1, 0] = y2 - y1 - tmp3 + tmp4
edge.e[2, 0] = pi_2_pi(yaw2 - yaw1 - tangle1 + tangle2)
edge.d1, edge.d2 = d1, d2
edge.yaw1, edge.yaw2 = yaw1, yaw2
edge.angle1, edge.angle2 = angle1, angle2
edge.id1, edge.id2 = t1, t2
edges.append(edge)
print("For nodes",(t1,t2))
print("Added edge with errors: \n", edge.e)
# Visualize measurement projections
plt.plot(RFID[0, 0], RFID[0, 1], "*k", markersize=20)
plt.plot([x1,x2],[y1,y2], '.', markersize=50, alpha=0.8)
plt.plot([x1, x1 + graphics_radius * np.cos(yaw1)],
[y1, y1 + graphics_radius * np.sin(yaw1)], 'r')
plt.plot([x2, x2 + graphics_radius * np.cos(yaw2)],
[y2, y2 + graphics_radius * np.sin(yaw2)], 'r')
plt.plot([x1,x1+tmp1], [y1,y1], label="obs 1 x")
plt.plot([x2,x2+tmp2], [y2,y2], label="obs 2 x")
plt.plot([x1,x1], [y1,y1+tmp3], label="obs 1 y")
plt.plot([x2,x2], [y2,y2+tmp4], label="obs 2 y")
plt.plot(x1+tmp1, y1+tmp3, 'o')
plt.plot(x2+tmp2, y2+tmp4, 'o')
plt.legend()
plt.grid()
plt.show()
.. parsed-literal::
0.0 1.427649841628278 -2.0126109674819155 -3.524048014922737
For nodes (0, 1)
Added edge with errors:
[[-0.02 ]
[-0.084]
[ 0. ]]
.. image:: graphSLAM_doc_files/graphSLAM_doc_9_1.png
Since the constraints equations derived before are non-linear,
linearization is needed before we can insert them into the information
matrix and information vector. Two jacobians
:math:`A = \frac{\partial e_{ij}}{\partial \boldsymbol{x}_i}` as
:math:`\boldsymbol{x}_i` holds the three variabls x, y, and theta.
Similarly, :math:`B = \frac{\partial e_{ij}}{\partial \boldsymbol{x}_j}`
.. code:: ipython3
# Initialize the system information matrix and information vector
H = np.zeros((n, n))
b = np.zeros((n, 1))
x_opt = copy.deepcopy(hxDR)
for edge in edges:
id1 = edge.id1 * STATE_SIZE
id2 = edge.id2 * STATE_SIZE
t1 = edge.yaw1 + edge.angle1
A = np.array([[-1.0, 0, edge.d1 * math.sin(t1)],
[0, -1.0, -edge.d1 * math.cos(t1)],
[0, 0, -1.0]])
t2 = edge.yaw2 + edge.angle2
B = np.array([[1.0, 0, -edge.d2 * math.sin(t2)],
[0, 1.0, edge.d2 * math.cos(t2)],
[0, 0, 1.0]])
# TODO: use Qsim instead of sigma
sigma = np.diag([C_SIGMA1, C_SIGMA2, C_SIGMA3])
Rt1 = calc_rotational_matrix(tangle1)
Rt2 = calc_rotational_matrix(tangle2)
edge.omega = np.linalg.inv(Rt1 @ sigma @ Rt1.T + Rt2 @ sigma @ Rt2.T)
# Fill in entries in H and b
H[id1:id1 + STATE_SIZE, id1:id1 + STATE_SIZE] += A.T @ edge.omega @ A
H[id1:id1 + STATE_SIZE, id2:id2 + STATE_SIZE] += A.T @ edge.omega @ B
H[id2:id2 + STATE_SIZE, id1:id1 + STATE_SIZE] += B.T @ edge.omega @ A
H[id2:id2 + STATE_SIZE, id2:id2 + STATE_SIZE] += B.T @ edge.omega @ B
b[id1:id1 + STATE_SIZE] += (A.T @ edge.omega @ edge.e)
b[id2:id2 + STATE_SIZE] += (B.T @ edge.omega @ edge.e)
print("The determinant of H: ", np.linalg.det(H))
plt.figure()
plt.subplot(1,2,1)
plt.imshow(H, extent=[0, n, 0, n])
plt.subplot(1,2,2)
plt.imshow(b, extent=[0, 1, 0, n])
plt.show()
# Fix the origin, multiply by large number gives same results but better visualization
H[0:STATE_SIZE, 0:STATE_SIZE] += np.identity(STATE_SIZE)
print("The determinant of H after origin constraint: ", np.linalg.det(H))
plt.figure()
plt.subplot(1,2,1)
plt.imshow(H, extent=[0, n, 0, n])
plt.subplot(1,2,2)
plt.imshow(b, extent=[0, 1, 0, n])
plt.show()
.. parsed-literal::
The determinant of H: 0.0
The determinant of H after origin constraint: 716.1972439134893
.. image:: graphSLAM_doc_files/graphSLAM_doc_11_1.png
.. image:: graphSLAM_doc_files/graphSLAM_doc_11_2.png
.. code:: ipython3
# Find the solution (first iteration)
dx = - np.linalg.inv(H) @ b
for i in range(number_of_nodes):
x_opt[0:3, i] += dx[i * 3:i * 3 + 3, 0]
print("dx: \n",dx)
print("ground truth: \n ",hxTrue)
print("Odom: \n", hxDR)
print("SLAM: \n", x_opt)
# performance will improve with more iterations, nodes and landmarks.
print("\ngraphSLAM localization error: ", np.sum((x_opt - hxTrue) ** 2))
print("Odom localization error: ", np.sum((hxDR - hxTrue) ** 2))
.. parsed-literal::
dx:
[[-0. ]
[-0. ]
[ 0. ]
[ 0.02 ]
[ 0.084]
[-0. ]]
ground truth:
[[0. 1.414]
[0. 1.414]
[0.785 0.985]]
Odom:
[[0. 1.428]
[0. 1.428]
[0.785 0.976]]
SLAM:
[[-0. 1.448]
[-0. 1.512]
[ 0.785 0.976]]
graphSLAM localization error: 0.010729072751057656
Odom localization error: 0.0004460978857535104
The references:
^^^^^^^^^^^^^^^
- http://robots.stanford.edu/papers/thrun.graphslam.pdf
- http://ais.informatik.uni-freiburg.de/teaching/ss13/robotics/slides/16-graph-slam.pdf
- http://www2.informatik.uni-freiburg.de/~stachnis/pdf/grisetti10titsmag.pdf
N.B. An additional step is required that uses the estimated path to
update the belief regarding the map.

216
docs/modules/slam/graphSLAM_formulation.rst Normal file
View File

@@ -0,0 +1,216 @@
.. _Graph SLAM Formulation:
Graph SLAM Formulation
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
Author Jeff Irion
Problem Formulation
^^^^^^^^^^^^^^^^^^^
Let a robot’s trajectory through its environment be represented by a
sequence of :math:`N` poses:
:math:`\mathbf{p}_1, \mathbf{p}_2, \ldots, \mathbf{p}_N`. Each pose lies
on a manifold: :math:`\mathbf{p}_i \in \mathcal{M}`. Simple examples of
manifolds used in Graph SLAM include 1-D, 2-D, and 3-D space, i.e.,
:math:`\mathbb{R}`, :math:`\mathbb{R}^2`, and :math:`\mathbb{R}^3`.
These environments are *rectilinear*, meaning that there is no concept
of orientation. By contrast, in :math:`SE(2)` problem settings a robot’s
pose consists of its location in :math:`\mathbb{R}^2` and its
orientation :math:`\theta`. Similarly, in :math:`SE(3)` a robot’s pose
consists of its location in :math:`\mathbb{R}^3` and its orientation,
which can be represented via Euler angles, quaternions, or :math:`SO(3)`
rotation matrices.
As the robot explores its environment, it collects a set of :math:`M`
measurements :math:`\mathcal{Z} = \{\mathbf{z}_j\}`. Examples of such
measurements include odometry, GPS, and IMU data. Given a set of poses
:math:`\mathbf{p}_1, \ldots, \mathbf{p}_N`, we can compute the estimated
measurement
:math:`\hat{\mathbf{z}}_j(\mathbf{p}_1, \ldots, \mathbf{p}_N)`. We can
then compute the *residual*
:math:`\mathbf{e}_j(\mathbf{z}_j, \hat{\mathbf{z}}_j)` for measurement
:math:`j`. The formula for the residual depends on the type of
measurement. As an example, let :math:`\mathbf{z}_1` be an odometry
measurement that was collected when the robot traveled from
:math:`\mathbf{p}_1` to :math:`\mathbf{p}_2`. The expected measurement
and the residual are computed as
.. math::
\begin{aligned}
\hat{\mathbf{z}}_1(\mathbf{p}_1, \mathbf{p}_2) &= \mathbf{p}_2 \ominus \mathbf{p}_1 \\
\mathbf{e}_1(\mathbf{z}_1, \hat{\mathbf{z}}_1) &= \mathbf{z}_1 \ominus \hat{\mathbf{z}}_1 = \mathbf{z}_1 \ominus (\mathbf{p}_2 \ominus \mathbf{p}_1),\end{aligned}
where the :math:`\ominus` operator indicates inverse pose composition.
We model measurement :math:`\mathbf{z}_j` as having independent Gaussian
noise with zero mean and covariance matrix :math:`\Omega_j^{-1}`; we
refer to :math:`\Omega_j` as the *information matrix* for measurement
:math:`j`. That is,
.. math::
p(\mathbf{z}_j \ | \ \mathbf{p}_1, \ldots, \mathbf{p}_N) = \eta_j \exp (-\mathbf{e}_j(\mathbf{z}_j, \hat{\mathbf{z}}_j))^{\mathsf{T}}\Omega_j \mathbf{e}_j(\mathbf{z}_j, \hat{\mathbf{z}}_j)
:label: infomat
where :math:`\eta_j` is the normalization constant.
The objective of Graph SLAM is to find the maximum likelihood set of
poses given the measurements :math:`\mathcal{Z} = \{\mathbf{z}_j\}`; in
other words, we want to find
.. math:: \mathop{\mathrm{arg\,max}}_{\mathbf{p}_1, \ldots, \mathbf{p}_N} \ p(\mathbf{p}_1, \ldots, \mathbf{p}_N \ | \ \mathcal{Z})
Using Bayes’ rule, we can write this probability as
.. math::
\begin{aligned}
p(\mathbf{p}_1, \ldots, \mathbf{p}_N \ | \ \mathcal{Z}) &= \frac{p( \mathcal{Z} \ | \ \mathbf{p}_1, \ldots, \mathbf{p}_N) p(\mathbf{p}_1, \ldots, \mathbf{p}_N) }{ p(\mathcal{Z}) } \notag \\
&\propto p( \mathcal{Z} \ | \ \mathbf{p}_1, \ldots, \mathbf{p}_N)
\end{aligned}
:label: bayes
since :math:`p(\mathcal{Z})` is a constant (albeit, an unknown constant)
and we assume that :math:`p(\mathbf{p}_1, \ldots, \mathbf{p}_N)` is
uniformly distributed `PROBABILISTIC ROBOTICS`_. Therefore, we
can use Eq. :eq:`infomat` and and Eq. :eq:`bayes` to simplify the Graph SLAM
optimization as follows:
.. math::
\begin{aligned}
\mathop{\mathrm{arg\,max}}_{\mathbf{p}_1, \ldots, \mathbf{p}_N} \ p(\mathbf{p}_1, \ldots, \mathbf{p}_N \ | \ \mathcal{Z}) &= \mathop{\mathrm{arg\,max}}_{\mathbf{p}_1, \ldots, \mathbf{p}_N} \ p( \mathcal{Z} \ | \ \mathbf{p}_1, \ldots, \mathbf{p}_N) \\
&= \mathop{\mathrm{arg\,max}}_{\mathbf{p}_1, \ldots, \mathbf{p}_N} \prod_{j=1}^M p(\mathbf{z}_j \ | \ \mathbf{p}_1, \ldots, \mathbf{p}_N) \\
&= \mathop{\mathrm{arg\,max}}_{\mathbf{p}_1, \ldots, \mathbf{p}_N} \prod_{j=1}^M \exp \left( -(\mathbf{e}_j(\mathbf{z}_j, \hat{\mathbf{z}}_j))^{\scriptstyle{\mathsf{T}}}\Omega_j \mathbf{e}_j(\mathbf{z}_j, \hat{\mathbf{z}}_j) \right) \\
&= \mathop{\mathrm{arg\,min}}_{\mathbf{p}_1, \ldots, \mathbf{p}_N} \sum_{j=1}^M (\mathbf{e}_j(\mathbf{z}_j, \hat{\mathbf{z}}_j))^{\scriptstyle{\mathsf{T}}}\Omega_j \mathbf{e}_j(\mathbf{z}_j, \hat{\mathbf{z}}_j).\end{aligned}
We define
.. math:: \chi^2 := \sum_{j=1}^M (\mathbf{e}_j(\mathbf{z}_j, \hat{\mathbf{z}}_j))^{\scriptstyle{\mathsf{T}}}\Omega_j \mathbf{e}_j(\mathbf{z}_j, \hat{\mathbf{z}}_j),
and this is what we seek to minimize.
Dimensionality and Pose Representation
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Before proceeding further, it is helpful to discuss the dimensionality
of the problem. We have:
- A set of :math:`N` poses
:math:`\mathbf{p}_1, \mathbf{p}_2, \ldots, \mathbf{p}_N`, where each
pose lies on the manifold :math:`\mathcal{M}`
- Each pose :math:`\mathbf{p}_i` is represented as a vector in (a
subset of) :math:`\mathbb{R}^d`. For example:
- An :math:`SE(2)` pose is typically represented as
:math:`(x, y, \theta)`, and thus :math:`d = 3`.
- An :math:`SE(3)` pose is typically represented as
:math:`(x, y, z, q_x, q_y, q_z, q_w)`, where :math:`(x, y, z)`
is a point in :math:`\mathbb{R}^3` and
:math:`(q_x, q_y, q_z, q_w)` is a *quaternion*, and so
:math:`d = 7`. For more information about :math:`SE(3)`
parameterization and pose transformations, see
[blanco2010tutorial]_.
- We also need to be able to represent each pose compactly as a
vector in (a subset of) :math:`\mathbb{R}^c`.
- Since an :math:`SE(2)` pose has three degrees of freedom, the
:math:`(x, y, \theta)` representation is again sufficient and
:math:`c=3`.
- An :math:`SE(3)` pose only has six degrees of freedom, and we
can represent it compactly as :math:`(x, y, z, q_x, q_y, q_z)`,
and thus :math:`c=6`.
- We use the :math:`\boxplus` operator to indicate pose composition
when one or both of the poses are represented compactly. The
output can be a pose in :math:`\mathcal{M}` or a vector in
:math:`\mathbb{R}^c`, as required by context.
- A set of :math:`M` measurements
:math:`\mathcal{Z} = \{\mathbf{z}_1, \mathbf{z}_2, \ldots, \mathbf{z}_M\}`
- Each measurement’s dimensionality can be unique, and we will use
:math:`\bullet` to denote a “wildcard” variable.
- Measurement :math:`\mathbf{z}_j \in \mathbb{R}^\bullet` has an
associated information matrix
:math:`\Omega_j \in \mathbb{R}^{\bullet \times \bullet}` and
residual function
:math:`\mathbf{e}_j(\mathbf{z}_j, \hat{\mathbf{z}}_j) = \mathbf{e}_j(\mathbf{z}_j, \mathbf{p}_1, \ldots, \mathbf{p}_N) \in \mathbb{R}^\bullet`.
- A measurement could, in theory, constrain anywhere from 1 pose to
all :math:`N` poses. In practice, each measurement usually
constrains only 1 or 2 poses.
Graph SLAM Algorithm
^^^^^^^^^^^^^^^^^^^^
The “Graph” in Graph SLAM refers to the fact that we view the problem as
a graph. The graph has a set :math:`\mathcal{V}` of :math:`N` vertices,
where each vertex :math:`v_i` has an associated pose
:math:`\mathbf{p}_i`. Similarly, the graph has a set :math:`\mathcal{E}`
of :math:`M` edges, where each edge :math:`e_j` has an associated
measurement :math:`\mathbf{z}_j`. In practice, the edges in this graph
are either unary (i.e., a loop) or binary. (Note: :math:`e_j` refers to
the edge in the graph associated with measurement :math:`\mathbf{z}_j`,
whereas :math:`\mathbf{e}_j` refers to the residual function associated
with :math:`\mathbf{z}_j`.) For more information about the Graph SLAM
algorithm, see [grisetti2010tutorial]_.
We want to optimize
.. math:: \chi^2 = \sum_{e_j \in \mathcal{E}} \mathbf{e}_j^{\scriptstyle{\mathsf{T}}}\Omega_j \mathbf{e}_j.
Let :math:`\mathbf{x}_i \in \mathbb{R}^c` be the compact representation
of pose :math:`\mathbf{p}_i \in \mathcal{M}`, and let
.. math:: \mathbf{x} := \begin{bmatrix} \mathbf{x}_1 \\ \mathbf{x}_2 \\ \vdots \\ \mathbf{x}_N \end{bmatrix} \in \mathbb{R}^{cN}
We will solve this optimization problem iteratively. Let
.. math:: \mathbf{x}^{k+1} := \mathbf{x}^k \boxplus \Delta \mathbf{x}^k = \begin{bmatrix} \mathbf{x}_1 \boxplus \Delta \mathbf{x}_1 \\ \mathbf{x}_2 \boxplus \Delta \mathbf{x}_2 \\ \vdots \\ \mathbf{x}_N \boxplus \Delta \mathbf{x}_2 \end{bmatrix}
:label: update
The :math:`\chi^2` error at iteration :math:`k+1` is
.. math:: \chi_{k+1}^2 = \sum_{e_j \in \mathcal{E}} \underbrace{\left[ \mathbf{e}_j(\mathbf{x}^{k+1}) \right]^{\scriptstyle{\mathsf{T}}}}_{1 \times \bullet} \underbrace{\Omega_j}_{\bullet \times \bullet} \underbrace{\mathbf{e}_j(\mathbf{x}^{k+1})}_{\bullet \times 1}.
:label: chisq_at_kplusone
We will linearize the residuals as:
.. math::
\begin{aligned}
\mathbf{e}_j(\mathbf{x}^{k+1}) &= \mathbf{e}_j(\mathbf{x}^k \boxplus \Delta \mathbf{x}^k) \\
&\approx \mathbf{e}_j(\mathbf{x}^{k}) + \frac{\partial}{\partial \Delta \mathbf{x}^k} \left[ \mathbf{e}_j(\mathbf{x}^k \boxplus \Delta \mathbf{x}^k) \right] \Delta \mathbf{x}^k \\
&= \mathbf{e}_j(\mathbf{x}^{k}) + \left( \left. \frac{\partial \mathbf{e}_j(\mathbf{x}^k \boxplus \Delta \mathbf{x}^k)}{\partial (\mathbf{x}^k \boxplus \Delta \mathbf{x}^k)} \right|_{\Delta \mathbf{x}^k = \mathbf{0}} \right) \frac{\partial (\mathbf{x}^k \boxplus \Delta \mathbf{x}^k)}{\partial \Delta \mathbf{x}^k} \Delta \mathbf{x}^k.
\end{aligned}
:label: linearization
Plugging :eq:`linearization` into :eq:`chisq_at_kplusone`, we get:
.. math::
\begin{aligned}
\chi_{k+1}^2 &\approx \ \ \ \ \ \sum_{e_j \in \mathcal{E}} \underbrace{[ \mathbf{e}_j(\mathbf{x}^k)]^{\scriptstyle{\mathsf{T}}}}_{1 \times \bullet} \underbrace{\Omega_j}_{\bullet \times \bullet} \underbrace{\mathbf{e}_j(\mathbf{x}^k)}_{\bullet \times 1} \notag \\
&\hphantom{\approx} \ \ \ + \sum_{e_j \in \mathcal{E}} \underbrace{[ \mathbf{e}_j(\mathbf{x^k}) ]^{\scriptstyle{\mathsf{T}}}}_{1 \times \bullet} \underbrace{\Omega_j}_{\bullet \times \bullet} \underbrace{\left( \left. \frac{\partial \mathbf{e}_j(\mathbf{x}^k \boxplus \Delta \mathbf{x}^k)}{\partial (\mathbf{x}^k \boxplus \Delta \mathbf{x}^k)} \right|_{\Delta \mathbf{x}^k = \mathbf{0}} \right)}_{\bullet \times dN} \underbrace{\frac{\partial (\mathbf{x}^k \boxplus \Delta \mathbf{x}^k)}{\partial \Delta \mathbf{x}^k}}_{dN \times cN} \underbrace{\Delta \mathbf{x}^k}_{cN \times 1} \notag \\
&\hphantom{\approx} \ \ \ + \sum_{e_j \in \mathcal{E}} \underbrace{(\Delta \mathbf{x}^k)^{\scriptstyle{\mathsf{T}}}}_{1 \times cN} \underbrace{ \left( \frac{\partial (\mathbf{x}^k \boxplus \Delta \mathbf{x}^k)}{\partial \Delta \mathbf{x}^k} \right)^{\scriptstyle{\mathsf{T}}}}_{cN \times dN} \underbrace{\left( \left. \frac{\partial \mathbf{e}_j(\mathbf{x}^k \boxplus \Delta \mathbf{x}^k)}{\partial (\mathbf{x}^k \boxplus \Delta \mathbf{x}^k)} \right|_{\Delta \mathbf{x}^k = \mathbf{0}} \right)^{\scriptstyle{\mathsf{T}}}}_{dN \times \bullet} \underbrace{\Omega_j}_{\bullet \times \bullet} \underbrace{\left( \left. \frac{\partial \mathbf{e}_j(\mathbf{x}^k \boxplus \Delta \mathbf{x}^k)}{\partial (\mathbf{x}^k \boxplus \Delta \mathbf{x}^k)} \right|_{\Delta \mathbf{x}^k = \mathbf{0}} \right)}_{\bullet \times dN} \underbrace{\frac{\partial (\mathbf{x}^k \boxplus \Delta \mathbf{x}^k)}{\partial \Delta \mathbf{x}^k}}_{dN \times cN} \underbrace{\Delta \mathbf{x}^k}_{cN \times 1} \notag \\
&= \chi_k^2 + 2 \mathbf{b}^{\scriptstyle{\mathsf{T}}}\Delta \mathbf{x}^k + (\Delta \mathbf{x}^k)^{\scriptstyle{\mathsf{T}}}H \Delta \mathbf{x}^k, \notag\end{aligned}
where
.. math::
\begin{aligned}
\mathbf{b}^{\scriptstyle{\mathsf{T}}}&= \sum_{e_j \in \mathcal{E}} \underbrace{[ \mathbf{e}_j(\mathbf{x^k}) ]^{\scriptstyle{\mathsf{T}}}}_{1 \times \bullet} \underbrace{\Omega_j}_{\bullet \times \bullet} \underbrace{\left( \left. \frac{\partial \mathbf{e}_j(\mathbf{x}^k \boxplus \Delta \mathbf{x}^k)}{\partial (\mathbf{x}^k \boxplus \Delta \mathbf{x}^k)} \right|_{\Delta \mathbf{x}^k = \mathbf{0}} \right)}_{\bullet \times dN} \underbrace{\frac{\partial (\mathbf{x}^k \boxplus \Delta \mathbf{x}^k)}{\partial \Delta \mathbf{x}^k}}_{dN \times cN} \\
H &= \sum_{e_j \in \mathcal{E}} \underbrace{ \left( \frac{\partial (\mathbf{x}^k \boxplus \Delta \mathbf{x}^k)}{\partial \Delta \mathbf{x}^k} \right)^{\scriptstyle{\mathsf{T}}}}_{cN \times dN} \underbrace{\left( \left. \frac{\partial \mathbf{e}_j(\mathbf{x}^k \boxplus \Delta \mathbf{x}^k)}{\partial (\mathbf{x}^k \boxplus \Delta \mathbf{x}^k)} \right|_{\Delta \mathbf{x}^k = \mathbf{0}} \right)^{\scriptstyle{\mathsf{T}}}}_{dN \times \bullet} \underbrace{\Omega_j}_{\bullet \times \bullet} \underbrace{\left( \left. \frac{\partial \mathbf{e}_j(\mathbf{x}^k \boxplus \Delta \mathbf{x}^k)}{\partial (\mathbf{x}^k \boxplus \Delta \mathbf{x}^k)} \right|_{\Delta \mathbf{x}^k = \mathbf{0}} \right)}_{\bullet \times dN} \underbrace{\frac{\partial (\mathbf{x}^k \boxplus \Delta \mathbf{x}^k)}{\partial \Delta \mathbf{x}^k}}_{dN \times cN}.\end{aligned}
Using this notation, we obtain the optimal update as
.. math:: \Delta \mathbf{x}^k = -H^{-1} \mathbf{b}. \label{eq:deltax}
We apply this update to the poses via :eq:`update` and repeat until convergence.
.. _PROBABILISTIC ROBOTICS: http://www.probabilistic-robotics.org/

34
docs/modules/slam/slam.rst → docs/modules/slam/slam_main.rst
View File

@@ -21,32 +21,12 @@ Ref:
- `Introduction to Mobile Robotics: Iterative Closest Point Algorithm`_
EKF SLAM
--------
This is an Extended Kalman Filter based SLAM example.
.. include:: ekf_slam.rst
The blue line is ground truth, the black line is dead reckoning, the red
line is the estimated trajectory with EKF SLAM.
The green crosses are estimated landmarks.
|4|
Ref:
- `PROBABILISTIC ROBOTICS`_
.. include:: FastSLAM1.rst
|5|
Ref:
- `PROBABILISTIC ROBOTICS`_
- `SLAM simulations by Tim Bailey`_
FastSLAM 2.0
------------
@@ -56,7 +36,8 @@ The animation has the same meanings as one of FastSLAM 1.0.
|6|
Ref:
References
~~~~~~~~~~
- `PROBABILISTIC ROBOTICS`_
@@ -77,6 +58,10 @@ The black stars are landmarks for graph edge generation.
|7|
.. include:: graphSLAM_doc.rst
.. include:: graphSLAM_formulation.rst
.. include:: graphSLAM_SE2_example.rst
Ref:
- `A Tutorial on Graph-Based SLAM`_
@@ -85,9 +70,12 @@ Ref:
.. _PROBABILISTIC ROBOTICS: http://www.probabilistic-robotics.org/
.. _SLAM simulations by Tim Bailey: http://www-personal.acfr.usyd.edu.au/tbailey/software/slam_simulations.htm
.. _A Tutorial on Graph-Based SLAM: http://www2.informatik.uni-freiburg.de/~stachnis/pdf/grisetti10titsmag.pdf
.. _FastSLAM Lecture: http://ais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/slam10-fastslam.pdf
.. [blanco2010tutorial] Blanco, J.-L.A tutorial onSE(3) transformation parameterization and on-manifold optimization.University of Malaga, Tech. Rep 3(2010)
.. [grisetti2010tutorial] Grisetti, G., Kummerle, R., Stachniss, C., and Burgard, W.A tutorial on graph-based SLAM.IEEE Intelligent Transportation Systems Magazine 2, 4 (2010), 31–43.
.. |3| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/SLAM/iterative_closest_point/animation.gif
.. |4| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/SLAM/EKFSLAM/animation.gif
.. |5| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/SLAM/FastSLAM1/animation.gif
.. |6| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/SLAM/FastSLAM2/animation.gif
.. |7| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/SLAM/GraphBasedSLAM/animation.gif