{ "cells": [ { "cell_type": "markdown", "metadata": {}, "source": [ "> [back](../README.md)\n", "\n", "# gs-rs [ˈdʒiːzrs] \n", "\n", "## Contact\n", "Samuel Valenzuela, Florian Rohm , Dr. Daniel Pape, daniel.pape(at)tngtech.com\n", "\n", "TNG Technology Consulting GmbH\n" ] }, { "cell_type": "markdown", "source": [ "## Abstract\n", "**gs-rs** is a Rust library for the optimization of non-linear least squares problems embeddable\n", "as a (hyper)graph.\n", "\n", "It is suitable for the optimization of an error function with respect to a set of parameters as in\n", "SLAM (simultaneous localization and mapping) or BA (bundle adjustment).\n" ], "metadata": { "collapsed": false } }, { "cell_type": "markdown", "source": [ "## Background Literature\n", "The optimization algorithm used by **gs-rs** is based on parts of **g2o**. This project should suffice to understand the optimization's implementation in **gs-rs**. \n", "The following papers by the developers of **g2o** are recommended if a deeper \n", "understanding of the theory behind the algorithm is of interest:\n", "* _g2o: A General Framework for Graph Optimization_, Kümmerle et al.: This paper documents the derivation of the algorithm's structure.\n", "* _A Tutorial on Graph-Based SLAM_, Grisetti et al.: This paper contains additional comments on the calculations in 2D and 3D. Here it is presented how the least squares optimization works on a manifold.\n" ], "metadata": { "collapsed": false } }, { "cell_type": "markdown", "source": [ "## Hypergraph-Embeddable Optimization Problems\n", "### Least Squares Optimization Problems\n", "#### Definition\n", "A least squares optimization problem with a set of constraints $\\mathcal{C}$ can be generally formulated as:\n", "\n", "\\begin{align}\n", " F(\\chi) &= \\sum_{k\\in\\mathcal{C}}e_k(\\chi_k, z_k)^{T}\\Omega_k e_k(\\chi_k, z_k)\\\\\n", " {\\chi}^* &= \\text{argmin}_{\\chi} F(\\chi),\n", "\\end{align}\n", "\n", "where\n", "\n", "\\begin{align}\n", " \\chi = (\\chi_1^T,\\dots,\\chi_n^T)^T\n", "\\end{align}\n", "\n", "is the structured vector of parameters subject to optimization and\n", "\n", "\\begin{align}\n", " \\chi_k = (\\chi_{k_1}^T,\\dots,\\chi_{k_q}^T)^T\n", "\\end{align}\n", "\n", "is the structured vector of parameter blocks subject to the constraint $k\\in\\mathcal{C}$.\n", "\n", "$z_k$ and $\\Omega_k$ denote the mean and information matrix of constraint $k$, respectively.\n", "\n", "Finally, $e_k(\\chi_k, z_k)$ is a vector error function measuring how well the parameter blocks\n", "$\\chi_k$ satisfy the constraint $k$.\n", "\n", "#### Linearization of the Error Function\n", "Given an initial guess $\\hat{\\chi}$ of the parameter set $\\chi$, we can expand the error function\n", "by means of a first order _Taylor_ expansion:\n", "\n", "\\begin{align}\n", " e_k(\\chi_k) &= e_k(\\hat{\\chi_k}+\\Delta\\chi_k)\\\\\n", " &\\approx e_k(\\hat{\\chi_k})+J_{k}\\Delta\\chi_k,\n", "\\end{align}\n", "\n", "where $J_{k}=\\nabla e_k(\\hat{\\chi_k})$ is the \n", "_Jacobian_ computed at $\\hat{\\chi}_k$.\n", "\n", "Insertion into formulation of the optimization problem yields:\n", "\n", "\\begin{align}\n", " F_k(\\hat{\\chi_k}+\\Delta\\chi_k) &= e_k(\\hat{\\chi_k}+\\Delta\\chi_k)^{T}\\Omega_k e_k(\\hat{\\chi_k}+\\Delta\\chi_k)\\\\\n", " &\\approx \\left(e_k(\\hat{\\chi_k})+J_{k}\\Delta\\chi_{k}\\right)^T\\Omega_k\\left(e_k(\\hat{\\chi_k})+J_{k}\\Delta\\chi_{k}\\right)\\\\\n", " &= \\underbrace{e_k(\\hat{\\chi_k})^T\\Omega_k e_k(\\hat{\\chi_k})}_{c_k}\n", " + 2\\underbrace{e_k(\\hat{\\chi_k})^{T}\\Omega_{k}J_k}_{b_k}\\Delta \\chi_k\n", " + \\Delta\\chi_k^T \\underbrace{J_k^T\\Omega_k J_k}_{H_k}\\Delta \\chi_k\\\\\n", " &= c_k + 2b_{k}\\Delta \\chi_k + \\Delta \\chi_k^T H_k \\Delta \\chi_k.\n", "\\end{align}\n", "\n", "Generalizing this result to the set of all constraints yields:\n", "\n", "\\begin{align}\n", " F(\\hat{\\chi}+\\Delta \\chi) &= \\sum_{k\\in\\mathcal{C}} F_k(\\hat{\\chi_k}+ \\Delta\\chi_k)\\\\\n", " &= \\sum_{k\\in\\mathcal{C}} c_k + 2b_{k}\\Delta \\chi_k + \\Delta \\chi_k^T H_k \\Delta \\chi_k\\\\\n", " &= c + 2b^T\\Delta\\chi + \\Delta \\chi^T H \\Delta \\chi.\n", "\\end{align}\n", "\n", "where\n", "\n", "\\begin{align}\n", " c &= \\sum_{k\\in\\mathcal{C}}c_{k}\\\\\n", " b &= \\bigoplus_{k\\in\\mathcal{C}}b_{k}\\\\\n", " \\Delta \\chi &= \\bigoplus_{k\\in\\mathcal{C}}\\Delta \\chi_{k}\\\\\n", " H &= \\bigoplus_{k\\in\\mathcal{C}} H_{k}.\n", "\\end{align}\n", "\n", "Here, $\\bigoplus$ denotes an addition of blocks for vectors and matrices, where the position is derived from the constraint index.\n", "\n", "We need to minimize $F(\\hat\\chi+\\Delta\\chi)$ in $\\Delta\\chi$. It can be done by solving the linear system\n", "\n", "\\begin{align}\n", " H\\Delta\\chi^\\star = -b\n", "\\end{align}\n", "\n", "#### Example\n", "Let us consider a system of two parameter blocks $\\chi_0$, $\\chi_1$ (e.g. poses in two dimensions)\n", "and three constraints $z_0$, $z_1$ (e.g. position measurements), $z_{01}$ (e.g. an odometry measurement), where the first two constraints\n", "control only one parameter block each and the\n", "last constraint connects the two parameter blocks.\n", "\n", "We will set $z_{0}=(0,1)^T, z_1 = (1,0)^T, z_{01} = (0.5, -0.5)^T$.\n", "Thus, we have a system, where the position measurements are further apart\n", "than the odometry requires.\n", "\n", "The information matrices are defined as follows:\n", "\n", "\\begin{align}\n", " \\Omega_{0} &= \\begin{pmatrix}\n", " 10 & 0 \\\\ 0 & 10\n", " \\end{pmatrix}\\\\\n", " \\Omega_{1} &= \\begin{pmatrix}\n", " 10 & 0 \\\\ 0 & 10\n", " \\end{pmatrix}\\\\\n", " \\Omega_{01} &=\\begin{pmatrix}\n", " 1 & 0 \\\\ 0 & 1\n", " \\end{pmatrix}\n", "\\end{align}\n", "\n", "That is, the odometry measurement is considered ten times less precise than the position measurement.\n", "\n", "We will define the error functions\n", "\n", "\\begin{align}\n", " e_0(\\chi_0,z_0) &= z_0 - \\chi_0 \\\\\n", " e_1(\\chi_1,z_1) &= z_1 - \\chi_1 \\\\\n", " e_{01}(\\chi_{01},z_{01}) &= z_{01} - (\\chi_1 - \\chi_0).\n", "\\end{align}\n", "\n", "Thus, for the _Jacobian_'s we find\n", "\n", "\\begin{align}\n", "J_0 &= \\begin{pmatrix}\n", " \\frac{\\partial (z_0 - \\chi_0)_x}{\\partial (\\chi_0)_x} & \\frac{\\partial (z_0 - \\chi_0)_x}{\\partial (\\chi_0)_y} \\\\\n", " \\frac{\\partial (z_0 - \\chi_0)_y}{\\partial (\\chi_0)_x} & \\frac{\\partial (z_0 - \\chi_0)_y}{\\partial (\\chi_0)_y}\n", "\\end{pmatrix}\\newline\n", "&= \\begin{pmatrix}\n", " 1 & 0 \\\\ 0 & 1\n", "\\end{pmatrix}\\newline\n", "J_1 &= \\begin{pmatrix}\n", " 1 & 0 \\\\ 0 & 1\n", "\\end{pmatrix}\\newline\n", "J_{01} &= \\begin{pmatrix}\n", " \\frac{\\partial (z_0 - (\\chi_1 - \\chi_0))_x}{\\partial (\\chi_0)_x} &\n", " \\frac{\\partial (z_0 - (\\chi_1 - \\chi_0))_x}{\\partial (\\chi_0)_y} &\n", " \\frac{\\partial (z_0 - (\\chi_1 - \\chi_0))_x}{\\partial (\\chi_1)_x} &\n", " \\frac{\\partial (z_0 - (\\chi_1 - \\chi_0))_x}{\\partial (\\chi_1)_y} \\\\\n", " \\frac{\\partial (z_0 - (\\chi_1 - \\chi_0))_y}{\\partial (\\chi_0)_x} &\n", " \\frac{\\partial (z_0 - (\\chi_1 - \\chi_0))_y}{\\partial (\\chi_0)_y} &\n", " \\frac{\\partial (z_0 - (\\chi_1 - \\chi_0))_y}{\\partial (\\chi_1)_x} &\n", " \\frac{\\partial (z_0 - (\\chi_1 - \\chi_0))_y}{\\partial (\\chi_1)_y}\n", "\\end{pmatrix}\\\\\n", "&= \\begin{pmatrix}\n", " -1 & 0 & 1 & 0 \\\\ 0 & -1 & 0 & 1\n", "\\end{pmatrix}\n", "\\end{align}\n", "\n", "Let us apply the position constraints as initial guesses of the pose variables:\n", "\n", "\\begin{align}\n", " \\hat{\\chi_0} &= z_0 = (0,1)^{T}\\\\\n", " \\hat{\\chi_1} &= z_1 = (1,0)^{T}.\n", "\\end{align}\n", "\n", "Now we can calculate the components of our target equation ?:\n", "\n", "\\begin{align}\n", " b_0 &= e_0^T \\cdotp \\Omega_0 J_{0}\\\\\n", " &= (0,0) \\cdotp \\begin{pmatrix}\n", " 10 & 0 \\\\ 0 & 10\n", " \\end{pmatrix} \\cdotp \\begin{pmatrix}\n", " 1 & 0 \\\\ 0 & 1\n", " \\end{pmatrix}\\newline\n", " &= (0,0).\\newline\n", " b_1 &= e_1^T \\cdotp \\Omega_{1} J_{1}\\newline\n", " &= (0,0) \\cdotp \\begin{pmatrix}\n", " 10 & 0 \\\\ 0 & 10\n", " \\end{pmatrix} \\cdotp \\begin{pmatrix}\n", " 1 & 0 \\\\ 0 & 1\n", " \\end{pmatrix}\\newline\n", " &= (0,0).\\\\\n", " b_{01} &= e_{01}^T \\cdotp \\Omega_{01} J_{01}\\newline\n", " &= \\left( (0.5, -0.5) - \\left( (1,0) - (0,1) \\right) \\right)\\cdotp \\begin{pmatrix}\n", " 1 & 0 \\\\ 0 & 1\n", " \\end{pmatrix} \\cdotp \\begin{pmatrix}\n", " 1 & 0 & -1 & 0 \\\\ 0 & 1 & 0 & -1\n", " \\end{pmatrix}\\newline\n", " &= (-0.5, 0.5)\\cdotp \\begin{pmatrix}\n", " 1 & 0 \\\\ 0 & 1\n", " \\end{pmatrix} \\cdotp \\begin{pmatrix}\n", " 1 & 0 & -1 & 0 \\\\ 0 & 1 & 0 & -1\n", " \\end{pmatrix}\\newline\n", " &= (-0.5, 0.5, 0.5, -0.5).\\newline\n", " \\Rightarrow b &= b_0 \\oplus b_1 \\oplus b_{01}\\newline\n", " &= (0,0) \\oplus (0,0) \\oplus (-0.5, 0.5, 0.5, -0.5)\\newline\n", " &= (-0.5, 0.5, 0.5, -0.5)\\newline\n", " H_0 &= J_{0}^T \\Omega_0 J_{0}\\newline\n", " &= \\begin{pmatrix}\n", " -1 & 0 \\\\ 0 & -1\n", " \\end{pmatrix} \\cdotp \\begin{pmatrix}\n", " 10 & 0 \\\\ 0 & 10\n", " \\end{pmatrix} \\cdotp \\begin{pmatrix}\n", " -1 & 0 \\\\ 0 & -1\n", " \\end{pmatrix}\\newline\n", " &= \\begin{pmatrix}\n", " 10 & 0 \\\\ 0 & 10\n", " \\end{pmatrix}.\\newline\n", " H_1 &= J_{1}^T \\Omega_1 J_{1}\\newline\n", " &= \\begin{pmatrix}\n", " 10 & 0 \\\\ 0 & 10\n", " \\end{pmatrix}.\\newline\n", " H_{01} &= J_{01}^T \\Omega_{01} J_{01}\\newline\n", " &= \\begin{pmatrix}\n", " 1 & 0 \\\\ 0 & 1 \\\\ -1 & 0 \\\\ 0 & -1\n", " \\end{pmatrix} \\cdotp \\begin{pmatrix}\n", " 1 & 0 \\\\ 0 & 1\n", " \\end{pmatrix} \\cdotp \\begin{pmatrix}\n", " 1 & 0 & -1 & 0 \\\\ 0 & 1 & 0 & -1\n", " \\end{pmatrix}\\newline\n", " &= \\begin{pmatrix}\n", " 1 & 0 & -1 & 0\\\\ 0 & 1 & 0 & -1 \\\\ -1 & 0 & 1 & 0 \\\\ 0 & -1 & 0 & 1\n", " \\end{pmatrix}.\\newline\n", " \\Rightarrow H &= H_0 \\oplus H_1 \\oplus H_{01}\\\\\n", " &= \\begin{pmatrix}\n", " 10 & 0 \\\\ 0 & 10\n", " \\end{pmatrix} \\oplus \\begin{pmatrix}\n", " 10 & 0 \\\\ 0 & 10\n", " \\end{pmatrix} \\oplus \\begin{pmatrix}\n", " 1 & 0 & -1 & 0\\\\ 0 & 1 & 0 & -1 \\\\ -1 & 0 & 1 & 0 \\\\ 0 & -1 & 0 & 1\n", " \\end{pmatrix}\\newline\n", " & = \\begin{pmatrix}\n", " 11 & 0 & -1 & 0\\\\ 0 & 11 & 0 & -1 \\\\ -1 & 0 & 11 & 0 \\\\ 0 & -1 & 0 & 11\n", " \\end{pmatrix}\n", "\\end{align}\n", "\n", "Now, the last step is to solve\n", "\n", "\\begin{align}\n", " H\\Delta \\chi^* &= -b\\newline\n", " \\begin{pmatrix}\n", " 11 & 0 & -1 & 0\\\\ 0 & 11 & 0 & -1 \\\\ -1 & 0 & 11 & 0 \\\\ 0 & -1 & 0 & 11\n", " \\end{pmatrix} \\Delta \\chi^* &=\n", " \\begin{pmatrix}\n", " 0.5 \\\\ -0.5 \\\\ -0.5 \\\\ 0.5\n", " \\end{pmatrix} \\newline\n", " \\Rightarrow \\Delta \\chi^* &= \\frac{1}{24} \\begin{pmatrix}\n", " 1 \\\\ -1 \\\\ -1 \\\\ 1\n", " \\end{pmatrix}.\n", "\\end{align}\n", "\n", "and apply the update\n", "\n", "\\begin{align}\n", " \\chi^{(1)} &= \\hat{\\chi} + \\Delta \\chi^{*}\\\\\n", " &= (0,1,1,0)^T + \\frac{1}{24}(1, -1, -1, 1)^{T}\\\\\n", " &\\approx (0.04, 0.96, 0.96, 0.04).\n", "\\end{align}\n", "\n", "Thus, our pose variables moved a little closer together, driven by the odometry constraint.\n" ], "metadata": { "collapsed": false, "pycharm": { "name": "#%% md\n" } } }, { "cell_type": "markdown", "source": [ "\n", "## Optimization Algorithm\n", "In the following sections, the optimization algorithm will be introduced. Firstly, the structure's main\n", "iteration steps will be introduced. Most of the work happens in the first step, the calculation of\n", "$H$ and $b$. Subsequently, the calculations which apply to all dimensions and factor types will be presented.\n", "\n", "### Iteration Steps\n", "Given a specific number of iterations $n$ and the initial guess $x_i^{(0)}$ for each variable, the optimizer algorithm will repeat the following steps $n$ times:\n", "\n", "1. Calculate $H$ and $b$ by setting them to $\\boldsymbol{0}$, then looping through all factors and updating their non-fixed variables' entries in $H$ and $b$.\n", "2. Calculate $\\Delta x$, the vector containing data about how much each current variable guess $x_i^{(k)}$ should be updated in this step, by solving the linear system\n", "\\begin{align}\n", "\tH \\Delta x = -b^T.\n", "\\end{align}\n", "3. Update the guesses for each non-fixed variable $x_i$ with\n", "\\begin{align}\n", "\tx_i^{(k+1)} = x_i^{(k)} + \\Delta x_i.\n", "\\end{align}\n", "\n", "In the case of 2D variables with a rotation, normalize the rotation to $[-\\pi, \\pi)$.\n", "In the case of 3D variables with a rotation, normalize the rotation of $\\Delta x_i$. \n", "As explained in a later section, the quaternion data will only include the imaginary part. \n", "Therefore, set the scalar part to $1$ before normalizing. The $+$ operator then resembles the concatenation of two isometries, \n", "analogously as the $\\circ$ operator later.\n", "\n", "\n", "\n", "### Calculation of $\\boldsymbol{H}$ and $\\boldsymbol{b}$\n", "How exactly the variables' entries in $H$ and $b$ are updated depends on the factor type. In all cases the factor's increments on \n", "parts of $H$ and $b$, $H^{\\text{fac}}$ and $b^{\\text{fac}}$,\n", "respectively, will be computed as follows:\n", "\n", "\\begin{align}\n", "H^{\\text{fac}} = J^T \\cdot \\Omega \\cdot J,\n", "\\end{align}\n", "\n", "\\begin{align}\n", "b^{\\text{fac}} = e^T \\cdot \\Omega \\cdot J,\n", "\\end{align}\n", "\n", "where $\\Omega$, $J$ and $e$ are the factor's information matrix, Jacobian matrix and error vector, respectively. While $\\Omega$ \n", "is a given constant of the factor, $J$ and $e$ have to be \n", "calculated for each factor in each iteration.\n", "\n", "If the factor only involves the variable $x_i$, $H$ and $b$ are updated as follows:\n", "\n", "\\begin{align}\n", "H_{ii} = H_{ii} + H^{\\text{fac}},\n", "\\end{align}\n", "\n", "\\begin{align}\n", "b_i = b_i + b^{\\text{fac}},\n", "\\end{align}\n", "\n", "where the subscripts of $H$ and $b$ denote the row and column index of the submatrix or subvector assigned to the respective variable. \n", "If the factor involves two variables $x_i$ and\n", "$x_j$, $H^{\\text{fac}}$ and $b^{\\text{fac}}$ will have the structure\n", "\n", "\\begin{align}\n", "H^{fac} =\n", "\\begin{pmatrix}\n", "\\boldsymbol{H}^{\\text{fac}}_{ii} & \\boldsymbol{H}^{\\text{fac}}_{ij}\\\\\n", "\\boldsymbol{H}^{\\text{fac}}_{ji} & \\boldsymbol{H}^{\\text{fac}}_{jj}\n", "\\end{pmatrix}\n", "\\end{align}\n", "\n", "and\n", "\n", "\\begin{align}\n", "b^{fac} =\n", "\\begin{pmatrix}\n", "\\boldsymbol{b}^{fac}_{i} & \\boldsymbol{b}^{fac}_{j}\n", "\\end{pmatrix},\n", "\\end{align}\n", "\n", "respectively, such that $H_{mn}$ will be incremented by $H^{\\text{fac}}_{mn}$ and $b_n$ \n", "will be incremented by $b^{\\text{fac}}_n$.\n", "Fixed variables are excluded from $H$ and $b$ and therefore do not have any submatrices or subvectors which would need to be updated. \n", "In this case, these parts of $H^{\\text{fac}}$ and $b^{\\text{fac}}$\n", "are simply ignored.\n", "\n", "In the following sections, the calculation is described for all 2D and 3D factors supported by **gs-rs**." ], "metadata": { "collapsed": false } }, { "cell_type": "markdown", "source": [ "## Optimization in 2D\n", "In the following sections, the individual 2D factors' calculations of $J$ and $e$ are presented. The functions $\\text{pos}(x)$ and $\\text{rot}(x)$ will be used to refer to the 2D position vector and the rotation angle of a 2D pose $x$, respectively. \n", "Similarly, the functions $\\text{pos}_x(x)$ and $\\text{pos}_y(x)$ will be used to refer to the single value within the respective dimension.\n", "\n", "Furthermore, the rotation matrices are denoted such that\n", "\n", "\\begin{align}\n", "\tR_\\alpha =\n", "\t\\begin{pmatrix}\n", "\t\t\\text{cos}(\\alpha) & -\\text{sin}(\\alpha)\\\\\n", "\t\t\\text{sin}(\\alpha) & \\text{cos}(\\alpha)\n", "\t\\end{pmatrix}\n", "\\end{align}\n", "\n", "equals the rotation matrix with the angle $\\alpha$ in 2D, and\n", "\n", "\\begin{align}\n", "\tR_z(\\alpha) =\n", "\t\\begin{pmatrix}\n", "\t\t\\text{cos}(\\alpha) & -\\text{sin}(\\alpha) & 0\\\\\n", "\t\t\\text{sin}(\\alpha) & \\text{cos}(\\alpha) & 0\\\\\n", "\t\t 0 & 0 & 1\n", "\t\\end{pmatrix}\n", "\\end{align}\n", "\n", "equals the rotation matrix for a rotation around the $z$ axis with the angle $\\alpha$ in 3D.\n", "\n", "### Position2D\n", "The _Position2D_ factor involves one _VehicleVariable_ $x_v$. \n", "The Jacobian matrix $J$ in this case is\n", "\n", "\\begin{align}\n", "\tJ = R_z(-\\text{rot}(x_v)).\n", "\\end{align}\n", "\n", "Given the measurement $x_m$, the error vector\n", "\n", "\\begin{align}\n", "\te = R_{-\\text{rot}(x_m)} \\circ (\\text{pos}(x_v) - \\text{pos}(x_m))\n", "\\end{align}\n", "\n", "can be computed as well.\n", "\n", "\n", "\n", "### Odometry2D\n", "\n", "The _Odometry2D_ factor involves two _VehicleVariable_s $x_i$ and $x_j$. \n", "Given the measurement $x_{ij}$, the Jacobian matrix $J$ is calculated as follows:\n", "\n", "\\begin{align}\n", "\t\\Delta x_{ij} = x_j - x_i\n", "\\end{align}\n", "\n", "\\begin{align}\n", "\t\\text{sin}_i = \\text{sin}(\\text{rot}(x_i))\n", "\\end{align}\n", "\n", "\\begin{align}\n", "\t\\text{cos}_i = \\text{cos}(\\text{rot}(x_i))\n", "\\end{align}\n", "\n", "\\begin{align}\n", "\t@inproceedings{kummerle2011g2o,\n", " title={g2o: A general framework for graph optimization},\n", " author={Kümmerle, Rainer and Grisetti, Giorgio and Strasdat, Hauke and Konolige, Kurt and Burgard, Wolfram},\n", " booktitle={2011 IEEE International Conference on Robotics and Automation},\n", " pages={3607--3613},\n", " year={2011},\n", " organization={IEEE}\n", " }\n", " \n", " @article{grisetti2010tutorial,\n", " title={A tutorial on graph-based SLAM},\n", " author={Grisetti, Giorgio and Kummerle, Rainer and Stachniss, Cyrill and Burgard, Wolfram},\n", " journal={IEEE Intelligent Transportation Systems Magazine},\n", " volume={2},\n", " number={4},\n", " pages={31--43},\n", " year={2010},\n", " publisher={IEEE}\n", " }\n", " \n", "\n", "\t\\begin{pmatrix}\n", "\t\t-\\text{cos}_i & -\\text{sin}_i & -\\text{sin}_i\\cdot\\text{pos}_x(\\Delta x_{ij}) + \\text{cos}_i\\cdot\\text{pos}_y(\\Delta x_{ij})\\\\\n", "\t\t \\text{sin}_i & -\\text{cos}_i & -\\text{cos}_i\\cdot\\text{pos}_x(\\Delta x_{ij}) - \\text{sin}_i\\cdot\\text{pos}_y(\\Delta x_{ij})\\\\\n", "\t\t 0 & 0 & -1\n", "\t\\end{pmatrix}\n", "\\end{align}\n", "\n", "\\begin{align}\n", "\tJ_j = R_z(-\\text{rot}(x_{ij})) \\cdot R_z(-\\text{rot}(x_i))\n", "\\end{align}\n", "\n", "\\begin{align}\n", "\tJ =\n", "\t\\begin{pmatrix}\n", "\t\t\\boldsymbol{J_i} & \\boldsymbol{J_j}\n", "\t\\end{pmatrix}\n", "\\end{align}\n", "\n", "The error vector $e$ is computed as follows:\n", "\n", "\\begin{align}\n", "\te_{pos} = R_{-\\text{rot}(x_{ij})} \\cdot (R_{-\\text{rot}(x_i)} \\cdot \\text{pos}(\\Delta x_{ij}) - \\text{pos}(x_{ij}))\n", "\\end{align}\n", "\n", "\\begin{align}\n", "\te_{rot} = \\text{rot}(\\Delta x_{ij}) - \\text{rot}(x_{ij})\n", "\\end{align}\n", "\n", "After normalizing $e_{rot}$ to $[-\\pi, \\pi)$ as $e_{rot}^0$ the full error vector can be constructed with\n", "\n", "\\begin{align}\n", "\te =\n", "\t\\begin{pmatrix}\n", "\t\t\\boldsymbol{e_{\\text{pos}}}\\\\\n", "\t\te_{\\text{rot}}^0\n", "\t\\end{pmatrix}\n", "\t.\n", "\\end{align}\n", "\n", "\n", "\n", "### Observation2D\n", "The `Observation2D` factor involves one `VehicleVariable` $x_i$ and one `LandmarkVariable` $x_j$. The measurement is denoted as $x_{ij}$, \n", "analogously to the previous section. Although $x_j$ and $x_{ij}$ are only positions rather than poses and therefore do not contain a rotation angle, the functions $\\text{pos}(x)$, $\\text{pos}_x(x)$ and $\\text{pos}_y(x)$ will be used nevertheless to make the calculation path more understandable. Given the measurement $x_{ij}$, the Jacobian matrix $J$ is calculated as follows:\n", "\n", "\\begin{align}\n", "\t\\text{pos}(\\Delta x_{ij}) = \\text{pos}(x_j) - \\text{pos}(x_i)\n", "\\end{align}\n", "\n", "\\begin{align}\n", "\t\\text{sin}_i = \\text{sin}(\\text{rot}(x_i))\n", "\\end{align}\n", "\n", "\\begin{align}\n", "\t\\text{cos}_i = \\text{cos}(\\text{rot}(x_i))\n", "\\end{align}\n", "\n", "\\begin{align}\n", "\tJ_i =\n", "\t\\begin{pmatrix}\n", "\t\t-\\text{cos}_i & -\\text{sin}_i & -\\text{sin}_i \\cdot \\text{pos}_x(\\Delta x_{ij}) + \\text{cos}_i \\cdot \\text{pos}_y(\\Delta x_{ij})\\\\\n", "\t\t \\text{sin}_i & -\\text{cos}_i & -\\text{cos}_i \\cdot \\text{pos}_x(\\Delta x_{ij}) - \\text{sin}_i \\cdot \\text{pos}_y(\\Delta x_{ij})\n", "\t\\end{pmatrix}\n", "\\end{align}\n", "\n", "\\begin{align}\n", "\tJ_j = R_{-\\text{rot}(x_i)}\n", "\\end{align}\n", "\n", "\\begin{align}\n", "\tJ =\n", "\t\\begin{pmatrix}\n", "\t\t\\boldsymbol{J_i} & \\boldsymbol{J_j}\n", "\t\\end{pmatrix}\n", "\\end{align}\n", "\n", "The error vector $e$ is computed as follows:\n", "\n", "\\begin{align}\n", "\te = R_{-\\text{rot}(x_i)} \\cdot \\text{pos}(\\Delta x_{ij}) - \\text{pos}(x_{ij})\n", "\\end{align}" ], "metadata": { "collapsed": false } }, { "cell_type": "markdown", "source": [ "## Optimization in 3D\n", "\n", "In 3D, rotations are often represented as quaternions.\n", "To reduce these to a minimal representation, only the imaginary part of the unit quaternions is saved within the solution $\\Delta x$,\n", "\n", "In the following sections, the individual 3D factors' calculations of $J$ and $e$ are presented.\n", "The functions $\\text{pos}(x)$ and $\\text{rot}_Q(x)$ will be used to refer to the 3D position vector and the rotation quaternion of a 3D pose or isometry $x$.\n", "When referring to the rotation as a 3D rotation matrix, the function $\\text{rot}_{RM}(x)$ will be used instead.\n", "When using $x$ in calculations, it will refer to the pose's 3D isometry containing both the position vector as translation as well as the rotation quaternion.\n", "The binary operator $\\circ$ will be used to concatenate these isometries.\n", "The inverse of an isometry will be denoted as $x^{-1}$.\n", "\n", "Some functions will be used to calculate gradients of a 3D isometry $x$, such as $\\frac{\\text{d}q}{\\text{d}R}(x)$, which is computed as follows using the trace function $\\text{tr}(M)$ of 3x3 matrices:\n", "\n", "\\begin{align}\n", "\ts = \\frac{1}{2}\\sqrt{\\text{tr}(\\text{rot}_{RM}(x)) + 1}\n", "\\end{align}\n", "\n", "\\begin{align}\n", "\ta_1 = -\\frac{\\text{rot}_{RM}(x)_{21}-\\text{rot}_{RM}(x)_{12}}{32s^3}\n", "\\end{align}\n", "\n", "\\begin{align}\n", "\ta_2 = -\\frac{\\text{rot}_{RM}(x)_{02}-\\text{rot}_{RM}(x)_{20}}{32s^3}\n", "\\end{align}\n", "\n", "\\begin{align}\n", "\ta_3 = -\\frac{\\text{rot}_{RM}(x)_{10}-\\text{rot}_{RM}(x)_{01}}{32s^3}\n", "\\end{align}\n", "\n", "\\begin{align}\n", "\tb = \\frac{1}{4s}\n", "\\end{align}\n", "\n", "\\begin{align}\n", "\t\\frac{\\text{d}q}{\\text{d}R}(x) =\n", "\t\\begin{pmatrix}\n", "\t\ta_1 & 0 & 0 & 0 & a_1 & b & 0 & -b & a_1\\\\\n", "\t\ta_2 & 0 & -b & 0 & a_2 & 0 & b & 0 & a_2\\\\\n", "\t\ta_3 & b & 0 & -b & a_3 & 0 & 0 & 0 & a_3\n", "\t\\end{pmatrix}\n", "\\end{align}\n", "\n", "It will always be assumed that the condition $\\text{tr}(\\text{rot}_{RM}(x)) > 0$ holds, i.e. we are dealing with proper rotations.\n", "\n", "Another function used in this context is $\\text{skew}(x)$, computed as follows using a 3D vector or position $x$:\n", "\n", "\\begin{align}\n", "\t\\text{skew}(x) =\n", "\t\\begin{pmatrix}\n", "\t\t 0 & x_z & -x_y\\\\\n", "\t\t-x_z & 0 & x_x\\\\\n", "\t\t x_y & -x_x & 0\n", "\t\\end{pmatrix},\n", "\\end{align}\n", "\n", "where $x_x$, $x_y$ and $x_z$ are the first, second and third vector components, respectively. The $\\text{skew}(x)$ can be interpreted as the matrix \n", "representation of the cross product with $x$.\n", "\n", "### Position3D\n", "\n", "The `Position3D` factor involves one `VehicleVariable` $x_v$.\n", "Given the measurement $x_m$, the Jacobian matrix $J$ is calculated as follows:\n", "\n", "\\begin{align}\n", "\tE = x_m^{-1} \\circ x_v\n", "\\end{align}\n", "\n", "\\begin{align}\n", "\t\\frac{\\text{d}te}{\\text{d}qj} = \\frac{\\text{d}q}{\\text{d}R}(E) \\circ\n", "\t\\begin{pmatrix}\n", "\t\t\\text{skew}(\\text{rot}_{RM}(E)_0)\\\\\n", "\t\t\\text{skew}(\\text{rot}_{RM}(E)_1)\\\\\n", "\t\t\\text{skew}(\\text{rot}_{RM}(E)_2)\n", "\t\\end{pmatrix}\n", "\\end{align}\n", "\n", "\\begin{align}\n", "\tJ =\n", "\t\\begin{pmatrix}\n", "\t\t\\boldsymbol{E} & \\boldsymbol{0}\\\\\n", "\t\t\\boldsymbol{0} & \\boldsymbol{\\frac{\\text{d}te}{\\text{d}qj}}\n", "\t\\end{pmatrix},\n", "\\end{align}\n", "\n", "where $M_i$ refers to the column at index $i$ of the matrix $M$.\n", "\n", "The error vector $e$ is computed as follows:\n", "\n", "\\begin{align}\n", "\te = x_m^{-1} \\circ x_v\n", "\\end{align}\n", "\n", "As mentioned before, the scalar part of the quaternion is removed in $e$. $e$ is then interpreted as a six-dimensional\n", "vector with the top three entries for its position and the bottom three entries for its rotation.\n", "\n", "### Odometry3D\n", "The `Odometry3D` factor involves two `VehicleVariable`s $x_i$ and $x_j$.\n", "Given the measurement $x_{ij}$, the Jacobian matrix $J$ is calculated as follows:\n", "\n", "\\begin{align}\n", "\tA = x_{ij}^{-1}\n", "\\end{align}\n", "\n", "\\begin{align}\n", "\tB = x_i^{-1} \\circ x_j\n", "\\end{align}\n", "\n", "\\begin{align}\n", "\tE = A \\circ B\n", "\\end{align}\n", "\n", "\\begin{align}\n", "\t\\frac{dte}{dqi} = \\text{rot}_{RM}(A) \\circ \\text{skew}(\\text{pos}(B))\n", "\\end{align}\n", "\n", "\\begin{align}\n", "\t\\frac{dre}{dqi} = \\frac{dq}{dR}(E) \\circ\n", "\t\\begin{pmatrix}\n", "\t\t\\boldsymbol{\\text{rot}_{RM}(A) \\circ \\text{skew}(\\text{rot}_{RM}(B)_0)^T}\\\\\n", "\t\t\\boldsymbol{\\text{rot}_{RM}(A) \\circ \\text{skew}(\\text{rot}_{RM}(B)_1)^T}\\\\\n", "\t\t\\boldsymbol{\\text{rot}_{RM}(A) \\circ \\text{skew}(\\text{rot}_{RM}(B)_2)^T}\n", "\t\\end{pmatrix}\n", "\\end{align}\n", "\n", "\\begin{align}\n", "\tJ_i =\n", "\t\\begin{pmatrix}\n", "\t\\boldsymbol{-\\text{rot}_{RM}(A)} & \\boldsymbol{\\frac{dte}{dqi}}\\\\\n", "\t\\boldsymbol{0} & \\boldsymbol{\\frac{dre}{dqi}}\n", "\t\\end{pmatrix}\n", "\\end{align}\n", "\n", "\\begin{align}\n", "\t\\frac{dre}{dqj} = \\frac{dq}{dR}(E) *\n", "\t\\begin{pmatrix}\n", "\t\t\\boldsymbol{\\text{skew}(\\text{rot}_{RM}(E)_0)}\\\\\n", "\t\t\\boldsymbol{\\text{skew}(\\text{rot}_{RM}(E)_1)}\\\\\n", "\t\t\\boldsymbol{\\text{skew}(\\text{rot}_{RM}(E)_2)}\n", "\t\\end{pmatrix}\n", "\\end{align}\n", "\n", "\\begin{align}\n", "\tJ_j =\n", "\t\\begin{pmatrix}\n", "\t\t\\boldsymbol{\\text{rot}_{RM}(E)} & \\boldsymbol{0}\\\\\n", "\t\t \\boldsymbol{0} & \\boldsymbol{\\frac{dre}{dqj}}\n", "\t\\end{pmatrix}\n", "\\end{align}\n", "\n", "\\begin{align}\n", "\tJ =\n", "\t\\begin{pmatrix}\n", "\t\t\\boldsymbol{J_i} & \\boldsymbol{J_j}\n", "\t\\end{pmatrix},\n", "\\end{align}\n", "\n", "where $M_i$ refers to the column at index $i$ of the matrix $M$.\n", "\n", "The error vector $e$ is computed as follows:\n", "\n", "\\begin{align}\n", "\te = x_{ij}^{-1} \\circ x_i^{-1} \\circ x_j\n", "\\end{align}\n", "\n", "As mentioned in section ?, the scalar part of the quaternion is removed in $e$. $e$ is then interpreted as a six-dimensional vector with the\n", "top three entries for its position and the bottom three entries for its rotation.\n", "\n", "### Observation3D\n", "The `Observation3D` factor involves one `VehicleVariable` $x_i$ and one `LandmarkVariable` $x_j$.\n", "In the following, the binary operator $\\circ$ denotes the transformation of the position given by the right operand by the isometry given by the left operand.\n", "\n", "With the measurement $x_{ij}$, the Jacobian matrix $J$ is calculated as follows:\n", "\n", "\\begin{align}\n", "\tJ =\n", "\t\\begin{pmatrix}\n", "\t\t\\boldsymbol{-I_3} & \\boldsymbol{\\text{skew}(\\text{pos}(x_i^{-1} \\circ \\text{pos}(x_j)))^T} & \\boldsymbol{\\text{rot}_{RM}(x_i^{-1})}\n", "\t\\end{pmatrix}\n", "\\end{align}\n", "\n", "Finally, the error vector $e$ is computed as follows:\n", "\n", "\\begin{align}\n", "\te = \\text{pos}(x_i^{-1} \\circ \\text{pos}(x_j)) - \\text{pos}(x_{ij})\n", "\\end{align}\n", "\n", "\n" ], "metadata": { "collapsed": false, "pycharm": { "name": "#%% md\n" } } }, { "cell_type": "markdown", "source": [ "## References\n", "[1] _g2o: A general framework for graph optimization_, Kümmerle, Grisetti, Strasdat, Konolige and Burgard, \n", "2011 IEEE International Conference on Robotics and Automation, 3607 - 3613 (2011), IEEE.\n", "\n", "[2] _A tutorial on graph-based SLAM_, Grisetti, Kummerle, Stachniss and Burgard,\n", " IEEE Intelligent Transportation Systems Magazine, 2(4), 31-43 (2010), IEEE\n", "\n", "\n" ], "metadata": { "collapsed": false } } ], "metadata": { "kernelspec": { "display_name": "Python 3", "language": "python", "name": "python3" }, "language_info": { "codemirror_mode": { "name": "ipython", "version": 3 }, "file_extension": ".py", "mimetype": "text/x-python", "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", "version": "3.5.2" }, "pycharm": { "stem_cell": { "cell_type": "raw", "source": [], "metadata": { "collapsed": false } } } }, "nbformat": 4, "nbformat_minor": 0 }