一、归一化积

设p1(x)p_1(x)p1(x)和p2(x)p_2(x)p2(x)是两个不同的概率密度函数，归一化积为：
p(x)=η⋅p1(x)p2(x)p(x)=\eta \cdot p_1(x)p_2(x) p(x)=η⋅p1(x)p2(x)
其中:
η=∫p1(x)p2(x)dx\eta=\int p_1(x)p_2(x)\mathrm{d}x η=∫p1(x)p2(x)dx
η\etaη是为了满足概率公理的归一化常数。

定理：对于p(x∣y1,y2)p(x|y_1,y_2)p(x∣y1,y2)，y1y_1y1和y2y_2y2是两次独立测量，有：
p(x∣y1,y2)=η⋅p(x∣y1)p(x∣y2)p(x|y_1,y_2) = \eta \cdot p(x|y_1)p(x|y_2) p(x∣y1,y2)=η⋅p(x∣y1)p(x∣y2)
推导，利用全概率公式我们可以知道：
p(x∣y1,y2)=p(y1,y2∣x)p(x)p(y1,y2)p(x|y_1,y_2) = \frac {p(y_1,y_2|x)p(x)} {p(y_1,y_2)} p(x∣y1,y2)=p(y1,y2)p(y1,y2∣x)p(x)
这里：
p(y1,y2∣x)=p(y1∣x)p(y2∣x)=p(x∣y1)p(y1)p(x)p(x∣y2)p(y2)p(x)p(y_1,y_2|x)=p(y_1|x)p(y_2|x)=\frac {p(x|y_1)p(y_1)} {p(x)} \frac {p(x|y_2)p(y_2)} {p(x)} p(y1,y2∣x)=p(y1∣x)p(y2∣x)=p(x)p(x∣y1)p(y1)p(x)p(x∣y2)p(y2)
则：
η=p(y1)p(y2)p(y1,y2)p(x)\eta = \frac {p(y_1)p(y_2)} {p(y_1,y_2)p(x)} η=p(y1,y2)p(x)p(y1)p(y2)
若p(x∣yi),i=1,2p(x|y_i),i=1,2p(x∣yi),i=1,2是高斯分布的概率密度函数；
p(x∣y1)=N(μ1,σ12)p(x∣y2)=N(μ2,σ22)p(x|y_1)=N(\mu_1,\sigma_1^2)\\~\\p(x|y_2)=N(\mu_2,\sigma_2^2) p(x∣y1)=N(μ1,σ12) p(x∣y2)=N(μ2,σ22)
对于新的分布p(x∣y1,y2)=N(μ,σ2)p(x|y_1,y_2) =N(\mu,\sigma^2)p(x∣y1,y2)=N(μ,σ2):
1σ2=1σ12+1σ22μσ2=μ1σ12+μ2σ22⟹σ2=σ12σ22σ12+σ22\frac {1}{\sigma^2} = \frac {1}{\sigma_1^2} +\frac {1}{\sigma_2^2} \\~\\ \frac {\mu}{\sigma^2} = \frac {\mu_1}{\sigma_1^2} +\frac {\mu_2}{\sigma_2^2} \\~\\ \Longrightarrow \sigma^2=\frac {\sigma_1^2\sigma_2^2} {\sigma_1^2+\sigma_2^2} σ21=σ121+σ221 σ2μ=σ12μ1+σ22μ2 ⟹σ2=σ12+σ22σ12σ22

我们可以推导出K个归一化：
p(x∣y1,y2,⋯,yk)=ηk⋅p(x∣y1)p(x∣y2)⋯p(x∣yk)p(x|y_1,y_2,\cdots,y_k) = \eta_k\cdot p(x|y_1)p(x|y_2)\cdots p(x|y_k) p(x∣y1,y2,⋯,yk)=ηk⋅p(x∣y1)p(x∣y2)⋯p(x∣yk)
有：
(σ2)−1=∑i=1k(σi2)−1(σ2)−1⋅μ=∑i=1k(σi2)−1⋅μk(\sigma^2)^{-1} = \sum_{i=1}^{k} (\sigma_i^2)^{-1}\\~\\ (\sigma^2)^{-1}\cdot \mu= \sum_{i=1}^{k} (\sigma_i^2)^{-1}\cdot \mu_k (σ2)−1=i=1∑k(σi2)−1 (σ2)−1⋅μ=i=1∑k(σi2)−1⋅μk

