读源码学算法之TSDF Volume模型

图1、ScalableTSDFVolume生成的mesh和tsdf模型

目前稠密三维重建主要使用两种框架，分别是基于体素的(volumetric-based) TSDF框架 和 基于面元（surfel-based）框架。基于体素的框架可以通过维护重建的历史信息，可以获得紧致的曲面和高质量的重建效果，在kinectfusion等一系列经典方法中被广泛的应用。

首先，推荐一个3D开源算法库：Open3D，它实现了很多经典的三维数据几何处理算法，代码风格友好，非常容易阅读。
Open3D介绍：http://www.open3d.org/docs/introduction.html
Github链接：https://github.com/intel-isl/Open3D.

之前看论文对TSDF的认识仅仅停留在表面，基于科研需要，为了深入学习TSDF框架及marching cubes网格抽取算法，我最近对TSDF部分进行了详细的研读。代码地址：https://github.com/intel-isl/Open3D/blob/master/examples/Cpp/IntegrateRGBD.cpp

测试代码用的是：ScalableTSDFVolume， 这实际上是原始TSDF（UniformTSDFVolume）的子类，它叫做Voxel Hashing， 它试用于较大场景的三维场景重建，并且可以高效的管理内存。而我这里主要讲UniformTSDFVolume的具体实现，主要涉及Integrate和ExtractTriangleMes两个函数。

1、UniformTSDFVolume::Integrate

除去前面的一大堆判断，这一部分主要有两行代码：

//计算深度图中每个点投影到相机座标后，该点到光心的距离 / 该点实际的深度
auto depth2cameradistance = geometry::Image::CreateDepthToCameraDistanceMultiplierFloatImage(intrinsic);
//实际的Integrate操作
IntegrateWithDepthToCameraDistanceMultiplier(image, intrinsic, extrinsic,*depth2cameradistance);

1.1 CreateDepthToCameraDistanceMultiplierFloatImage

接下来深入到第一个函数：CreateDepthToCameraDistanceMultiplierFloatImage， 它根据一张深度图上每个像素的深度和位置，计算出每个距离深度比，方便后续根据深度数据直接算出体素的实际距离。

std::shared_ptr<Image> Image::CreateDepthToCameraDistanceMultiplierFloatImage(
        const camera::PinholeCameraIntrinsic &intrinsic) {
    auto fimage = std::make_shared<Image>();
    fimage->Prepare(intrinsic.width_, intrinsic.height_, 1, 4);
    float ffl_inv[2] = {
            1.0f / (float)intrinsic.GetFocalLength().first,  //fx
            1.0f / (float)intrinsic.GetFocalLength().second, //fy
    };
    float fpp[2] = {
            (float)intrinsic.GetPrincipalPoint().first,     //cx
            (float)intrinsic.GetPrincipalPoint().second,    //cy
    };
    std::vector<float> xx(intrinsic.width_);    //640
    std::vector<float> yy(intrinsic.height_);   //480 
    for (int j = 0; j < intrinsic.width_; j++) { 
        xx[j] = (j - fpp[0]) * ffl_inv[0];      // (j-cx)/fx
    }
    for (int i = 0; i < intrinsic.height_; i++) {
        yy[i] = (i - fpp[1]) * ffl_inv[1];      // (i-cy)/fy
    }
    for (int i = 0; i < intrinsic.height_; i++) {
        float *fp =(float *)(fimage->data_.data() + i * fimage->BytesPerLine());
        for (int j = 0; j < intrinsic.width_; j++, fp++) {
            *fp = sqrtf(xx[j] * xx[j] + yy[i] * yy[i] + 1.0f);  // sqrt(x^2+y^2+z^2)/z, 距离深度比
        }
    }
    return fimage;
}

相机模型为：

$Z\begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = \begin{bmatrix} f_x & 0 & c_x\\ 0 & f_y & c_y\\ 0 & 0 & 1 \end{bmatrix} \cdot \begin{bmatrix}X \\ Y \\ Z \end{bmatrix}$

展开写就是这个样子：

$u = Xf_x/Z+ c_x$， $v = Yf_y/Z+ c_y$

