DSO零空间与尺度漂移

DSO作为一种单目视觉里程计，观其代码用的也不是Sim3优化，那么它是如何保持零空间的稳定性的呢？这块的代码其实看得不是很懂，大致记录一下自己的理解。

零空间

所谓零空间，在数学上是指方程
$$\mathbf{H}\mathbf{x}=0\text{\hspace{1em}(1)}$$
的解所形成的空间 { x ∣ H x = 0 } \{\mathbf x|\mathbf H \mathbf x=\mathbf 0\} {x∣Hx=0}。很显然，当矩阵$\mathbf{H}$的行列式等于0时，方程有无数解，这些解就构成了所谓的零空间。
在DSO中求解的增量方程，由于绝对尺度这个信息的缺失，对于同一个增量方程，显然有不同的状态量${\mathbf{x}}_1,{\mathbf{x}}_2$满足
$$\mathbf{H}{\mathbf{x}}_1=\mathbf{b}\text{\hspace{1em}(2)}$$
$$\mathbf{H}{\mathbf{x}}_2=\mathbf{b}\text{\hspace{1em}(3)}$$
两式一减，就得到了公式（1）的形式：
$$\mathbf{H}({\mathbf{x}}_2-{\mathbf{x}}_1)=0\text{\hspace{1em}(4)}$$
设$\Delta \mathbf{x}={\mathbf{x}}_2-{\mathbf{x}}_1$，这里的$\Delta \mathbf{x}$就是状态变量在零空间中的游荡量，表现在地图中可能是地图的尺度突然缩小，相机移动量也对应缩小，同时不会对能量函数造成影响，如下图1所示：

图１　相机一个急转弯后尺度突然坍塌

为什么尺度会漂移

初始化的时候不是归一化过了吗？这不就相当于是人为设了一个尺度了吗，为什么还会有尺度问题呢？这是因为DSO采用的滑动窗口法进行优化，当前面的关键帧离开滑动窗口后，最多只能通过少量的边缘化的点提供一定的先验信息。在某些情况下比如转弯过快等，关键帧生成速度很快，前面的帧来不及留下足够多的先验信息时（因为前面帧的点大多投影不到现在的帧上，构不成残差项），如图１的情况，这时，就容易出现求解出来的增量在零空间上游荡。

怎么处理

DSO采用将增量正交化的方式来缓解这种情况，相应代码在函数EnergyFunctional::orthogonalize中。如图２所示，三角形代表相机位姿，黑色实现是相机轨迹，矩形框代表滑动窗口，圆点表示点，虚线表示途中第三帧相机位姿的零空间，红色箭头表示高斯牛顿法求解增量方程得到的解x，蓝色箭头表示最后求解出的x和零空间正交的分量。

图２ 增量正交化来缓解零空间漂移

来看代码

在优化函数EnergyFunctional::solveSystemF中，首先求解得到了状态增量x。

VecX x;
x = SVecI.asDiagonal() * HFinalScaled.ldlt().solve(SVecI.asDiagonal() * bFinal_top);

然后进入EnergyFunctional::orthogonalize。

orthogonalize(&x, 0);

然后，构建零空间矩阵N。

// decide to which nullspaces to orthogonalize.
std::vector<VecX> ns;
ns.insert(ns.end(), lastNullspaces_pose.begin(), lastNullspaces_pose.end());
ns.insert(ns.end(), lastNullspaces_scale.begin(), lastNullspaces_scale.end());

// make Nullspaces matrix
MatXX N(ns[0].rows(), ns.size());
for(unsigned int i=0;i<ns.size();i++)
	N.col(i) = ns[i].normalized();

尺度的零空间很好理解，但不是很明白为什么要加入位姿的零空间，或者说这里的lastNullspaces_pose是指什么？这里看一下lastNullspaces_pose和lastNullspaces_scale是怎么定义的，这部分定义在FrameHessian::setStateZero中。

// scale change
SE3 w2c_leftEps_P_x0 = (get_worldToCam_evalPT());
w2c_leftEps_P_x0.translation() *= 1.00001;
w2c_leftEps_P_x0 = w2c_leftEps_P_x0 * get_worldToCam_evalPT().inverse();
SE3 w2c_leftEps_M_x0 = (get_worldToCam_evalPT());
w2c_leftEps_M_x0.translation() /= 1.00001;
w2c_leftEps_M_x0 = w2c_leftEps_M_x0 * get_worldToCam_evalPT().inverse();
nullspaces_scale = (w2c_leftEps_P_x0.log() - w2c_leftEps_M_x0.log())/(2e-3);