二、深度融合

我们假设所有的深度值都是服从高斯分布的，假设有两个状态ξ1\xi_1ξ1、ξ2\xi_2ξ2分别对相机坐标Twc1T_{wc_1}Twc1下的深度值进行观测，那么我们可以表示为：
p(d∣ξ1)=N(μ1,σ12)p(d∣ξ2)=N(μ2,σ22)p(d \space|\space \xi_1)= N(\mu_1 \space,\sigma_1^2)\\~\\ p(d \space|\space \xi_2)= N(\mu_2 \space,\sigma_2^2) p(d ∣ ξ1)=N(μ1 ,σ12) p(d ∣ ξ2)=N(μ2 ,σ22)
我们对这两个观测进行深度融合，即求出在共同观测ξ1\xi_1ξ1、ξ2\xi_2ξ2下的分布：
p(d∣ξ1,ξ2)=N(μ,σ2)p(d \space|\space \xi_1,\space \xi_2)= N(\mu \space,\sigma^2) p(d ∣ ξ1, ξ2)=N(μ ,σ2)
借助上述归一化积的方法，我们可以得到关系：
p(d∣ξ1,ξ2)=η⋅p(d∣ξ1)⋅p(d∣ξ2)p(d \space|\space \xi_1,\space \xi_2) = \eta \cdot p(d \space|\space \xi_1)\cdot p(d \space|\space \xi_2) p(d ∣ ξ1, ξ2)=η⋅p(d ∣ ξ1)⋅p(d ∣ ξ2)
所以深度融合可以看作两个高斯分布的乘积（归一化乘积）。

即：
1σ2=1σ12+1σ22μσ2=μ1σ12+μ2σ22\frac {1}{\sigma^2} = \frac {1}{\sigma_1^2} +\frac {1}{\sigma_2^2} \\~\\ \frac {\mu}{\sigma^2} = \frac {\mu_1}{\sigma_1^2} +\frac {\mu_2}{\sigma_2^2} \\ σ21=σ121+σ221 σ2μ=σ12μ1+σ22μ2
然后求出：
σ2=σ12σ22σ12+σ22μ=σ22μ1+σ12μ2σ12+σ22\sigma^2=\frac {\sigma_1^2\sigma_2^2} {\sigma_1^2+\sigma_2^2}\\~\\ \mu = \frac {\sigma_2^2\mu_1+\sigma_1^2 \mu_2} {\sigma_1^2+\sigma_2^2} σ2=σ12+σ22σ12σ22 μ=σ12+σ22σ22μ1+σ12μ2
这就是深度融合的过程。

三、不确定度的计算

这里不考虑光度误差带来的影响，即光度不变性，只考虑几何误差。

我们利用极线搜索的方式，首先考虑对极约束：
p2TFp1=0p_2^TFp_1 = 0 p2TFp1=0
p1p_1p1、p2p_2p2是两个相机下的像素坐标；
假设在相机2的极线方程为：
au2+bv2+c=0，即p2T⋅[abc]=0au_2+bv_2+c=0，即\quad p_2^T\cdot \begin{bmatrix} a \\ b \\ c\end{bmatrix} = 0 au2+bv2+c=0，即p2T⋅⎣⎡abc⎦⎤=0
根据对极约束，我们就可以得到：
Fp1=[abc]Fp_1 =\begin{bmatrix} a \\ b \\ c\end{bmatrix} Fp1=⎣⎡abc⎦⎤
这样利用极线搜索的方式就可以找到最佳的像素匹配点（不考虑光度误差，认为是完全匹配）。

另外我们需要求出原匹配特征点三角计算的深度值 p′p'p′和最佳像素点（极线搜索方式得到）的深度值ppp，如下图：