那么，

$(u - cx)/ f_x= X/Z$， $(v - c_y) / f_y= Y/Z$

所以

$\sqrt{X^2/Z^2 + Y^2/Z^2 + 1} = \sqrt{X^2/Z^2 + Y^2/Z^2 + Z^2/Z^2} = \sqrt{X^2 + Y^2 + Z^2} / Z$

1.2 IntegrateWithDepthToCameraDistanceMultiplier

接下来深入到第二个函数：IntegrateWithDepthToCameraDistanceMultiplier， 这是最主要的函数。
先上代码，然后分析

void UniformTSDFVolume::IntegrateWithDepthToCameraDistanceMultiplier(
        const geometry::RGBDImage &image,
        const camera::PinholeCameraIntrinsic &intrinsic,
        const Eigen::Matrix4d &extrinsic,
        const geometry::Image &depth_to_camera_distance_multiplier) {
    const float fx = static_cast<float>(intrinsic.GetFocalLength().first);
    const float fy = static_cast<float>(intrinsic.GetFocalLength().second);
    const float cx = static_cast<float>(intrinsic.GetPrincipalPoint().first);
    const float cy = static_cast<float>(intrinsic.GetPrincipalPoint().second);
    const Eigen::Matrix4f extrinsic_f = extrinsic.cast<float>();
    const float voxel_length_f = static_cast<float>(voxel_length_);
    const float half_voxel_length_f = voxel_length_f * 0.5f;
    const float sdf_trunc_f = static_cast<float>(sdf_trunc_);
    const float sdf_trunc_inv_f = 1.0f / sdf_trunc_f;
    const Eigen::Matrix4f extrinsic_scaled_f = extrinsic_f * voxel_length_f;
    const float safe_width_f = intrinsic.width_ - 0.0001f;
    const float safe_height_f = intrinsic.height_ - 0.0001f;

    for (int x = 0; x < resolution_; x++) {
        for (int y = 0; y < resolution_; y++) {
            Eigen::Vector4f pt_3d_homo(float(half_voxel_length_f + voxel_length_f * x + origin_(0)),
                                       float(half_voxel_length_f + voxel_length_f * y + origin_(1)),
                                       float(half_voxel_length_f + origin_(2)),
                                       1.f);
            Eigen::Vector4f pt_camera = extrinsic_f * pt_3d_homo;
            
            for (int z = 0; z < resolution_; z++,  
                     pt_camera(0) += extrinsic_scaled_f(0, 2),
                     pt_camera(1) += extrinsic_scaled_f(1, 2),
                     pt_camera(2) += extrinsic_scaled_f(2, 2)) {
                // Skip if negative depth after projection
                if (pt_camera(2) <= 0)
                    continue;

                // Skip if x-y coordinate not in range
                float u_f = pt_camera(0) * fx / pt_camera(2) + cx + 0.5f;
                float v_f = pt_camera(1) * fy / pt_camera(2) + cy + 0.5f;  
                if (!(u_f >= 0.0001f && u_f < safe_width_f && v_f >= 0.0001f && v_f < safe_height_f)) 
                    continue;
               
                // Skip if negative depth in depth image
                int u = (int)u_f;
                int v = (int)v_f;
                float d = *image.depth_.PointerAt<float>(u, v);
                if (d <= 0.0f) 
                    continue;
               
                int v_ind = IndexOf(x, y, z);
                float sdf = (d - pt_camera(2)) *(*depth_to_camera_distance_multiplier.PointerAt<float>(u, v));
                if (sdf > -sdf_trunc_f) {  
                    // integrate
                    float tsdf = std::min(1.0f, sdf * sdf_trunc_inv_f);
                    voxels_[v_ind].tsdf_ = (voxels_[v_ind].tsdf_ * voxels_[v_ind].weight_ + tsdf) /
                            (voxels_[v_ind].weight_ + 1.0f);
                    if (color_type_ == TSDFVolumeColorType::RGB8) {
                        const uint8_t *rgb = image.color_.PointerAt<uint8_t>(u, v, 0);
                        Eigen::Vector3d rgb_f(rgb[0], rgb[1], rgb[2]);
                        voxels_[v_ind].color_ = (voxels_[v_ind].color_ * voxels_[v_ind].weight_ +rgb_f) /(voxels_[v_ind].weight_ + 1.0f);
                    } else if (color_type_ == TSDFVolumeColorType::Gray32) {
                        const float *intensity =image.color_.PointerAt<float>(u, v, 0);
                        voxels_[v_ind].color_ = (voxels_[v_ind].color_.array() * voxels_[v_ind].weight_ + (*intensity)) /(voxels_[v_ind].weight_ + 1.0f);
                    }
                    voxels_[v_ind].weight_ += 1.0f;
                }
            }
        }
    }
}

