# 2d Laser 和 Odomter 内外参数标定工具原理及使用方法

## 标定原理

### 原理概述

$\left( \begin{array}{l}{v} \\ {\omega}\end{array}\right)=\boldsymbol{J} \left( \begin{array}{c}{\omega_{\mathrm{L}}} \\ {\omega_{\mathrm{R}}}\end{array}\right)$

$\boldsymbol{J}=\left( \begin{array}{ll}{J_{11}} & {J_{12}} \\ {J_{21}} & {J_{22}}\end{array}\right)=\left( \begin{array}{cc}{+r_{\mathrm{L}} / 2} & {+r_{\mathrm{R}} / 2} \\ {-r_{\mathrm{L}} / b} & {+r_{\mathrm{R}} / b}\end{array}\right)$

$\boldsymbol{s}^{k}=\ominus\left(\boldsymbol{q}^{k} \oplus \boldsymbol{\ell}\right) \oplus\left(\boldsymbol{q}^{k+1} \oplus \boldsymbol{\ell}\right) ~~~~~~~ (1)$

$\left( \begin{array}{l}{a_{x}} \\ {a_{y}} \\ {a_{\theta}}\end{array}\right) \oplus \left( \begin{array}{l}{b_{x}} \\ {b_{y}} \\ {b_{\theta}}\end{array}\right)=\left( \begin{array}{c}{a_{x}+b_{x} \cos \left(a_{\theta}\right)-b_{y} \sin \left(a_{\theta}\right)} \\ {a_{y}+b_{x} \sin \left(a_{\theta}\right)+b_{y} \cos \left(a_{\theta}\right)} \\ {a_{\theta}+b_{\theta}}\end{array}\right)$
$\ominus​$ 表示求逆：
$\ominus \left( \begin{array}{l}{a_{x}} \\ {a_{y}} \\ {a_{\theta}}\end{array}\right)=\left( \begin{array}{c}{-a_{x} \cos \left(a_{\theta}\right)-a_{y} \sin \left(a_{\theta}\right)} \\ {+a_{x} \sin \left(a_{\theta}\right)-a_{y} \cos \left(a_{\theta}\right)} \\ {-a_{\theta}}\end{array}\right)$

$\boldsymbol{s}^{k}=\ominus \boldsymbol{\ell} \oplus \boldsymbol{r}^{k}\left(r_{\mathrm{L}}, r_{\mathrm{R}}, b\right) \oplus \boldsymbol{\ell}~~~~~~~(2)$

$\mathcal{J}=-\frac{1}{2} \sum_{k=1}^{n}\left\|\hat{\boldsymbol{s}}^{k}-\ominus \ell \oplus \boldsymbol{r}^{k}\left(r_{\mathrm{L}}, r_{\mathrm{R}}, b\right) \oplus \ell\right\|_{\mathbf{\Sigma}_{k}^{-1}}^{2} ~~~~~~~(3)$

### 标定算法的具体实现

1. 先求线性部分：首先由于激光坐标系和机器人坐标系都是水平面运动，所以两个传感器得到的两个时刻之间的角度变化是相等的 $\boldsymbol{s}_{\theta}^{k}=\boldsymbol{r}_{\theta}^{k}$. 因此直接利用公式（2）中的对应项构建等式，用线性最小二乘直接求出轮速计的内参数矩阵的两个元素：
$\left( \begin{array}{c}{\hat{J}_{21}} \\ {\hat{J}_{22}}\end{array}\right)=\left[\sum_{k} \frac{\boldsymbol{L}_{k}^{T} \boldsymbol{L}_{k}}{\left(\sigma_{\theta}^{k}\right)^{2}}\right]^{-1} \sum_{k} \frac{\boldsymbol{L}_{k}^{T}}{\left(\sigma_{\theta}^{k}\right)^{2}} \hat{s}_{\theta}^{k}$
具体公式可以参照论文。

2. 再闭式求解非线性部分：有了部分线性内参数，作者对公式（3）进行了简单的变形，变成了$\mathcal{J}=-\frac{1}{2} \sum_{k}\left\|\boldsymbol{\ell} \oplus \hat{\boldsymbol{s}}^{k}-\boldsymbol{r}^{k} \oplus \boldsymbol{\ell}\right\|_{\mathbf{\Sigma}_{k}^{-1}}^{2}$. 这样这个公式就能转化成带约束的二次型问题，然后利用拉格朗日乘数转化成闭式解。具体的推导请参看论文。

3. outlier remove : 采样的数据中可能有轮子打滑等无效数据在里面，会降低标定精度，因此标定过程反复迭代多次，每迭代一次，就计算一下每个采样数据的 Chi-2 误差。
$\chi^{k}=\left\|\hat{\boldsymbol{s}}^{k}-\ominus \hat{\ell} \oplus \boldsymbol{r}^{k}\left(\hat{r}_{\mathrm{L}}, \hat{r}_{\mathrm{R}}, \hat{b}\right) \oplus \hat{\ell}\right\|_{\mathbf{\Sigma}_{k}^{-1}}$
然后根据误差大小排序，丢弃掉一定百分比的数据。

4. 协方差估计：协方差的估计直接采用信息矩阵的逆来计算。如果给定一个模型 $\boldsymbol{y}_{k}=\boldsymbol{f}_{k}(\boldsymbol{x})+\boldsymbol{\epsilon}_{k}$ 其中 $\boldsymbol{f}$ 是可导函数，$\boldsymbol{\epsilon}_{k}$ 是协方差为 $\mathbf{\Sigma}_{k}$ 的高斯噪声。协方差的下界 Cramer-Rao bound，$\operatorname{cov}(\hat{\boldsymbol{x}}) \geq \mathcal{I}(\boldsymbol{x})^{-1}$. 在这个应用中，$\boldsymbol{x}=\left(r_{\mathrm{R}}, r_{\mathrm{L}}, b, \ell_{x}, \ell_{y}, \ell_{\theta}\right)$$\boldsymbol{y}_{k}=\hat{\boldsymbol{s}}^{k}$ ，观测函数 $\boldsymbol{f}_{k}$ 为公式（2）所示。这个模型最小二乘估计的参数 $\boldsymbol{x}$ 的信息矩阵为

$\mathcal{I}(\boldsymbol{x})=\sum_{k} \frac{\partial \boldsymbol{f}_{k}}{\partial \boldsymbol{x}} \boldsymbol{\Sigma}_{k}^{-\mathbf{1}} \frac{\partial \boldsymbol{f}_{k}}{\partial \boldsymbol{x}}$

## 工具使用

   cd catkin_ws/src
git clone https://github.com/MegviiRobot/OdomLaserCalibraTool
cd ..
catkin_make
source devel/setup.bash
roslaunch example.launch