Color Map Optimization for 3D Reconstruction with Consumer Depth Cameras核心算法翻译及理解，附python open3d实现

方案概述

输入

RGB-D

网格重建

KinectFusion

关键帧选取

由于相机是手持的，会导致运动模糊。
为了最大化纹理质量，需要从输入图像序列中选取一个子集。
模糊评估：使用Crete【2007】提出的方法量化模糊程度。
然后，在选定的子集中进一步选取。
选定一帧后，在该帧后的$(t_{-},t_{+})$时间间隔内选取模糊程度最低的一帧（实际中使用$(t_{-},t_{+})=(1,5)s$）。关键帧表示为 { I i } \{I_i\} {Ii​}
对于每个关键帧$I_i$，将mesh网格渲染到$I_i$的图像平面上，使用距离$I_i$最近的深度图算出的相机pose。通过比较每个顶点的深度值和相应的深度进行可见性测试，由此可以确定关键帧$I_i$对应的顶点子集，这个顶点子集进一步通过投影点到图像边缘的距离（实际中阈值为9个像素）和深度不连续性进行滤除。保留的顶点表示为 { P i } \{\mathbf P_i\} {Pi​}

相机姿态优化

相机姿态优化本质上是一个非线性最小二乘问题，下面对其目标函数和优化方法进行说明。

目标函数

输入：关键帧选取中确定的纹理图片集合 { I i } \{I_i\} {Ii​}以及相关联的顶点子集 { P i } \{\mathbf P_i\} {Pi​}
目标：优化相机姿态矩阵${\mathbf{T}}_i$，通过${\mathbf{T}}_i$可以将顶点映射到对应的纹理图片上。$\forall \mathbf{p}\in \mathbf{P}$，最大化$\mathbf{p}$在所有相关联图像 I p = { I i : p ∈ P } I_p=\{I_i:\mathbf p\in \mathbf P\} Ip​={Ii​:p∈P}中的颜色一致性

采用齐次坐标（homogeneous coordinates）进行计算，因此${\mathbf{P}}_i\subset {\mathbb{P}}^3$，${\mathbf{T}}_i$为$4\times 4$的矩阵。

齐次坐标（homogeneous coordinates）：在投影空间中，两条平行线会相交于无限远处的一点，在笛卡尔坐标$(X,Y)$的基础上增加一个新的分量$w$，变成$(x,y,w)$，$X=x/w,Y=y/w$

