REMODE解析

　　纯视觉的三维重建（不考虑用结构光的那一类）常用的有两大类方法：一类是SfM，缺点是计算量比较大，做不到实时运行；另一类是KinectFusion为代表的用深度相机做实时重建，缺点是一般只能在室内小范围运行。REMODE（REgularized MOnocular Depth Estimation）[1]是用单目RGB相机实现三维重建的不错的工作，可以像SfM一样实现大范围的三维重建（比如配合无人机），又可以像KinectFusion一样实时运行。第一作者Matia Pizzoli从2012年11月到2014年4月（LinkedIn上的资料）在苏黎世大学Davide Scaramuzza的实验室做博士后，REMODE就是在此期间完成的。第二作者Christian Forster从2012年到2016年跟着Davide Scaramuzza读博士，有两个比较重要的工作，一是SVO（Matia Pizzoli是第二作者），二是IMU预积分。两个人现在都在Oculus做计算机视觉算法工程师。

　　整体来看，REMODE并没有特别的学术创新，更像是融合现有技术挖掘新的应用场景，就像是搭配几个不错的食材炒了一盘新的菜。REMODE主要借鉴了以下四种技术：

1）深度滤波器[2]。这是最核心的技术，可以从概率分布的角度融合多帧观测估计出像素的深度信息。论文[2]本身已经可以做稠密三维重建，作者的个人网站上有四个demo视频（http://george-vogiatzis.org/real_time_mvs/）。REMODE在实验评估部分也是和论文[2]做比较。
2）论文[4]和DTAM[5]的正则化损失函数（regularized cost），目的是平滑表面。由于深度滤波器是单独计算每个像素的深度，没有考虑像素与像素之间的约束关系，所以重建出的表面会有很多毛刺、噪声。对正则化损失函数优化求解后，可以得到更好的重建效果。
3）SVO[3]。论文[2]中，作者在物体周围贴了很多marker来辅助估计相机的位置姿态。而REMODE直接用了SVO，不需要贴marker。
4）凸优化[6]，加速求解正则化损失函数。

　　本文主要解读深度滤波器和正则化损失函数，这两部分内容是REMODE的核心，解决了估计深度的问题。SVO可以参考之前的文章。凸优化那篇论文很有影响力，2011年到现在已经有2000多的引用量，但是是纯数学推导，就没细看。按REMODE中的描述，该方法根据原损失函数构造了一个新的函数（primal dual formulation），优化时交替地对prima variable和dual variable分别执行梯度下降和梯度上升，可以达到加速优化求解的作用。

1. 深度滤波器

　　深度滤波器的本质是贝叶斯估计：已知先验概率分布，根据新的测量值求出后验概率分布。所以可以很方便地一步一步迭代求解，计算效率高。深度的观测值是沿极线找到匹配然后三角化得出的，这一过程很容易产生误匹配导致“坏测量”。所以深度滤波器把观测值的分布看成正态分布和均匀分布的叠加，“好测量”（内点）服从围绕真值的正态分布，“坏测量”（外点）服从区间 $[d_{min},d_{max}]$ 内的均匀分布，具体公式如下：

$p(d_k|\hat{d},\rho)={\rho}{N}(d_k|\hat{d},\tau_k^2)+(1-\rho){U}(d_k|d_{min}, d_{max})$

其中， $\rho$ 和 $\tau_k^2$ 分别是内点的概率和方差， $\hat{d}$ 是真值。假设每一次观测都是独立的，按时间戳顺序第r次观测初始化了一个深度滤波器的“种子”（seed，每个种子对应一个地图点），第k次观测后求出的后验概率为

$p(\hat{d},\rho|d_{r+1},...,d_k) {\propto} p(\hat{d},\rho)\prod\limits_{k}p(d_k|\hat{d},\rho)$

其中， $p(\hat{d},\rho)$ 是真值和内点概率的先验信息。上面这个公式是很难直接处理的，论文[2]提出可以用高斯分布和Beta分布的乘积来近似，高斯分布描述深度，而Beta分布则描述内点的比例：

$q(\hat{d},\rho|a_k,b_k,\mu_k,\sigma_k^2)=Beta(\rho|a_k,b_k){N}(\hat{d}|\mu_k,\sigma_k^2)$

其中， $\mu_k$ 和 $\sigma_k^2$ 是深度估计的均值和方差， $a_k$ 和 $b_k$ 控制Beta分布，可以看成是这个“种子”从初始化到k时刻所有观测中内点和外点的数量。如果第k-1次后验估计用上面高斯 $\times$ Beta描述，则第k次观测后的更新方程为：

