读源码学算法之TSDF Volume模型
目前稠密三维重建主要使用两种框架，分别是基于体素的(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} Z⎣⎡uv1⎦⎤=⎣⎡fx000fy0cxcy1⎦⎤⋅⎣⎡XYZ⎦⎤
展开写就是这个样子：
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 (u−cx)/fx=X/Z， ( v − c y ) / f y = Y / Z (v - c_y) / f_y= Y/Z (v−cy)/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.sdf∗vox.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