尺度变化很好理解，平移分量变化一个小的幅度，对应的位姿的李代数变化，体现在图２中虚线就可以理解为位移方向上的缩放。

for(int i=0;i<6;i++)
{
	Vec6 eps; eps.setZero(); eps[i] = 1e-3;
	SE3 EepsP = Sophus::SE3::exp(eps);
	SE3 EepsM = Sophus::SE3::exp(-eps);
	SE3 w2c_leftEps_P_x0 = (get_worldToCam_evalPT() * EepsP) * get_worldToCam_evalPT().inverse();
	SE3 w2c_leftEps_M_x0 = (get_worldToCam_evalPT() * EepsM) * get_worldToCam_evalPT().inverse();
	nullspaces_pose.col(i) = (w2c_leftEps_P_x0.log() - w2c_leftEps_M_x0.log())/(2e-3);
}

位姿这里，可以看到，李代数每个维度变化一个小量（世界坐标系下），对应的相机坐标系下的位姿的李代数变化。这个为什么会是零空间呢？！

那么怎么抑制零空间的漂移呢？考虑已经得到了的增量$\mathbf{x}$，这里包含了相机内参增量和所有滑动窗口中关键帧的位姿增量和光度增量，这里只有位姿增量涉及零空间的抑制。考虑方程
$$\mathbf{N}\Delta \mathbf{k}=\mathbf{x}\text{\hspace{1em}(5)}$$
这里的$N$是前面提到的零空间矩阵，通常情况下零空间的解（比如图２中的一条线）不会和增量方程求解出来的$\mathbf{x}$撞车，也就是说，公式（５）是无解的，但我们可以找到一个唯一的解${\mathbf{k}}_0$，使得$\Vert \mathbf{N}\Delta \mathbf{k}-\mathbf{x}\Vert$最小，反应在图２中就是蓝色箭头和虚线的垂足。
这其实就是一个最小二乘问题，我们可以直接写出答案
$${\mathbf{k}}_0=({\mathbf{N}}^T\mathbf{N}{)}^{-1}{\mathbf{N}}^T\mathbf{x}\text{\hspace{1em}(6)}$$
这里的$({\mathbf{N}}^T\mathbf{N}{)}^{-1}{\mathbf{N}}^T$其实就是$\mathbf{N}$的伪逆${\mathbf{N}}^{†}$，求出伪逆后，令
$$\mathbf{x}\leftarrow \mathbf{x}-\mathbf{N}{\mathbf{N}}^{†}\mathbf{x}\text{\hspace{1em}(7)}$$
就得到了图２中的蓝色箭头。在程序中输出一下，可以发现此时的$\mathbf{x}$和$\mathbf{N}{\mathbf{k}}_0$的内积确实是０（不为０的时候基本就是gg的时候了）。

// compute Npi := N * (N' * N)^-1 = pseudo inverse of N.
Eigen::JacobiSVD<MatXX> svdNN(N, Eigen::ComputeThinU | Eigen::ComputeThinV);

VecX SNN = svdNN.singularValues();
double minSv = 1e10, maxSv = 0;
for(int i=0;i<SNN.size();i++)
{
	if(SNN[i] < minSv) minSv = SNN[i];
	if(SNN[i] > maxSv) maxSv = SNN[i];
}
for(int i=0;i<SNN.size();i++)
	{ if(SNN[i] > setting_solverModeDelta*maxSv) SNN[i] = 1.0 / SNN[i]; else SNN[i] = 0; }

MatXX Npi = svdNN.matrixU() * SNN.asDiagonal() * svdNN.matrixV().transpose(); 	// [dim] x 9.
MatXX NNpiT = N*Npi.transpose(); 	// [dim] x [dim].
MatXX NNpiTS = 0.5*(NNpiT + NNpiT.transpose());	// = N * (N' * N)^-1 * N'.

这部分主要就是通过SVD分解求出伪逆，然后

if(b!=0) *b -= NNpiTS * *b;

减去零空间的部分，得到正交化后的结果。
基本的逻辑就是，我既然在上一次优化时得到一个局部最优解，那么它在它所在的零空间上应该就是最优的，在这个方向上就不要动了，当我在这一次的优化时，发现得到的增量含有零空间的成分，我充分怀疑这部分是漂移过来的，而不是我想要的，我就把它正交掉，只保留我觉得有效的部分。

参考文献
[1] Engel J, Koltun V, Cremers D. Direct sparse odometry[J]. IEEE transactions on pattern analysis and machine intelligence, 2018, 40(3): 611-625.
[2] https://www.jianshu.com/p/609fa0cce409
[3] https://mp.weixin.qq.com/s/WG8GRionGbzHVubNMTpMTg