DSO中的深度滤波
在滑动窗口中的作用:
滑动窗口维持一组关键帧,而在这些关键帧中存在深度值未收敛的未成熟点,而深度滤波从新的关键帧中试图找到这些未成熟点的投影,并通过投影位置和相对位姿将未成熟点进一步收敛,当未成熟点深度值收敛到一个较小的范围内后,视作成熟的地图点,此时停止收敛。
深度滤波的步骤:
1.计算参考坐标系中的极线;
2.沿其极限方向确定最佳匹配位置
λ
∗
\lambda^*
λ∗ (也叫视差);
3.根据视差
λ
∗
\lambda^*
λ∗ 计算逆深度值
d
∗
d^*
d∗。
其中:
第一步会受到几何误差的影响,即由于两帧的相对位姿或相机校准估计值的不准确而造成的影响;
第二步会受到光度误差的影响,可能来自
I
0
、
I
1
I_0 、I_1
I0、I1 图像本身?(灰度值?还是相对放射变换参数误差?);
第三步通过基线来衡量这些误差。
1.计算参考坐标系中的极线:
极线方程由以下公式表达:
L
:
=
{
l
0
+
λ
(
l
X
l
y
)
∣
λ
∈
S
}
\begin{equation*} L\ :=\ \left\{l_{0} +\lambda \binom{l_{X}}{l_{y}}\Bigl| \lambda \in S\right\} \end{equation*}
L := {l0+λ(lylX)∣
∣λ∈S}
其中
l
0
l_0
l0 表示无穷远处的深度点,
(
l
X
l
y
)
\binom{l_{X}}{l_{y}}
(lylX) 表示极线的方向。
λ
\lambda
λ 是属于搜索长度S的视差。
在DSO中,首先假设之前滑动窗口内维持了一个逆深度的范围 d m i n d_{min} dmin 和 d m a x d_{max} dmax ,用这两个逆深度值对未成熟点进行投影,投影到新帧上形成一条线段,这条线段就是之后要进行深度收敛的极线。
Vec3f pr = hostToFrame_KRKi * Vec3f(u,v, 1);
Vec3f ptpMin = pr + hostToFrame_Kt*idepth_min;
float uMin = ptpMin[0] / ptpMin[2];
float vMin = ptpMin[1] / ptpMin[2];
if(std::isfinite(idepth_max))
{
ptpMax = pr + hostToFrame_Kt*idepth_max;
uMax = ptpMax[0] / ptpMax[2];
vMax = ptpMax[1] / ptpMax[2];
}
......
2.计算像素匹配的不确定度
所谓像素匹配的不确定度,就是看沿着这条极线上找到的最佳匹配点(找和主导帧中像素点的光度值相同或相近的点)与真实投影位置(不可知,假设有误差,但最佳匹配点肯定与真实投影位置在同一等光度线上)可能存在更大的误差。
因此要计算一下等光度线和极线的夹角(实际上DSO中计算的是极限和图像梯度的夹角)。如果夹角过小(与图像梯度夹角过大),微小的几何误差也会带来较大的影响,因此标记为bad。
// (dIx*dx + dIy*dy)^2
float a = (Vec2f(dx,dy).transpose() * gradH * Vec2f(dx,dy));
// (dIx*dy - dIy*dx)^2
float b = (Vec2f(dy,-dx).transpose() * gradH * Vec2f(dy,-dx)); // (dx, dy)垂直方向的乘积
// 计算的是极线方向和梯度方向的夹角大小,90度则a=0, errorInPixel变大;平行时候b=0
float errorInPixel = 0.2f + 0.2f * (a+b) / a;
//* errorInPixel大说明垂直, 这时误差会很大, 视为bad
if(errorInPixel*setting_trace_minImprovementFactor > dist && std::isfinite(idepth_max))
{
if(debugPrint)
printf("NO SIGNIFICANT IMPROVMENT (%f)!\n", errorInPixel);
lastTraceUV = Vec2f(uMax+uMin, vMax+vMin)*0.5;
lastTracePixelInterval=dist;
return lastTraceStatus = ImmaturePointStatus::IPS_BADCONDITION;
}
if(errorInPixel >10) errorInPixel=10;
3.在极线上找到最小光度误差的位置
上一步筛选了符合要求的极线,这一步就是沿着极线找光度误差最小的点。
在DSO中是从
d
m
i
n
d_{min}
dmin 开始与主导帧的像素点(及其周围八个邻域图像)计算光度误差,并将光度误差的结果保存到一个数组中。
之后,按照固定步长(dso中是单位1)沿着极线向
d
m
a
x
d_{max}
dmax 的方向逐一计算并保存光度误差。
// 初始化值
dx = dx/dist;
dy = dy/dist;
float errors[100];
float bestU=0, bestV=0, bestEnergy=1e10;
int bestIdx=-1;
if(numSteps >= 100) numSteps = 99;
// 计算光度误差
for(int i=0;i<numSteps;i++)
{
float energy=0;
for(int idx=0;idx<patternNum;idx++)
{
计算光度误差residual;
energy += hw *residual*residual*(2-hw);
}
errors[i] = energy;
if(energy < bestEnergy)
{
bestU = ptx; bestV = pty; bestEnergy = energy; bestIdx = i;
}
ptx+=dx;
pty+=dy;
}
4.在最佳位置邻域进行搜索(高斯牛顿优化)
上一步结束后,已经得到了最优点的位置,接下来就在最优点附近进行优化,找残差最小的位置。
采用高斯牛顿法,求解
m
i
n
x
F
(
x
)
min_xF(x)
minxF(x) 其中,
x
x
x 在该问题中表示坐标向量,
F
(
x
)
F(x)
F(x) 输出残差信息。
for(int idx=0;idx<patternNum;idx++)
{
Vec3f hitColor = getInterpolatedElement33(frame->dI,
(float)(bestU+rotatetPattern[idx][0]),
(float)(bestV+rotatetPattern[idx][1]),wG[0]);
if(!std::isfinite((float)hitColor[0])) {energy+=1e5; continue;}
float residual = hitColor[0] - (hostToFrame_affine[0] * color[idx] + hostToFrame_affine[1]);
float dResdDist = dx*hitColor[1] + dy*hitColor[2]; // 极线方向梯度
float hw = fabs(residual) < setting_huberTH ? 1 : setting_huberTH / fabs(residual);
H += hw*dResdDist*dResdDist;
b += hw*residual*dResdDist;
energy += weights[idx]*weights[idx]*hw *residual*residual*(2-hw);
}
5.更新逆深度值以及逆深度的不确定性
通过若干次高斯牛顿优化得到亚像素级投影点的位置,也确定了该点的投影误差,接下来就是计算逆深度以及对逆深度的不确定性进行更新。