19-33行：从$z = 0$所在平面依次遍历所有voxel， 这里为了加速计算，先计算$(x,y,0)$体素所在中心点（+half_voxel_length_f）在世界座标系的座标。

Eigen::Vector4f pt_3d_homo(float(half_voxel_length_f + voxel_length_f * x + origin_(0)),
                           float(half_voxel_length_f + voxel_length_f * y + origin_(1)),
                           float(half_voxel_length_f + origin_(2)),
                           1.f);

注意：世界座标系可能不在体素(0,0,0)的位置，但世界座标系一般与体素的座标系的方向保持一致，（即没有旋转但可能有相对平移变换）。如果世界座标系在体素中（2,2,0）的位置，那么这里origin_=（-2,-2,0）。 这里加上 origin_实际就是移动体素的原点位置。

Eigen::Vector4f pt_camera = extrinsic_f * pt_3d_homo;
for (int z = 0; z < resolution_; z++,    
     pt_camera(0) += extrinsic_scaled_f(0, 2),
     pt_camera(1) += extrinsic_scaled_f(1, 2),
     pt_camera(2) += extrinsic_scaled_f(2, 2))

pt_camera是世界座标系中pt_3d_homo转移到当前相机座标系的结果。但这来自于$(x,y,0)$处体素的转换结果。 随着$z$每次递增1，其实只需要在pt_camera的基础上不断累加extrinsic_scaled_f( 0/1/2, 2 )即可，这个很容易推导。

36-49行：将相机座标系中pt_camera，转换到当前图像空间，+0.5是为了四舍五入。从而得到图像空间的位置$(u,v)$。
进一步，通过实际深度 与 测量深度 之差 （×距离深度比） 拿到距离之差，也就是sdf值。

float u_f = pt_camera(0) * fx / pt_camera(2) + cx + 0.5f;
float v_f = pt_camera(1) * fy / pt_camera(2) + cy + 0.5f; 
int u = (int)u_f;
int v = (int)v_f;
float d = *image.depth_.PointerAt<float>(u, v);  //取得深度图中(u, v)点的深度， 然后利用1.2中距离深度比拿到实际距离。
float sdf = (d - pt_camera(2)) * (*depth_to_camera_distance_multiplier.PointerAt<float>(u, v));

52-63行：接下来，sdf融合。公式为：
$vox.sdf = (vox.sdf * vox.w + sdf) / (vox.w+ 1)$
$vox.w = vox.w + 1$

 voxels_[v_ind].tsdf_ =(voxels_[v_ind].tsdf_ * voxels_[v_ind].weight_ + tsdf) /
                       (voxels_[v_ind].weight_ + 1.0f);
 voxels_[v_ind].weight_ += 1.0f;

2、UniformTSDFVolume::ExtractTriangleMes