$p(\hat{d},\rho|d_{r+1},...,d_k) {\approx} q(\hat{d},\rho|a_{k-1},b_{k-1},\mu_{k-1},\sigma_{k-1}^2)p(d_k|\hat{d},\rho)const$

此时，后验估计 $p(\hat{d},\rho|d_{r+1},...,d_k)$ 不再符合高斯 $\times$ Beta的假设，需要再次用高斯 $\times$ Beta来近似，具体方法是让二者对 $\hat{d}$ 和 $\rho$ 的一阶矩和二阶矩相等。这样，每一步的后验估计都可以用高斯 $\times$ Beta描述，经过一系列推导后就可以得到相应的更新公式，具体的推导可以查阅论文[2]的补充材料。不过需要注意的是，补充材料中公式（19）写错了，应该是 ${1}/{s^2}={1}/{\sigma^2}+{1}/{\tau^2}$ 。下面贴出REMODE中深度滤波器更新的代码（也可以参考SVO中DepthFilter::updateSeed的代码），比较不理解的是求出c1和c2后的normalization的操作，这是论文[2]的推导中没有的。

 1     // REMODE中深度滤波器更新的代码。
 2     const float tau_sq = tau * tau;
 3     const float s_sq = (tau_sq * sigma_sq) / (tau_sq + sigma_sq);
 4     const float m    = s_sq * (mu / sigma_sq + depth / tau_sq);
 5     float c1   = (a / (a+b)) * normpdf(depth, mu, sigma_sq+tau_sq);
 6     float c2   = (b / (a+b)) * (1.0f / dev_ptr->scene.depth_range);
 7     const float norm_const = c1 + c2;
 8     c1 = c1 / norm_const;
 9     c2 = c2 / norm_const;
10     const float f = c1 * ((a + 1.0f) / (a + b + 1.0f)) + c2 *(a / (a + b + 1.0f));
11     const float e = c1 * (( (a + 1.0f)*(a + 2.0f)) / ((a + b + 1.0f) * (a + b + 2.0f))) +
12         c2 *(a*(a + 1.0f) / ((a + b + 1.0f) * (a + b + 2.0f)));
13 
14     if(isnan(c1*m))
15     {
16       return;
17     }
18 
19     const float mu_prime = c1 * m + c2 * mu;
20     dev_ptr->sigma->atXY(x, y) = c1 *(s_sq + m*m) + c2 * (sigma_sq + mu*mu) - mu_prime*mu_prime;
21     dev_ptr->mu->atXY(x, y) = mu_prime;
22     const float a_prime = ( e - f ) / ( f - e/f );
23     dev_ptr->a->atXY(x, y) = a_prime;
24     dev_ptr->b->atXY(x, y) = a_prime * ( 1.0f-f ) / f;

　　下图图总结了深度滤波器的概况， $E_{\rho}[q]$ 是内点概率 $\rho$ 的期望值。按照论文[2]的解释， $E_{\rho}[q]={a_k}/{(a_k+b_k)}$ 。但REMODE实际的代码中在和 $\eta_{outlier}$ 比较时用的是 $(a_k-1)/(a_k+b_k-2)$ ，没太想明白为什么。关于参数的选择，论文[2]中 $\eta_{inlier}=0.1$ ， $\eta_{outlier}=0.05$ ， $\sigma_{thr}=(Z_{max}-Z_{min})/10000$ ；REMODE中 $\eta_{inlier}=0.7$ ， $\eta_{outlier}=0.05$ ， $\sigma_{thr}=(Z_{max}-Z_{min})/1000$ 。

 1 // REMODE中深度滤波器收敛与否的判断
 2 // if E(inlier_ratio) > eta_inlier && sigma_sq < epsilon
 3   if( ((a / (a + b)) > dev_ptr->eta_inlier)
 4       && (sigma_sq < dev_ptr->epsilon) )
 5   { // The seed converged
 6     dev_ptr->convergence->atXY(x, y) = ConvergenceStates::CONVERGED;
 7   }
 8   else if((a-1) / (a + b - 2) < dev_ptr->eta_outlier)
 9   { // The seed failed to converge
10     dev_ptr->convergence->atXY(x, y) = ConvergenceStates::DIVERGED;
11   }
12   else
13   {
14     dev_ptr->convergence->atXY(x, y) = ConvergenceStates::UPDATE;
15   }

