Dense SLAM 采用 dense 配准的形式计算 pose，dense 配准优化，损失函数项一般有两个，一个是如 ICP 算法，cost 项是匹配点的点到平面距离，另一个是匹配点的像素值误差。这里给出 ICP 算法介绍和推导。
关于 ICP 算法
ICP 算法主要用于点云配准，在两帧点云位姿差别较大时，通常先用 3D 特征匹配计算粗略位姿，然后再用 ICP 算法对位姿做 refine。在实时的三维重建中，由于相邻两帧之间位姿变化比较小，在变量求解时，直接用上一帧的位姿初始化。
在 Dense SLAM 中，ICP 算法通常通过极小化匹配点的点到平面的距离计算 pose，ICP 算法有好多变种。
算法步骤
1、对输入的两幅深度图像，做 3 层抽样，对于抽样后的深度图像做滤波，点云配准以 coarse-to-fine 的方式进行。
2、在给定深度图像内参情况下，通过原始深度图像可以计算点的三维坐标（用于点云的配准和融合），对于滤波后的深度图像也计算三维点云坐标（用于计算法向量）。
3，根据上一次迭代求得的 pose 变换点坐标。
4、对于两幅点云根据投影算法计算匹配点。
5、对于匹配点极小化点到平面距离计算位姿。
6、目标函数误差小于一定值时，或者到达设置的迭代次数停止迭代，否则进入第 3 步。
投影算法计算匹配点
由于两帧图像间位姿变换比较小，采用投影算法计算匹配点，投影算法计算匹配点比基于 kd-tree 的匹配算法速度快，对于一幅深度图像三维点坐标 pi ，可以计算在另一幅图像中的投影像素坐标：
k(ud, vd,1)=KTjipi
pi ：深度图像 i 中点的三维坐标，
Tji：从图像 i 到图像 j 到 pose，
K ：相机内参数，
(ud, vd)： pi 在图像 j 中的投影像素坐标。
对于图像 i 中的点 pi ，在图像 j 像素位置 (ud, vd) 周围找三维坐标值和法向量相近的点作为匹配点，示意图如下：
极小化点到平面距离计算 pose
点到平面距离示意图如下：
目标函数如下：
E=∑i=1k((Rpi+t−qi)⋅ni)2
上式表达式中 R, t 是待求解的 pose，在每次迭代时实际是值很小的 ΔR 和 Δt ，
pi 和 qi 分别是原始点和目标点， ni 指目标点的法向量。
上式关于 R 只有三个自由度，用 r=(rx, ry, rz) 表示，平移向量 t 表示为 t=(tx, ty, tz)。
旋转矩阵 R 是非线性的，从而目标函数也是非线性，这里将 R 线性化。
当相邻两帧之间位姿变化较小时，可以有以下近似： sin(θ)=θ, cos(θ)=1
在三个方向上 R 的旋转角为 r=(rx, ry, rz)，对于 Rpi+t≈pi+r×pi+t ，并且 r×pi⋅ni=r⋅(pi×ni) ，由以上近似，目标函数可以写为：
E≈∑i=1k((pi−qi)⋅ni+r⋅(pi×ni)+t⋅ni)2
以上目标函数对 6 个维度位姿参数求导并且令导数为 0，可以得到 Ax+b=0 ， A 的表达式为：
A=(p1×n1n1……pk×nknk)⎛⎝⎜⎜(p1×n1)⊤…(pk×nk)⊤n⊤1…n⊤k⎞⎠⎟⎟.
写成单个点累加形式：
A=∑i=1k(pi×nini)((pi×ni)⊤n⊤i).
b 的表达式为：
b=(p1×n1n1……pk×nknk)⎛⎝⎜⎜(p1−q1)⊤⋅n1…(pk−qk)⊤⋅nk⎞⎠⎟⎟.
写成单个点累加形式：
b=∑i=1k((pi×ni)τ, ni)τ⋅((pi−qi)τ⋅ni)
未知参量 x 表达式为 x=(rx, ry, rz, tx, ty tz)。
上式对于单点求和的形式，可以用 GPU 做并行计算，累加的结果传到 CPU ，在 CPU中求解 Ax+b=0 。
版权声明：本文为博主原创文章，未经博主允许不得转载。