相机能获得稠密的彩色像素而深度估计不够精确，激光能获得精确的深度但是值较为稀疏，融合激光和相机是一种常见的思路。
 目前主流的方法是将稀疏的激光测距融合到视觉中，然后对深度进行上采样。但这种方法存在两个问题：

1. 相机和激光的外参标定误差较大。
2. 上采样得到的深度由于光滑性假设，在一些区域不够准备。

 本文提出的方法，在概率模型框架下，优化外参和相机位姿，进行离线三维重建。

系统流程介绍

 对图像进行特征提取和匹配，得到地标点坐标和地标点和图像特征的联系，在点云之间也构建联系，综合点云和图像的观测计算外参和相机位姿，然后根据得到的外参和相机位姿更新点云的观测。

相机观测

 在双目图像上提取特征，使用SGM算法从双目恢复深度，得到特征点在相机坐标系下的位姿。考虑到SGM的精度问题，使用稀疏激光进行监督，只有和附近的激光距离真值接近的特征点才被保留，同时，平滑区域的激光也可用于填补SGM的漏洞。
 在现有的图像中，两两进行特征匹配，对相似的特征，生成地标点。

激光观测

 采用BSC（Binary Shape Context）描述子用于初步配准点云，在关联的点云对中，随机选取一部分点作为关键点，然后使用点-面形式建立关联。

优化框架

 假设观测独立，同时误差满足高斯分布，得到以下代价函数。
$P\left(\mathcal{O}_{c}, \mathcal{O}_{l} \mid \mathcal{T}, \mathcal{L}, \mathbf{T}_{e}\right)=P\left(\mathcal{O}_{c} \mid \mathcal{T}, \mathcal{L}\right) P\left(\mathcal{O}_{l} \mid \mathcal{T}, \mathbf{T}_{e}\right)$P(Oc​,Ol​∣T,L,Te​)=P(Oc​∣T,L)P(Ol​∣T,Te​)
$P\left(\mathcal{O}_{c} \mid \mathcal{T}, \mathcal{L}\right)=\prod_{\mathbf{o}_{c} \in \mathcal{O}_{c}} P\left(\mathbf{o}_{c} \mid \mathbf{T}_{i}, \mathbf{l}_{k}\right)$P(Oc​∣T,L)=oc​∈Oc​∏​P(oc​∣Ti​,lk​)
$P\left(\mathcal{O}_{l} \mid \mathcal{T}, \mathbf{T}_{e}\right)=\prod_{\mathbf{o}_{l} \in \mathcal{O}_{l}} P\left(\mathbf{o}_{l} \mid \mathbf{T}_{i}, \mathbf{T}_{j}, \mathbf{T}_{e}\right)$P(Ol​∣T,Te​)=ol​∈Ol​∏​P(ol​∣Ti​,Tj​,Te​)
$P\left(\mathbf{o}_{c} \mid \mathbf{T}_{i}, \mathbf{l}_{k}\right) \propto \exp \left(-\frac{1}{2} w_{\mathbf{o}_{c}}\left(E_{f}^{2}+E_{d}^{2}\right)\right)$P(oc​∣Ti​,lk​)∝exp(−21​woc​​(Ef2​+Ed2​))
$P\left(\mathbf{o}_{l} \mid \mathbf{T}_{i}, \mathbf{T} j, \mathbf{T}_{e}\right) \propto \exp \left(-\frac{1}{2} w_{\mathbf{o}_{l}} E_{l}^{2}\right)$P(ol​∣Ti​,Tj,Te​)∝exp(−21​wol​​El2​)
$\text { feature: } E_{f}=\frac{\left\|\phi\left(\mathbf{l}_{k} \mid \mathbf{K}, \mathbf{T}_{i}\right)-\mathbf{u}\right\|}{\sigma_{p}}$ feature: Ef​=σp​∥ϕ(lk​∣K,Ti​)−u∥​
$\text { depth: } E_{d}=\frac{\left\|\psi\left(\mathbf{l}_{k} \mid \mathbf{T}_{i}\right)\right\|-d}{\sigma_{d}}$ depth: Ed​=σd​∥ψ(lk​∣Ti​)∥−d​
$\text { laser: } E_{l}=\frac{\mathbf{n}^{\mathrm{T}}\left(\psi\left(\mathbf{p} \mid \mathbf{T}_{l, i j}\right)-\mathbf{q}\right)}{\sigma_{l}}$ laser: El​=σl​nT(ψ(p∣Tl,ij​)−q)​
 在迭代更新激光观测前，将误差较高的观测作为误差去除。

感想

 相较于其他的一些激光，视觉建图算法，本文建立的约束其实是很粗糙的，但是离线全局优化之后结果还是不错。