则目标函数为：
$$E(\mathbf{C},\mathbf{T})=\sum _i\sum _{\mathbf{p}\in \mathbf{P}}(\mathbf{C}(\mathbf{p})-{\Gamma }_i(\mathbf{p},{\mathbf{T}}_i){)}^2$$
其中，${\Gamma }_i(\mathbf{p},{\mathbf{T}}_i)$是点$\mathbf{p}$在图像$I_i$对应投影点的颜色值，$\mathbf{C}(\mathbf{p})$是集合 { Γ i ( p , T i ) } I i ∈ I p \{\Gamma_i(\mathbf p,\mathbf T_i)\}_{I_i\in I_\mathbf p} {Γi​(p,Ti​)}Ii​∈Ip​​(所有和$\mathbf{p}$相关联的纹理图像)的平均值。
为了进一步优化，下面阐述一下${\Gamma }_i(\mathbf{p},{\mathbf{T}}_i)$的计算过程：该过程由三步组成，一次刚体变换$\mathbf{g}$，一次投影变换$\mathbf{u}$，以及一次颜色变换${\Gamma }_i$，即${\Gamma }_i(\mathbf{u}(\mathbf{g}(\mathbf{p},{\mathbf{T}}_i)))$：
$$\begin{gathered} \mathbf{g}(\mathbf{p},{\mathbf{T}}_i)={\mathbf{T}}_i\mathbf{p} \\ \mathbf{u}(g_x,g_y,g_z,g_w)=(\frac{g_x{f}_x}{g_z}+c_x,\frac{g_y{f}_y}{g_z}+c_y{)}^{\mathbf{T}} \end{gathered}$$
其中，$f_x,f_y$表示焦距，$(c_x,c_y{)}^{\mathbf{T}}$表示焦点。则${\Gamma }_i(u_x,u_y)$就图像$I_i$是相应坐标点处的灰度值（双线性插值）。
最后，目标函数可以写作：
$$E(\mathbf{C},\mathbf{T})=\sum _i\sum _{\mathbf{p}\in \mathbf{P}}{r}_{i,p}^2$$
残差项$r_{i,p}$为：
$$r_{i,p}=\mathbf{C}(\mathbf{p})-{\Gamma }_i(\mathbf{u}(\mathbf{g}(\mathbf{p},{\mathbf{T}}_i))$$

解释：我们有一个重建好的网格和一组关键帧数据，以及每一张关键帧相应的相机姿态。由于重建过程中估计的相机姿态可能存在一定的误差，直接进行纹理映射会产生重影模糊等问题，因此要根据这些数据对相机姿态进行优化。优化的大体思路是：首先根据网格顶点3D坐标和关键帧的相机姿态，将3D顶点其投影到2D平面，得到若干张投影图，根据每个顶点投影后的uv坐标获取到关键帧相应位置的像素值，每个顶点在每个关键帧（这里就用到了之前的可见性，只需要“可见”的关键帧）中都可以得到一个对应的像素值${\Gamma }_i(\mathbf{p},{\mathbf{T}}_i)$，将这些像素值求平均就是$\mathbf{C}(\mathbf{p})$，优化的目标就是让$\mathbf{C}(\mathbf{p})$和每一个${\Gamma }_i(\mathbf{p},{\mathbf{T}}_i)$尽可能的接近。

高斯牛顿法

非线性最小二乘问题可以通过高斯牛顿法进行优化求解。我们将上面的目标函数中需要优化的参数（即$\mathbf{C},\mathbf{T}$包含的所有参数）表示为${\mathbf{x}}^0=[{\mathbf{C}}^0,{\mathbf{T}}^0]$。
对于每个$i$，${\mathbf{T}}_i^0$是由距离$I_i$最近的深度图提供的相机姿态，
对于每个$\mathbf{p}$，$C^0(\mathbf{p})$是 { Γ i ( p , T i 0 ) } I i ∈ I p \lbrace\Gamma_i(\mathbf p,\mathbf T_i^0)\rbrace_{I_i \in I_\mathbf p} {Γi​(p,Ti0​)}Ii​∈Ip​​的平均值。
在每次迭代中，对参数$\mathbf{x}$进行更新：
$${\mathbf{x}}^{k+1}={\mathbf{x}}^k+\Delta \mathbf{x}$$
其中，$\Delta \mathbf{x}$是如下线性方程的解：
$${\mathbf{J}}_{\mathbf{r}}^{\mathbf{T}}{\mathbf{J}}_{\mathbf{r}}\Delta \mathbf{x}=-{\mathbf{J}}_{\mathbf{r}}^{\mathbf{T}}\mathbf{r}$$
其中，$\mathbf{r}=\mathbf{r}(\mathbf{x})$是目标函数中的残差项，${\mathbf{J}}_{\mathbf{r}}={\mathbf{J}}_{\mathbf{r}}(\mathbf{x})$是$\mathbf{r}$的雅可比矩阵（Jacobian）：
$$\begin{gathered} \mathbf{r}=[r_{i,\mathbf{p}}(\mathbf{x}){\mid }_{\mathbf{x}={\mathbf{x}}^k}{]}_{(i,\mathbf{p})}, \\ {\mathbf{J}}_{\mathbf{r}}=[\nabla r_{i,\mathbf{p}}(\mathbf{x}){\mid }_{\mathbf{x}={\mathbf{x}}^k}{]}_{(i,\mathbf{p})} \end{gathered}$$
在每次迭代中，根据目标函数的定义计算残差项r，$r_{i,\mathbf{p}}$关于$\mathbf{C}$和${\mathbf{T}}_j{\mid }_{j\ne i}$的偏导都是平凡解。为了计算$r_{i,\mathbf{p}}$关于${\mathbf{T}}_i$的偏导，我们将${\mathbf{T}}_i$局部线性化近似为${\mathbf{T}}_i^k$：
$${\mathbf{T}}_i\approx \left(\begin{array}{cccc}1& -{\gamma }_i& {\beta }_i& {\alpha }_i\\ {\gamma }_i& 1& -{\alpha }_i& b_i\\ -{\beta }_i& {\alpha }_i& 1& c_i\\ 0& 0& 0& 1\end{array}\right){\mathbf{T}}_i^k$$
令${\xi }_i=({\alpha }_i,{\beta }_i,{\gamma }_i,a_i,b_i,c_i{)}^{\mathbf{T}}$，$\Delta \mathbf{x}$则为$C(\mathbf{p})$和${\xi }_i$的组合.由链式法则，可得：
$$\nabla r_{i,\mathbf{p}}({\xi }_i){\mid }_{\mathbf{x}={\mathbf{x}}^k}=-\frac{\partial }{\partial {\xi }_i}({\Gamma }_i\circ \mathbf{u}\circ \mathbf{g}){\mid }_{\mathbf{x}={\mathbf{x}}^k}=-\nabla {\Gamma }_i(\mathbf{u}){\mathbf{J}}_{\mathbf{u}}(\mathbf{g}){\mathbf{J}}_{\mathbf{g}}({\xi }_i){\mid }_{\mathbf{x}={\mathbf{x}}^k}$$
其中，$\nabla {\Gamma }_i$是${\Gamma }_i$的梯度，使用Scharr核计算。${\mathbf{J}}_{\mathbf{u}}(\mathbf{g})$是u的Jacobian矩阵，${\mathbf{J}}_{\mathbf{g}}({\xi }_i)$是g关于${\xi }_i$的Jacobian矩阵.

交替优化法

高斯牛顿法计算复杂度很高，因此作者提出了一种交替优化法，每次迭代交替优化C和T。
当T固定时，非线性最小二乘问题就变成了线性最小而成问题，并且有以下闭解：
$$C(\mathbf{p})=\frac{1}{n_{\mathbf{P}}}\sum {\Gamma }_i(\mathbf{p},{\mathbf{T}}_i)$$
其中$n_{\mathbf{P}}$是与顶点p相关的图片的个数，这样就相当于只需计算p在各个与其相关的图片上的投影点的平均灰度值。
当C固定时，目标函数分解为多个独立的目标函数，每个${\mathbf{T}}_i$对应一个：
$$E_i(\mathbf{T})=\sum _{\mathbf{p}\in {\mathbf{P}}_i}{r}_{i,\mathbf{p}}^2$$
每一项$E_i(\mathbf{T})$只和${\xi }_i$的六个变量有关，在每次迭代中，使用高斯牛顿法迭代更新这六个变量，每个${\mathbf{T}}_i$都是相互独立的，因此可以并行计算。这样的计算是非常高效的。

非刚体校正

非刚体校正是为了修复不准确的Mesh和镜头畸变等导致的误差。

目标函数

对每一张图片$I_i$，非刚体校正可以用一个变形函数$F_i$表示。
$${\mathbf{F}}_i({\mathbf{v}}_{i,l})={\mathbf{v}}_{i,l}+{\mathbf{f}}_{i,l}$$
其中，${\mathbf{v}}_{i,l}\in {\mathbf{V}}_i$，表示变形的控制范围。${\mathbf{f}}_{i,l}$是一个二维向量，该图像平面上的所有投影点$\mathbf{u}$校正为：
$${\mathbf{F}}_i(\mathbf{u})=\mathbf{u}+\sum _i{\theta }_l(\mathbf{u}){\mathbf{f}}_{i,l}$$
${\theta }_l$为双线性插值函数，则${\mathbf{F}}_i(\mathbf{u})$是 { F i ( v i , l ) } v i , l ∈ V i \{\mathbf F_i(\mathbf v_{i,l})\}_{\mathbf v_{i,l}\in\mathbf V_i} {Fi​(vi,l​)}vi,l​∈Vi​​的线性组合。${\mathbf{V}}_i$是一个$20\times 16$的网格（$21\times 17$个控制点）。
之前的目标函数可纳入非刚体变换：
$$E_c(\mathbf{C},\mathbf{T},\mathbf{F})=\sum _i\sum _{\mathbf{p}\in {\mathbf{P}}_i}{r}_{i,\mathbf{p}}^2$$
残差项为
$$r_{i,\mathbf{p}}=C(\mathbf{p})-{\Gamma }_i({\mathbf{F}}_i(\mathbf{u}(\mathbf{g}(\mathbf{p},{\mathbf{T}}_i))))$$
为了防止校正函数漂移，加入$L^2$正则项：
$$E_r(\mathbf{F})=\sum _i\sum _l{\mathbf{f}}_{i,l}^T{\mathbf{f}}_{i,l}$$
则完整的目标函数为：
$$E(\mathbf{C},\mathbf{T},\mathbf{F})=E_c(\mathbf{C},\mathbf{T},\mathbf{F})+\lambda Er(\mathbf{F})$$
实际中作者使用$\lambda =0.1$

优化

目标函数引入了$m+720n$个变量，直接使用高斯牛顿法计算量非常大，而交替优化法可以将优化问题转换为独立线性问题，只需720个变量。
在第k论迭代中，首先固定T和F，优化C。
然后固定C，优化T和F。

python实现

在open3d开源库中，集成了这篇论文的算法。作者所用的数据库可以点击这里下载。
在open3d中，给出了demo代码如下：

# examples/Python/Advanced/o3d.color_map.color_map_optimization.py

import open3d as o3d
from trajectory_io import *
import os, sys
sys.path.append("../Utility")
from file import *

path = "[path_to_fountain_dataset]"
debug_mode = False

if __name__ == "__main__":
    o3d.utility.set_verbosity_level(o3d.utility.VerbosityLevel.Debug)

    # Read RGBD images
    rgbd_images = []
    depth_image_path = get_file_list(os.path.join(path, "depth/"),
                                     extension=".png")
    color_image_path = get_file_list(os.path.join(path, "image/"),
                                     extension=".jpg")
    assert (len(depth_image_path) == len(color_image_path))
    for i in range(len(depth_image_path)):
        depth = o3d.io.read_image(os.path.join(depth_image_path[i]))
        color = o3d.io.read_image(os.path.join(color_image_path[i]))
        rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(
            color, depth, convert_rgb_to_intensity=False)
        if debug_mode:
            pcd = o3d.geometry.PointCloud.create_from_rgbd_image(
                rgbd_image,
                o3d.camera.PinholeCameraIntrinsic(
                    o3d.camera.PinholeCameraIntrinsicParameters.
                    PrimeSenseDefault))
            o3d.visualization.draw_geometries([pcd])
        rgbd_images.append(rgbd_image)

    # Read camera pose and mesh
    camera = o3d.io.read_pinhole_camera_trajectory(
        os.path.join(path, "scene/key.log"))
    mesh = o3d.io.read_triangle_mesh(
        os.path.join(path, "scene", "integrated.ply"))

    # Before full optimization, let's just visualize texture map
    # with given geometry, RGBD images, and camera poses.
    option = o3d.color_map.ColorMapOptimizationOption()
    option.maximum_iteration = 0
    o3d.color_map.color_map_optimization(mesh, rgbd_images, camera, option)
    o3d.visualization.draw_geometries([mesh])
    o3d.io.write_triangle_mesh(
        os.path.join(path, "scene", "color_map_before_optimization.ply"), mesh)

    # Optimize texture and save the mesh as texture_mapped.ply
    # This is implementation of following paper
    # Q.-Y. Zhou and V. Koltun,
    # Color Map Optimization for 3D Reconstruction with Consumer Depth Cameras,
    # SIGGRAPH 2014
    option.maximum_iteration = 300
    option.non_rigid_camera_coordinate = True
    o3d.color_map.color_map_optimization(mesh, rgbd_images, camera, option)
    o3d.visualization.draw_geometries([mesh])
    o3d.io.write_triangle_mesh(
        os.path.join(path, "scene", "color_map_after_optimization.ply"), mesh)

使用作者的数据跑出来效果还是很不错的，但是在实测中使用自己扫描的数据，效果就差了一些。