std::shared_ptr<geometry::TriangleMesh> UniformTSDFVolume::ExtractTriangleMesh() {
    // implementation of marching cubes, based on http://paulbourke.net/geometry/polygonise/
    auto mesh = std::make_shared<geometry::TriangleMesh>();
    double half_voxel_length = voxel_length_ * 0.5;
    // Map of "edge_index = (x, y, z, 0) + edge_shift" to "global vertex index"
    std::unordered_map<
            Eigen::Vector4i, int, utility::hash_eigen::hash<Eigen::Vector4i>,
            std::equal_to<Eigen::Vector4i>,
            Eigen::aligned_allocator<std::pair<const Eigen::Vector4i, int>>>
            edgeindex_to_vertexindex;
    int edge_to_index[12];
    for (int x = 0; x < resolution_ - 1; x++) {
        for (int y = 0; y < resolution_ - 1; y++) {
            for (int z = 0; z < resolution_ - 1; z++) {
                int cube_index = 0;
                float f[8];  //依次遍历voxel的8个顶点
                Eigen::Vector3d c[8];
                for (int i = 0; i < 8; i++) {
                    Eigen::Vector3i idx = Eigen::Vector3i(x, y, z) + shift[i];
                    if (voxels_[IndexOf(idx)].weight_ == 0.0f) {
                        cube_index = 0;
                        break;
                    } else {
                        f[i] = voxels_[IndexOf(idx)].tsdf_;
                        if (f[i] < 0.0f) {
                            cube_index |= (1 << i); //内部的顶点，对应的位标记为1
                        }
                        if (color_type_ == TSDFVolumeColorType::RGB8) {
                            c[i] = voxels_[IndexOf(idx)].color_.cast<double>() / 255.0;
                        } else if (color_type_ == TSDFVolumeColorType::Gray32) {
                            c[i] = voxels_[IndexOf(idx)].color_.cast<double>();
                        }
                    }
                }
                //完全在曲面内部或外部不予考虑，因为没有面穿过当前voxel
                if (cube_index == 0 || cube_index == 255) { 
                    continue;
                }
                for (int i = 0; i < 12; i++) { //依次遍历voxel的12条边
                    if (edge_table[cube_index] & (1 << i)) { //当前曲面与当前voxel的第i条边相交
                        Eigen::Vector4i edge_index = Eigen::Vector4i(x, y, z, 0) + edge_shift[i];
                        if (edgeindex_to_vertexindex.find(edge_index) == edgeindex_to_vertexindex.end()) {
                            edge_to_index[i] = (int)mesh->vertices_.size();  //当前边对应的交点编号
                            edgeindex_to_vertexindex[edge_index] =(int)mesh->vertices_.size(); //存入上述映射
                            Eigen::Vector3d pt( //相交边的起点所在voxel的中心
                                    half_voxel_length + voxel_length_ * edge_index(0), 
                                    half_voxel_length + voxel_length_ * edge_index(1),
                                    half_voxel_length + voxel_length_ * edge_index(2));
                            double f0 = std::abs((double)f[edge_to_vert[i][0]]);  //edge_index第1个端点的sdf
                            double f1 = std::abs((double)f[edge_to_vert[i][1]]);  //edge_index第2个端点的sdf
                            pt(edge_index(3)) += f0 * voxel_length_ / (f0 + f1);  //插值得到曲面交点
                            mesh->vertices_.push_back(pt + origin_); //新的曲面交点插入mesh中

                            if (color_type_ != TSDFVolumeColorType::NoColor) {
                                const auto &c0 = c[edge_to_vert[i][0]];
                                const auto &c1 = c[edge_to_vert[i][1]];
                                mesh->vertex_colors_.push_back((f1 * c0 + f0 * c1) / (f0 + f1));
                            }
                        } else {
                            edge_to_index[i] = edgeindex_to_vertexindex.find(edge_index) ->second;
                        }
                    }
                }
                for (int i = 0; tri_table[cube_index][i] != -1; i += 3) {
                    mesh->triangles_.push_back(Eigen::Vector3i(
                            edge_to_index[tri_table[cube_index][i]],
                            edge_to_index[tri_table[cube_index][i + 2]],
                            edge_to_index[tri_table[cube_index][i + 1]]));
                }
            }
        }
    }
    return mesh;
}

18-34行：依次遍历座标为$(x,y,z)$的体素的8个顶点，这里使用了shift变量， 用于标记所有8个顶点相对于$(x,y,z)$的偏移量，定义如下：

const Eigen::Vector3i shift[8] = {
        Eigen::Vector3i(0, 0, 0), Eigen::Vector3i(1, 0, 0),
        Eigen::Vector3i(1, 1, 0), Eigen::Vector3i(0, 1, 0),
        Eigen::Vector3i(0, 0, 1), Eigen::Vector3i(1, 0, 1),
        Eigen::Vector3i(1, 1, 1), Eigen::Vector3i(0, 1, 1),
};

