（以下主要以Open3D庫為例，解析TSDF演算法的具體流程）

前言

Open3D是Inter Visual Computing Lab的研究科學家Qianyi Zhou（前）和Jaesik Park共同完成的開源庫，支持處理3D數據的軟體快速開發。

http://www.open3d.org/docs/introduction.html

https://github.com/IntelVCL/Open3D

TSDF（truncated signed distance function）是一種利用結構化點雲數據並以參數表達表面的表面重建演算法。核心是將點雲數據映射到一個預先定義的三維立體空間中，並用截斷符號距離函數表示真實場景表面附近的區域，建立表面模型[1]。

TSDF演算法流程

先大致介紹下演算法流程，再對照下面具體代碼進行分析。

1.對於構造的立體中的每個體素g,轉化g為世界坐標系下得三維位置點p(推算);

2.由第i幀深度數據的相機位姿矩陣 T_(??,??) ,由 T_(??,??)^{-1}p 求得p在相機坐標系下得映射點v;並由Kinect相機內參矩陣，反投影v點求深度圖像中的對應像素點x；

3.計算點p沿投影光線到最近表面的距離 f_i (p) 。首先計算距離符號函數:

??????_?? (??)←‖T_(??,??)^{-1} ??‖???_?? (??)

如果 sdf_i (p) 大於0時，取

??_?? (??)=min?(1,(??????_?? (??))/??)??????(??????_?? (??))

否則取

??_?? (??)=max?(?1,(??????_?? (??))/??)??????(??????_?? (??))

4.計算 f_i (p) 對應的權重係數 w_i (p) : 其中θ為投影光線與表面法向量的夾角;

w_i (p)∝cos?(θ)/R_i (x)

5. F_i=(F_{i-1}?W_{i-1}+W_i?f_i)/(W_{i-1}+W_i ) W_i=W_{i-1}+W_i

進行融合求得截斷符號距離函數 F_i (p) 與權重 W_i(p) ;

6.將 ??_?? (??) 和 W_i(p) 存儲於體素g中;

7.實現所有數據幀對應點雲融合。利用光線投影法找到的所有 F_i (p) 值為0的體素點g,可以映射到世界坐標中提取出場景模型的表面點雲[2]。

Open3D中的TSDF

  • 測試主函數

int main(int argc, char *argv[])
{
using namespace three;

auto volume = ScalableTSDFVolume::ScalableTSDFVolume(4.0 / 512.0, 0.04, TSDFVolumeColorType::RGB8);
// 聲明TSDF體的voxel_length(體素邊長），sdf_trunc（截斷距離），colorType(著色需要）

vector<Eigen::Matrix4d> Extrinsics;
GetExtrinsic(filename_log, Extrinsics); //獲取外參

for (int i = 0; i < 5; i++) {
Eigen::Matrix4d extrinsic = Extrinsics[i];
Image image_color, image_depth;
ReadImageFromJPG(filename_color, image_color);
ReadImageFromPNG(filename_depth, image_depth);
shared_ptr<RGBDImage> image_rgbd = CreateRGBDImageFromColorAndDepth(image_color, image_depth, 1000, 3.0, false);
// 融合生成 RGB + depth 格式圖
 volume.Integrate(*image_rgbd, PinholeCameraIntrinsicParameters::PrimeSenseDefault, extrinsic); // 核心函數:利用該幀數據生成TSDF
 }

auto mesh = volume.ExtractTriangleMesh();//三角化表面模型
 mesh->ComputeVertexNormals();//計演算法向量
 DrawGeometries({ mesh }, "Mesh");//顯示Mesh

return 1;
}

