读源码学算法之TSDF Volume模型

读源码学算法之TSDF Volume模型_第1张图片

图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 [ u v 1 ] = [ f x 0 c x 0 f y c y 0 0 1 ] ⋅ [ X Y Z ] 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} Zuv1=fx000fy0cxcy1XYZ

展开写就是这个样子:

u = X f x / Z + c x u = Xf_x/Z+ c_x u=Xfx/Z+cx v = Y f y / Z + c y v = Yf_y/Z+ c_y v=Yfy/Z+cy

那么,

( u − c x ) / f x = X / Z (u - cx)/ f_x= X/Z (ucx)/fx=X/Z ( v − c y ) / f y = Y / Z (v - c_y) / f_y= Y/Z (vcy)/fy=Y/Z

所以

X 2 / Z 2 + Y 2 / Z 2 + 1 = X 2 / Z 2 + Y 2 / Z 2 + Z 2 / Z 2 = X 2 + Y 2 + Z 2 / 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 X2/Z2+Y2/Z2+1 =X2/Z2+Y2/Z2+Z2/Z2 =X2+Y2+Z2 /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 z = 0 z=0所在平面依次遍历所有voxel, 这里为了加速计算,先计算 ( x , y , 0 ) (x,y,0) (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 ) (x,y,0) (x,y,0)处体素的转换结果。 随着 z z z每次递增1,其实只需要在pt_camera的基础上不断累加extrinsic_scaled_f( 0/1/2, 2 )即可,这个很容易推导。

36-49行:将相机坐标系中pt_camera,转换到当前图像空间,+0.5是为了四舍五入。从而得到图像空间的位置 ( u , v ) (u,v) (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融合。公式为:
v o x . s d f = ( v o x . s d f ∗ v o x . w + s d f ) / ( v o x . w + 1 ) vox.sdf = (vox.sdf * vox.w + sdf) / (vox.w+ 1) vox.sdf=(vox.sdfvox.w+sdf)/(vox.w+1)
v o x . w = v o x . w + 1 vox.w = 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 ) (x,y,z) (x,y,z)的体素的8个顶点,这里使用了shift变量, 用于标记所有8个顶点相对于 ( x , y , z ) (x,y,z) (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 ) (x,y,z) (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

你可能感兴趣的:(Computer,Graphics)