同时使用8个二进制位，用1标记曲面内部的顶点（sdf < 0）, 0标记曲面外的顶点

f[i] = voxels_[IndexOf(idx)].tsdf_;
if (f[i] < 0.0f) 
    cube_index |= (1 << i); //内部的顶点，对应的位标记为1， 否则标记为0

39-41行：依次遍历座标为$(x,y,z)$的体素的12条边，这里使用了edge_shift变量，标记了边的起点和方向（有了方向相当于指明了边的终点），edge_shift定义如下， 前三个数表示起点，最后一个数表示方向，看如下注释很清楚。

// First 3 elements: edge start vertex coordinate (assume origin at (0, 0, 0))
// The last element: edge direction {0: x, 1: y, 2: z}
const Eigen::Vector4i edge_shift[12] = {
        Eigen::Vector4i(0, 0, 0, 0),  // Edge  0: {0, 1}
        Eigen::Vector4i(1, 0, 0, 1),  // Edge  1: {1, 2}
        Eigen::Vector4i(0, 1, 0, 0),  // Edge  2: {3, 2}
        Eigen::Vector4i(0, 0, 0, 1),  // Edge  3: {0, 3}
        Eigen::Vector4i(0, 0, 1, 0),  // Edge  4: {4, 5}
        Eigen::Vector4i(1, 0, 1, 1),  // Edge  5: {5, 6}
        Eigen::Vector4i(0, 1, 1, 0),  // Edge  6: {7, 6}
        Eigen::Vector4i(0, 0, 1, 1),  // Edge  7: {4, 7}
        Eigen::Vector4i(0, 0, 0, 2),  // Edge  8: {0, 4}
        Eigen::Vector4i(1, 0, 0, 2),  // Edge  9: {1, 5}
        Eigen::Vector4i(1, 1, 0, 2),  // Edge 10: {2, 6}
        Eigen::Vector4i(0, 1, 0, 2),  // Edge 11: {3, 7}
};

42-61行：注意这里有一个变量叫：edgeindex_to_vertexindex，请注意它的定义方式，他实际上是一个map， 对每一条与模型表面相交的边edgeindex，与edgeindex上具体的交点 vertexindex绑定（每个交点给定一个全局的编号）。

std::unordered_map< Eigen::Vector4i, int, utility::hash_eigen::hash<Eigen::Vector4i>,
                    std::equal_to<Eigen::Vector4i>,
                    Eigen::aligned_allocator<std::pair<const Eigen::Vector4i, int>>
                  > edgeindex_to_vertexindex;

42-44行：如果当前边edge_index还没存入edgeindex_to_vertexindex，那么接下来就要把edge_index和对应的交点存入

edge_to_index[i] = (int)mesh->vertices_.size();  //当前边绑定的交点 编号
edgeindex_to_vertexindex[edge_index] =(int)mesh->vertices_.size(); //将上述映射存入edgeindex_to_vertexindex

45-48行：edge_index起点所在的voxel的中心。
49-52行：得到edge_index的两个sdf，并通过插值（注意sdf必然一正一负）得到交点，并将新的交点插入mesh中。

pt(edge_index(3)) += f0 * voxel_length_ / (f0 + f1);

59-61行：如果当前边edge_index已经存入edgeindex_to_vertexindex，那么接下来就只需要提供对应的交点即可
64-70行：上面的代码已经得到了所有的交点，接下来将临近的3个交点，构成一个三角面片插入。

for (int i = 0; tri_table[cube_index][i] != -1; i += 3) {
    mesh->triangles_.push_back(Eigen::Vector3i(
          						edge_to_index[tri_table[cube_index][i]],
                            	edge_to_index[tri_table[cube_index][i + 2]],
                           	 	edge_to_index[tri_table[cube_index][i + 1]])
                           	  );
}

参考资料
Kinect Fusion 算法浅析：精巧中带坑
三维重建中的表面模型构建–TSDF算法
https://github.com/andyzeng/tsdf-fusion