(用Open3D里的RGBD數據作測試,5幀RGBD）

  • 深入Integrate() 理解TSDF過程

void ScalableTSDFVolume::Integrate(const RGBDImage &image,
const PinholeCameraIntrinsic &intrinsic,
const Eigen::Matrix4d &extrinsic)
{
if ((image.depth_.num_of_channels_ != 1) ||
(image.depth_.bytes_per_channel_ != 4) ||
(image.depth_.width_ != intrinsic.width_) ||
(image.depth_.height_ != intrinsic.height_) ||
(color_type_ == TSDFVolumeColorType::RGB8 &&
image.color_.num_of_channels_ != 3) ||
(color_type_ == TSDFVolumeColorType::RGB8 &&
image.color_.bytes_per_channel_ != 1) ||
(color_type_ == TSDFVolumeColorType::Gray32 &&
image.color_.num_of_channels_ != 1) ||
(color_type_ == TSDFVolumeColorType::Gray32 &&
image.color_.bytes_per_channel_ != 4) ||
(color_type_ != TSDFVolumeColorType::None &&
image.color_.width_ != intrinsic.width_) ||
(color_type_ != TSDFVolumeColorType::None &&
image.color_.height_ != intrinsic.height_)) {
PrintWarning("[ScalableTSDFVolume::Integrate] Unsupported image format.
");
return;
}
auto depth2cameradistance = CreateDepthToCameraDistanceMultiplierFloatImage
intrinsic);
auto pointcloud = CreatePointCloudFromDepthImage(image.depth_, intrinsic,
extrinsic, 1000.0, 1000.0, depth_sampling_stride_);
std::unordered_set<Eigen::Vector3i, hash_eigen::hash<Eigen::Vector3i>>
touched_volume_units_;
for (const auto &point : pointcloud->points_) {
auto min_bound = LocateVolumeUnit(point - Eigen::Vector3d(
sdf_trunc_, sdf_trunc_, sdf_trunc_));
auto max_bound = LocateVolumeUnit(point + Eigen::Vector3d(
sdf_trunc_, sdf_trunc_, sdf_trunc_));
for (auto x = min_bound(0); x <= max_bound(0); x++) {
for (auto y = min_bound(1); y <= max_bound(1); y++) {
for (auto z = min_bound(2); z <= max_bound(2); z++) {
auto loc = Eigen::Vector3i(x, y, z);
if (touched_volume_units_.find(loc) ==
touched_volume_units_.end()) {
touched_volume_units_.insert(loc);
auto volume = OpenVolumeUnit(Eigen::Vector3i(x, y, z));
volume->IntegrateWithDepthToCameraDistanceMultiplier(
image, intrinsic, extrinsic,
*depth2cameradistance);
}
}
}
}
}
}

（取自Open3DsrcCoreIntegrationScalebleTSDFVolume.cpp）

1.CreateDepthToCameraDistanceMultiplierFloatImage():

創建一個和深度圖同樣大小的浮點圖，其中像素(i,j)的值為

√(((?????_??)/??_?? )^2+((?????_??)/??_?? )^2+1)

由像平面坐標繫到相機坐標系的轉換關係：

得： (u?u_0)/??_?? =??/?? (v?v_0)/??_?? =??/??

√(((u?u_0)/??_?? )^2+((v-v_0)/??_?? )^2+1)=√(??^2+??^2+??^2 )/??

即沿相機光心到目標點的投影距離與深度的比值，或者說由深度到投影距離的一種轉換關係。

2.CreatePointCloudFromDepthImage() ：

獲取從深度圖轉換到空間坐標系下的點雲坐標。

3.按上一步讀取某點，遍歷該point±sdf_trunc的點坐標;

4.volume=OpenVolumeUnit(Eigen::Vector3i(x,y,z))：

以遍歷的點為起始點(origin_)創建體素。

體素的構建

5. volume->IntegrateWithDepthToCameraDistanceMultiplier()：

生成TSDF隱式函數(tsdf_)

  • 深入IntegrateWithDepthToCameraDistanceMultiplier()

void UniformTSDFVolume::IntegrateWithDepthToCameraDistanceMultiplier(
const RGBDImage &image, const PinholeCameraIntrinsic &intrinsic,
const Eigen::Matrix4d &extrinsic,
const 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;

#ifdef _OPENMP
#pragma omp parallel for schedule(static)
#endif
 for (int x = 0; x < resolution_; x++) {
for (int y = 0; y < resolution_; y++) {
int idx_shift = x * resolution_ * resolution_ + y * resolution_;
float *p_tsdf = (float *)tsdf_.data() + idx_shift;
float *p_weight = (float *)weight_.data() + idx_shift;
float *p_color = (float *)color_.data() + idx_shift * 3;
Eigen::Vector4f voxel_pt_camera = extrinsic_f * Eigen::Vector4f(
half_voxel_length_f + voxel_length_f * x +
(float)origin_(0),
half_voxel_length_f + voxel_length_f * y +
(float)origin_(1),
half_voxel_length_f + (float)origin_(2),
1.0f);
for (int z = 0; z < resolution_; z++,
voxel_pt_camera(0) += extrinsic_scaled_f(0, 2),
voxel_pt_camera(1) += extrinsic_scaled_f(1, 2),
voxel_pt_camera(2) += extrinsic_scaled_f(2, 2),
p_tsdf++, p_weight++, p_color += 3) {
if (voxel_pt_camera(2) > 0) {
float u_f = voxel_pt_camera(0) * fx /
voxel_pt_camera(2) + cx + 0.5f;
float v_f = voxel_pt_camera(1) * fy /
voxel_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) {
int u = (int)u_f;
int v = (int)v_f;
float d = *PointerAt<float>(image.depth_, u, v);
if (d > 0.0f) {
float sdf = (d - voxel_pt_camera(2)) * (
*PointerAt<float>(
depth_to_camera_distance_multiplier,
u, v));
if (sdf > -sdf_trunc_f) {
// integrate
 float tsdf = std::min(1.0f,
sdf * sdf_trunc_inv_f);
*p_tsdf = ((*p_tsdf) * (*p_weight) + tsdf) /
(*p_weight + 1.0f);
if (color_type_ ==
TSDFVolumeColorType::RGB8) {
const uint8_t *rgb = PointerAt<uint8_t>(
image.color_, u, v, 0);
p_color[0] = (p_color[0] *
(*p_weight) + rgb[0]) /
(*p_weight + 1.0f);
p_color[1] = (p_color[1] *
(*p_weight) + rgb[1]) /
(*p_weight + 1.0f);
p_color[2] = (p_color[2] *
(*p_weight) + rgb[2]) /
(*p_weight + 1.0f);
} else if (color_type_ ==
TSDFVolumeColorType::Gray32) {
const float *intensity = PointerAt<float>(
image.color_, u, v, 0);
// PrintError("intensity : %f
", *intensity);
 p_color[0] = (p_color[0] *
(*p_weight) + *intensity) /
(*p_weight + 1.0f);
p_color[1] = (p_color[1] *
(*p_weight) + *intensity) /
(*p_weight + 1.0f);
p_color[2] = (p_color[2] *
(*p_weight) + *intensity) /
(*p_weight + 1.0f);
}
*p_weight += 1.0f;
}
}
}
}
}
}
}
}