　　另一个需要考虑的内容是观测值方差 $\tau_k$ 的估计。REMODE假设图像上的方差是1个像素，反向投影出去得到深度的方差。具体的求解可以参考论文[1]，大致的思路是：由于 $C_r$ 、 $C_k$ 、 $_rp$ 三点坐标已知，可以很方便地求出三条边长，然后用余弦定理求出 $\alpha$ 和 $\beta$ ；再估计出 $\beta^+$ 后，可以根据正弦定理求出 $_rp^+$ ，从而根据 $\tau_k^2=(||_rp^+||-||_rp||)^2$ 求出 $\tau_k$ 。在图像不同位置， $\beta^+$ 的大小是不同的，REMODE中用 $\beta+2tan^{-1}(\frac{1}{2f})$ 来近似。

2. 平滑表面

　　如前文所说，深度滤波器中每个像素是单独计算的，没有考虑像素之间的约束，所以计算出的深度图比较粗糙。平滑表面的作用就是引入像素之间的约束，一方面去掉毛刺，另一方面又要保留边界。REMODE参照论文[4]构造了如下的正则化损失函数：

$\min_F\int_{\Omega}\{G(u)||{\bigtriangledown}F(u)||_{\epsilon}+\lambda||F(u)-D(u)||_1\}du,$

其中， $D$ 是深度滤波器得到的深度图， $F$ 是优化的目标。损失函数的第一项是正则化项，让表面尽量平滑，第二项是数据项，保留原始的数据信息， $\lambda$ 控制这两项的比例大小。正则化项的权重 $G$ 等于：

$G(u)=E_\rho[q](u)\frac{\sigma^2(u)}{\sigma_{max}^2}+\{1-E_\rho[q](u)\}=1-E_\rho[q](u)\frac{\sigma_{max}^2-\sigma^2(u)}{\sigma_{max}^2},$

方差 $\sigma^2(u)$ 越小，内点概率 $E_\rho[q](u)$ 越大，深度滤波器的原始数据越可信，正则化项的权重 $G(u)$ 越小，越信任数据项。 $||{\bigtriangledown}F(u)||_{\epsilon}$ 沿用了DTAM的构造方式：

$||{\bigtriangledown}F(u)||_{\epsilon}=\left\{ \begin{aligned} &\frac{||{\bigtriangledown}F(u)||_2^2}{2\epsilon} && 如果||{\bigtriangledown}F(u)||_2\le\epsilon,\\ &||{\bigtriangledown}F(u)||_1-\frac{\epsilon}{2} && 其他. \end{aligned} \right.$

这里 $||{\bigtriangledown}F(u)||_{\epsilon}$ 是Huber范数，由L1范数（元素绝对值的和）和L2范数（元素平方和的平方根）组成。对于 $||{\bigtriangledown}F(u)||_2$ 比较小的情况，一般是遇到了光滑的表面，L2范数可以惩罚毛刺；而对于梯度比较大的情况，一般是遇到了不连续的边界，如果仍然用L2范数，正则化项会很大，导致强行平滑了不连续的边界，所以这种情况会换成L1范数。

参考文献：

[1] Pizzoli M, Forster C, Scaramuzza D. REMODE: Probabilistic, monocular dense reconstruction in real time[C]//Robotics and Automation (ICRA), 2014 IEEE International Conference on. IEEE, 2014: 2609-2616.

[2] Vogiatzis G, Hernández C. Video-based, real-time multi-view stereo[J]. Image and Vision Computing, 2011, 29(7): 434-441.

[3] Forster C, Pizzoli M, Scaramuzza D. SVO: Fast semi-direct monocular visual odometry[C]//Robotics and Automation (ICRA), 2014 IEEE International Conference on. IEEE, 2014: 15-22.

[4] Bresson X, Esedoḡlu S, Vandergheynst P, et al. Fast global minimization of the active contour/snake model[J]. Journal of Mathematical Imaging and vision, 2007, 28(2): 151-167.

[5] Newcombe R A, Lovegrove S J, Davison A J. DTAM: Dense tracking and mapping in real-time[C]//Computer Vision (ICCV), 2011 IEEE International Conference on. IEEE, 2011: 2320-2327.

[6] Chambolle A, Pock T. A first-order primal-dual algorithm for convex problems with applications to imaging[J]. Journal of mathematical imaging and vision, 2011, 40(1): 120-145.