鱼眼相机去畸变python 鱼眼相机slam

Abstract

在urban场景用GPS和鱼眼有multipath effect. 我们提出了一种SLAM-Based IM(Integrity Monitoring)算法来计算位置保护等级. 我们用GPS pseudoranges的连续数据, 像素光度, 车辆动态和satellite ephemeris (卫星星历表)来同时计算车辆位置和landmarks: GPS卫星和图像像素.

我们估计了fault mode vector通过GPS measurement residuals来分析temporal correlation和通过视觉光度残差来计算spatial correlation.

仔细说, 为了检测和孤立视觉错误, 我们研发了基于superpixel的RANSAC算法来做spatial voting.

对于一个estimated fault mode, 我们通过使用线性的Graph-SLAM framework的worst-case failure slope analysis计算protection level.

我们在semi-urban场景, 展现了成功的错误的检测和孤立.

1. Introduction

IM是导航方案估计中很重要的东西.

因为好多种的障碍, GPS系统会接收到更少的观测, 因为urban环境下degraded satellite visibility. 他们也suffer from multipath和由传输过程引起的异常现象.

为了解决上述的挑战, 一种可能的方案是多传感器融合. 视觉传感器在urban场景还不错因为丰富的纹理.

我们考虑了global landmarks-GPS和局部landmarks-像素keypoint.

这里世界系被表示为Earth-Centered Earth-Fixed (ECEF) 坐标系.

2. SLAM-based IM using GPS and Fish-eye Camera

1. 初始化阶段, 我们用PVT和通过GPS receiver algorithm计算的卫星来初始化3D地图. 我们把所有的GPS测量误差状态设为0.5表示neutrality(中性). 在视觉方面, 我们用初始标定来估计尺度.
2. 我们用hybrid sky detection算法来预处理图像, 来区分sky-pixel和non-sky pixel. 检测出来的sky-pixel是用来区分LOS和NLOS的卫星的.
3. 我们考虑non-sky pixels和GPS伪距和carrier-to-noise density (C/N_0), 接收器的运动模型和卫星轨迹模型是我们算法的估计输入. 我们把这些观测结合在extended图优化里来估计总体的状态向量. 状态向量包括: 车辆, GPS卫星, 图像像素.
4. 我们单独分析了观测残差against empirical distribution(经验分布)来检测和区分GPS faults. 我们用super-pixel RANSAC来做spatial voting来检测和孤立视觉错误.
5. 最后, 利用估计的fault mode和总体的状态向量, 我们formulate failure slope for Graph-SLAM.

2.1 观测预处理

• GPS伪距和\(C/N_0\)
• 像素internsity(从鱼眼图拿到)

GPS Module

我们描述GPS观测如下:

$\[\rho^{k}=\left\|\mathbf{y}^{k}-\mathbf{x}\right\|+\left(c \delta t-c \delta t^{k}\right)+\eta^{k} \]$

这里\(x\)表示车辆的3D位置, \(y\)表示第k个卫星.

\(c\delta t\)和 \(c\delta t^k\)表示接收器的始终bias和第k个卫星始终的bias.

\(\mu ^k\)

---

第k个卫星的估计协方差用\(C/N_0\)来表示. 注意LOS和NLOS卫星的分类是用未知的状态向量给的. 那么第k个卫星的测量协方差就是:

$\[\left(\sigma^{k}\left(\boldsymbol{x}_{t}, \boldsymbol{y}_{t}^{k}\right)\right)^{2}=\sqrt{b^{k}+a^{k} \frac{1}{\left(C / N_{0}\right)^{k}}} \text { from }[17] \]$

• \(x_t\)表示车辆的状态向量, 在t时刻的, 包括3D位置, 速度, 时钟bias, 时钟drift和3D attitude.
• \(y_t^i\)表示第i个卫星的状态向量, 包括位置, 速度, 时钟bias, 时钟drift corrections.
• \(b^k\)和\(a^k\)是视觉系数, .....

2.2 Extended graph optimization

我们的扩展图优化有四个误差项:

• GPS伪距
• non-sky intensity
• 接收器运动模型
• 卫星轨迹模型

$\[\begin{aligned} e_{t}\left(\boldsymbol{\theta}_{t}\right)=&\left.\sum_{k=1}^{N} \Lambda\left(\left(\left(\vec{r}_{t}^{k}+1\right) \sigma_{t}^{k}\right)^{-1} \| \rho_{t}^{k}-h\left(\boldsymbol{x}_{t}, y_{t}^{k}\right) |\right)+\sum_{k=1}^{N} \Lambda\left(\hat{\Omega}_{t}^{k}\right)^{-1}\left\|\boldsymbol{y}_{t}^{i}-f\left(u_{t}^{k}, \bar{y}_{t-1}^{k}\right)\right\|\right) \\ &+\Lambda\left(\left(\bar{x}_{t} \boldsymbol{I}+\hat{\Sigma}_{t}\right)^{-1}\left\|x_{t}-g\left(u_{E, t}, \bar{x}_{t-1}\right)\right\|\right)+\sum_{y \in \mathbb{H}} \Lambda\left(\left(\left(\bar{s}_{t-1}(u)+1\right) \omega_{t}(u)\right)^{-1}\left|I_{k f}(\boldsymbol{u})-I_{t}\left(\pi\left(w\left(\Delta \mu_{t}, u\right)\right)\right)\right|\right) \end{aligned} \]$

• \(h\)是GPS观测模型, \(g\)是receiver的运动模型, \(f\)是卫星轨迹模型.
• \(\overline{\boldsymbol{x}}_{t-1}\)是车辆的状态向量的估计, \(\overline{\boldsymbol{y}}_{t-1}^{k}\)
• \(u_{R, t}\)是车辆的运动控制输入, \(u_t^k\)是卫星的运动控制输入

上面式子的前三项是GSP伪距, 卫星星历表, 车辆状态向量. 最后一项是non-sky像素.

2.3 IM for Graph-SLAM framework

1) Multiple FDI module

GPS faults: 为了在伪距中检测和估计GPS faults, 我们把评估每一个残差against 经验高斯分布----这玩意儿标识在没有错误的情况下观测的误差分布.

这个已经是正当的因为我们发现GPS观测在没有错误的情况下就是遵循高斯分布的[11].

Vision faults: 与GPS faults不同, 因为数据关联错误引起的vision faults会展现出high spatial correlation across image pixels and low temporal correlation. 这个也是正当的因为视觉faults are localizaed to a group of neighboring pixels and are not isolated to a standalone pixel. 我们用superpixle-based RANSAC来spatial voting.

我们先把图像分成clusters, 也就是superpixels.

\(\Gamma\): 是non-sky pixels的superpixel的个数.

2) Protection level computation

3. Experiment Results

4. Conclusions