(取自Open3DsrcCoreIntegrationUniformTSDFVolume.cpp)

體素的小立方體（16*16*16）

1.將空間坐標系下的點雲坐標(origin_)及每次迭代後的新點坐標)轉換到相機坐標系下，得到(x,y,z);

2.然後再將該相機坐標系下的點轉換到像平面坐標系下，得到該點像素坐標，映射到深度圖上獲取該點對應的深度值d，即該點對應的真實測量到的表面深度;

3. SDF為沿相機光心投影，該點到真實表面的差值。

sdf=(d?z)?√(((?????_??)/??_?? )^2+((?????_??)/??_?? )^2+1) =(d?z)?√(??^2+??^2+??^2 )/??

4.判斷 sdf>-mu ，滿足則 tsdf=min(1,sdf/mu) ,將距離轉換成[-1,1]內的函數值;

5.在每個體素內都有一個隱式函數F，通過tsdf和權重來不斷擬合。當所有點雲附近的所有體素都完成擬合後，找到所有F為0的體素點，這些點雲即可表示場景模型的表面點雲。

??_??=(??_{???1}???_{???1}+????????)/(??_{???1}+1)

[1] Curless B, Levoy M. A Volumetric Method for Building Complex Models from Range Images[C].Proceedings of the 23rd annual conference on Computer graphics and interactive techniques,ACM,1996:303-312

[2] 杜海洋,基於Kinect的三維重建與動作交互技術研究[D].西安科技大學，2015


推薦閱讀：
相关文章