这样我们就可以计算第k个状态的深度μk\mu_kμk和方差σk2\sigma_k^2σk2：
μk=p，σk=∣∣p∣∣−∣∣p′∣∣p(dk∣ξk)=N(μk,σk2)\mu_k =p，\quad \sigma_k = ||p||-||p' ||\\ ~\\ p(d_k\space | \space \xi_k ) = N(\mu_k,\space \sigma_k^2) μk=p，σk=∣∣p∣∣−∣∣p′∣∣ p(dk ∣ ξk)=N(μk, σk2)
这样，之后就可以利用深度融合的方法进行更新，直到计算收敛。】

参考文献

机器人学中的概率估计 — Timothy D. Barfoot
视觉SLAM 十四讲从理论到实践 ---- 高翔
Engel J, Sturm J, Cremers D. Semi-dense Visual Odometry for a Monocular Camera. 2013.
Engel J, Schps T, Cremers D. LSD-SLAM: Large-scale direct monocular SLAM[J]. Springer, Cham, 2014.

SLAM--深度滤波器相关推荐

激光SLAM深度剖析
激光SLAM深度剖析系列文章索引本系列文章,是对激光SLAM的深度解读,一作为学习用,二作为分享用. 详细的解释.直观的图表.丰富的注释.简化的测试代码. 作者:公众号:小白学移动机器人之前写过的 ...
全景视觉空间直线检测_视觉SLAM深度解读
近年来,SLAM技术取得了惊人的发展,领先一步的激光SLAM已成熟的应用于各大场景中,视觉SLAM虽在落地应用上不及激光SLAM,但也是目前研究的一大热点,今天我们就来详细聊聊视觉SLAM的那些事儿. ...
lisp提取长方形坐标_语义SLAM | 深度学习用于特征提取 : SuperPoint（一）
0. 写在前面 SuperPoint 是基于自监督训练的特征点检测和描述符提取方法,是2018年MagicLeap发表的一篇文章中提出的.MagicLeap是一个备受争议的做VR的公司,大家如果对他的 ...
视觉SLAM深度解读
点击上方"小白学视觉",选择加"星标"或"置顶" 重磅干货,第一时间送达本文转自 | 新机器视觉近年来,SLAM技术取得了惊人的发展,领 ...
[学习SLAM]深度学习+视觉SLAM 的可行性/方向
时间:2019.07 作者:干磊背景:本文统计的是2018年及以前的相关论,未涉及2019年的论文. 1,深度学习+SLAM的可行性长期来讲,深度学习有极大可能会去替代目前SLAM技术中的某些模块 ...
【踩坑记录】实体机器人运行Cartographer 3D Slam(深度摄像头)--未解决
[运行背景] ROS1.0 20.04 noetic 机器人:NXRobo SPARK-T 安装cartographer请看: [安装学习]安装Cartographer ROS(noetic)_Ho ...
深度学习在slam的应用Deep Learning Applications in SLAM
2022 摘要同时定位与映射(SLAM)是近年来智能机器人领域的研究热点.其处理对象是视觉图像.深度学习在计算机视觉领域取得了巨大成功,这使得深度学习与slam技术的结合成为一个可行的方案.本文总结 ...
基于Kinect深度图像采集和SLAM室内地图创建算法的matlab仿真
目录一.理论基础二.核心程序三.仿真测试结果一.理论基础移动机器人的定位和地图创建是机器人领域的热点研究问题,也是导航中重要环节. 对于已知环境中的机器人自主定位和已知机器人位置的地图创建已 ...
深度学习slam算法工程师
打算以后从事slam+深度学习方向的工作,整理一些招聘网站上的工作要求,作为自己的工作目标. 1.50%配合深度学习算法工程师,集成现有的物体识别框架和贝叶斯方法,构建包含物体标签的地图,用于家用机器 ...
slam 单目稠密深度详解
单目计算深度比较复杂,一般可以用RGB-D相机直接得到深度,但还是练习一下. 因为是稠密重建,对每个像素都算深度,所以就不是提取特征了. 仅凭一幅图像无法估计出深度,要用不同视角下的图像来估计. 特征 ...

SLAM--深度滤波器

目录

一、归一化积

二、深度融合

三、不确定度的计算

参考文献

SLAM--深度滤波器相关推荐

最新文章

热门文章