1. プロジェクト紹介
主なプロセス:深度画像内の各ボクセルの TSDF 値を計算し、前のボクセルの TSDF 値に基づいて後者の TSDF 値 (加重平均) を更新し、最後にすべてのボクセルの結果を取得して、3D モデルにつなぎ合わせます。
TSDF は非常に大きなメモリ空間を必要とし、GPU は
2KB単一のボクセルのほぼすべての情報を保存する必要があります。したがって、只适用于小场景下的三维重建（如室内环境）。
(1) プロジェクトで指定した x と y のサイズは [3, 2]、単位はメートル (m)
(2) プロジェクトで指定したボクセルのサイズは 0.02、単位はメートル (m) )
(3) 総メモリ: (3 x 2 x 1) / (0.02 x 0.02 x0.02) * 2KB = (750000) * 2KB = 1464.844MB =1.43GB
注: 1 KB = 1024 B (バイト)、1 MB = 1024 KB、1 GB = 1024 MB、1 TB = 1024 GB
GPUとCPUをサポート
Fusion.pyファイルのクラスではTSDFVolumeGPUを使用するかどうかをパラメータで選択しておりuse_gpu=False、両者の速度は大きく異なります。False の場合は CPU を選択し、True の場合は CUDA と PyCUDA をインストールする必要があります。- TSDF マップを CPU ストレージから GPU に移動するには、(1)
cuda.mem_alloc事前に GPU 内のメモリ空間を開く呼び出し、(2)cuda.memcpy_htod開いたメモリ空間を TSDF マップ データの保存に使用する呼び出しの 2 つの手順が必要です。
備考: htod はホストからデバイスへ、つまりデータが CPU から GPU に保存されることを意味します。逆の場合は、cuda.memcpy_dtoh を使用します。- 最終出力は
mesh.plyファイルであり、このファイルを開くツールは数多くあります。付属のソフトウェアについては、ここで説明しますMeshLab。
2. アルゴリズム原理
2.1. 各ボクセルには 2 つの値があります: TSDF 値 (再構成サーフェスの生成に使用)、RGB グレー値 (再構成サーフェスにカラー テクスチャを付加)
像素(pixel)2D 画像の最小単位体素(voxel)ですが体素体小さな立方体で構成される 3D モデルである 3D 立方体の最小単位です。
各ボクセルには 2 つの値があります。
TSDF值（用于生成重建表面）、RGB灰度值（给重建表面贴上彩色纹理）
注: 3D 再構成の初期化段階では、直方体内のすべてのボクセルの TSDF 値は 1 で埋められ、RGB 値は 0 で埋められます。
ここで、TSDF の値の範囲は です
[-1, 1]。これは、ボクセルと最も近いオブジェクト表面の間の距離を表します。
- 1: ボクセル x を表しますカメラと物体の表面の間。
- -1: ボクセルを表します表面の裏側。
- 0になる傾向がある: つまりボクセル表面で。
2.2、TSDFアルゴリズム
打ち切り (T) に基づく符号付き距離関数（Truncated Signed Distance Function，TSDF）
- これは、等電位面 (オブジェクトの表面) を計算するための一般的な 3D 再構成方法です。
- SDF は 2003 年に Sosher によって提案され、
TSDFにSDF基づいて提案されました截断距离(T)。
例: ボクセルの SDF 値が 30 より大きい場合は、値 30 を割り当てます。15 未満の場合は、値 15 を割り当てます。最後に、すべてのボクセルに対して取得された SDF 値は [15, 30] の範囲内、つまり切り捨てられます。
ボクセルの SDF 値。最も近いオブジェクト サーフェスまでの距離を表します。T は切り捨てを意味し、定数値に近すぎる、または遠すぎる SDF 値を設定します。
- 小さな白い四角形: TSDF マップ内のボクセルを表します。
- 青い三角形: カメラの視野を示します。
- 緑色の断面線: オブジェクトの断面を示します。
- 緑色の点線: オブジェクト断面の深度情報。
- 濃い青色の直線: カメラの光学中心と、オブジェクトの断面との交点 P を持つボクセル X に沿って直線を作成します。この交点は、平面上でボクセル X に最も近い点です。Xiaobai の科学研究ノート: TSDF に基づく 3D 再構築をゼロから学ぶ
ステップ 1: ボクセル ボディの確立
実際の撮影環境と再構成する点群の分布に応じて、すべての 3D 画像の 3D 点が直方体内に収まるように十分な大きさの直方体を構築します (再構成するオブジェクトを完全に囲むことができます) X Y Z = [L x W x H]。プロジェクトで指定された x と y のサイズは [3, 2]、単位はメートル (m) です。
z方向をカメラの撮影位置とすると、 とxのy方向の極値が画像の境界となります。
- (1) 4 つの境界は 4 つの交点 (境界点) を取得します。
(0, 0), (W, 0), (0, H), (W, H)- (2) z 方向の深さ範囲:
0 ~ L- 最後に、直方体の 8 つの頂点 (境界点) が取得されます
(0, 0, 0)、(W, 0, 0)、(W, H, 0)、(0, H, 0)、(0, 0, L)、(W, 0, L)、(W, H, L)、(0, H, L)。
ステップ 2: グリッドの分割 (ボクセル化)
直方体の内部空間を小さな立方体 (ボクセル) に分割し、ユーザーはボクセルのサイズをカスタマイズできます。その中には、ボクセルが小さいほど、最終モデルが構築されるボクセルが多くなり、モデリングの精度が高くなりますが、実行速度が遅くなります。プロジェクトで指定されたボクセル サイズは 0.02、単位はメートル (m) です。
理解例：
[-1, 1]その範囲内で、指定したボクセルサイズが0.02mであれば、50ボクセルを分割できます。最後に、各ボクセルの 8 つの頂点座標を(-1+0.02x, -1+0.02y, -1+0.02*z)世界座標から計算できます。
ステップ 3: 反復更新: TSDF 値 + 重み値
投影されたワールド座標系のボクセル ボリュームは、逆変換によってカメラ座標系に変換され、画像座標系に投影されます。
- (1) 現在のフレーム画像の TSDF 値と重みを計算します。。この時点で、すべてのボクセルを走査する必要があります。各ボクセルの TSDF 値を計算し、各ボクセルの TSDF 値を切り捨て、各ボクセルの TSDF 値を更新し、各ボクセルの重みを計算します。
- (2) 現在のフレーム画像をグローバル フュージョン結果とフュージョンします。。
備考: 現在のフレームが最初のフレームである場合、最初のフレームは融合結果になります。そうでない場合は、現在のフレームを前の融合結果と融合する必要があります。
(1) 導出例: 世界座標系の任意のボクセルを
三维坐标点p例に挙げます。ワールド座標は、ワールド座標 -> カメラ座標 -> ピクセル座標から最終的に生成された 3D モデル (ボクセル ボディ) です。
- 座標変換。深度画像のカメラ姿勢行列から、カメラ座標系の世界座標系の点 P を求め、カメラ内部参照行列と
映射点v逆写像点 Vから深度画像内の対応関係を求めます。像素点x- 座標点 p の tsdf 値を計算します。
- このときの座標点pのsdf値は、 となります
sdf( p ) = value( x ) - distance( v )。このうち、ピクセル点 x の深度値は value( x )、点 v からカメラ座標の原点までの距離は distance( v ) です。- それから
引入截断距离计算tsdf( p )。
判定１：在截断距离 U = [-1, 1] 以内：tsdf( p ) = sdf( p ) / | U |；
判定２：如果sdf( p ) > 0，tsdf( p ) = 1；
判定３：若果sdf( p ) < 0，tsdf( p ) = -1；- 座標点 p の重みを計算します: w( p ) = cos(θ) / distance( v )。このうち、θは投影光線と表面法線ベクトルとのなす角度である。
- その後、深度画像のフレームが追加されるたびに、上記のステップが実行されます。最後に、結果は Marching Cube に出力され、オブジェクトの表面が計算されます。TSDF アルゴリズムの学習
(2) 現在のフレーム tsdf( p ) を通じて融合 TSDF 値を更新します。。
注１：つまり、マルチフレーム画像には重複部分が多く、複数の重複部分を融合する必要がある。つまり、マルチフレーム画像上のボクセルに対応するTSDF値と重みが個別に計算され、最後に加重平均がとられます。
注2:このアップデートモードはインクリメンタルアップデートを採用しています。つまり、2 番目のフレームの結果が最初のフレームと重み付けされて平均され、3 番目のフレームと 2 番目のフレームの重み付けされた平均が実行されます。
注 3: 3D モデルはリアルタイムで更新されます。つまり、すべての深度マップが計算されて結果がまとめてリリースされるのを待つのではなく、フレームが計算されるたびに結果が更新されます。
式は次のとおりです: TSDF アルゴリズムの原理とソース コード分析
その中で:
TSDF( p ) は融合後のボクセル p の TSDF 値を表します;
W( p ) は融合後のボクセル p の重み値を表します;
tsdf( p ) はボクセル p の現在のフレームの TSDF 値、
w( p ) はボクセル p の現在のフレームの重み値を表します。
ステップ 4: 等値面を見つける
マーチング キューブ アルゴリズムを使用して、TSDF グリッドdist加权和为0的等值面、つまりオブジェクトの表面を検索します。
- 太字の赤い曲線: オブジェクトの表面 (人の顔)
- オブジェクトの内部
- オレンジ色の数字: 負の値。オブジェクトの表面から離れるほど、値は大きくなります。
- 赤い数字: 負の値。切り詰める。
- オブジェクトの外側
- 紫色の数字: 正の値。オブジェクトの表面から離れるほど、値は大きくなります。
- 濃い青色の数字: 正の値。切り詰める。
3. プロジェクトの説明
3.1、ソースコードのダウンロード (Github)
Github公式ウェブサイトのダウンロード: https://github.com/andyzeng/tsdf-fusion-python
3.2. データセットの説明
データ セットはRGB-D データセット 7 シーンから取得されます。つまり、7 シーン データ セットの 1000 個の画像がRGB深度图像解像度 2cm の TSDF ボクセル ボリューム (3D サーフェス グリッドと点群) に融合されます。
| データセット (データ) パラメーター | 説明する | データ形式 | 例 |
|---|---|---|---|
| カメラリファレンス | カメラの内部パラメータ（ハードウェア） | txt マトリックス形式 | camera-intrinsics.txt |
| RGB画像 | シーンの複数の視点から撮影された画像 | frame-000000.color.jpg |
|
| 深度マップ | 物体の距離を表す画像 | frame-000000.depth.png |
|
| カメラポーズ | 画像はマルチビューなので、各ビューのカメラポーズも異なります | txt マトリックス形式 | frame-000000.pose.txt |
注1: ファイルは1つであり相机内参、1000枚の画像データが1つの に相当しますRGB图像、深度图、相机位姿。
注2: カメラ固有パラメータとカメラポーズは主に座標系変換に使用されます。
注 3: 深度マップは、高度なカメラを通じて取得するか、RGB 画像を通じて深度推定を行うことができます。
3.3. 文書の説明
プロジェクト全体は、2 つのフォルダー、2 つの (.py) ファイル、2 つの (.ply) ファイルの 3 つの部分に分かれています。
フォルダ
data: データ セットを保存する
images: 3D 再構成プロセス マップを保存する (fusion-movie.gif)
書類
demo.py：メイン関数ファイル
fusion.py：定義済みファイル
README.md：公式サイトのプロジェクトプロフィール
生成された結果ファイル
mesh.ply: はポリゴンメッシュファイル形式。
- 複数の三角形または四角形のパッチが含まれており、各パッチは複数の頂点で構成されます。各頂点の色、法線、テクスチャ座標などの情報も含まれる場合があります。
- コンピュータ グラフィックス、3D モデリング、ビジュアライゼーションなどの分野でよく使用されます。
pc.ply: は点群ファイル形式。
- これにはいくつかの点の座標情報のみが含まれており、各点は他の属性 (色、法線、強度など) を持つ場合があります。
- 3D スキャン、LIDAR、リモート センシング、医療画像処理などの分野でよく使用されます。点群データは、3D モデルの構築、形状や構造の分析、欠陥や異常の検出などに使用できます。
同じ点: どちらも 3D シーンまたはオブジェクトを記述する点群ファイル形式です。
相違点:mesh.plyにもっと注意を払い表面的细节和形状、 にpc.plyもっと点的位置和属性。
4. 環境構築+ツールのインストール
4.0、ImportError: _arpack のインポート中に DLL のロードに失敗しました: 指定されたプログラムが見つかりません。
原因分析: 依存関係の欠落、scipy パッケージの欠落、誤って設定された環境変数、および互換性のない Python バージョン。
(1) インストール依存関係 (NumPy と SciPy)（
2）scipyパッケージがありません。アンインストールしてpip uninstall scipy、最新バージョンをインストールしますpip install scipy。注: scipy の下位バージョンをインストールしようとしましたが、まだ問題があります。
(2) チェック環境変数。特に、PATH 環境変数に、Python および関連する依存関係がインストールされているパスが含まれていることを確認してください。
(3) アップグレードPythonのバージョン。古いバージョンでは互換性の問題が発生する場合があります。
情報を調べてみると、上記の4つの原因がまとめられていますが、最終的にブロガーの問題の原因は「scipyパッケージが見つからない」という理由にあり、上記の方法で解決できます。
4.1. 環境設定
プロジェクトは Numpy + opencv + pycuda + numba + skimage に基づいて完了できます。
4.1.1、Anaconda + Pycharm + OpenCV
【ディープラーニング環境構成】 Anaconda + Pycharm + CUDA +cuDNN + Pytorch + Opencv (リソースはアップロード済み)
4.1.2. pycuda のインストール
pycuda をインストールします
pip install pycuda。通常、インストールは失敗しますが、pycuda.whl ダウンロード アドレスホイールを使用してインストールできます。詳細なチュートリアルは次のとおりです: [GPU アクセラレーション] pycuda インストール例外: pycuda のビルドに失敗しました エラー: pycuda のホイールをビルドできませんでした
4.1.3. numbaのインストール
バグのヒント: ImportError: Numba には NumPy 1.22 以下が必要です。
numba: をインストールしてくださいpip install numba。この方法では、numba のバージョンが自動的に検出され、numpy のバージョンが対応していない場合は、現在の numpy を自動的にアンインストールし、対応するバージョンを再インストールします。覚えておいてください: llvmlite フォルダーは削除しないでください。削除すると、プロジェクトの実行中にそのようなモジュールが存在しないというメッセージが表示されます。エラー: 「llvmlite」をアンインストールできません。これは distutils がインストールされたプロジェクトです。
4.1.4. scikit-image のインストール (正常に実行)
ステップ 1: 上記の環境を構成すると、次の図に示すように、正常に実行できるようになります。
- (1) Fusion.py ファイルの TSDFVolume クラスで、パラメーター use_gpu=False を通じて GPU を使用するかどうかを選択します。
- (2) Demo.py ファイルで、「実行」をクリックして実行します。
ステップ 2: 結果ファイルを生成するときに、次の例外が発生します。
バグのヒント: AttributeError: module 'skimage.measure' has noattribute 'marching_cubes_lewiner'
エラーの理由: scikit-image (skimage)の導入バージョンの問題が原因で発生します。scikit-image の古いバージョンでは、Marching_cubes_lewiner 関数は skimage.measure モジュールで定義されていましたが、新しいバージョンでは別のモジュールに移動されました。
解決：
- scikit-image の古いバージョンの場合は、バージョンをアップグレードしてこの問題を解決してください。
- 11.
pip listインストールされている scikit-image のバージョン番号が 0.19.2 であることを確認します。- 22.
pip uninstall scikit-imageアンインストールします。- 33. scikit-image をインストールします
pip install scikit-image。最新バージョンがデフォルトでインストールされます。上記のバグ プロンプトのリンクに記載されている方法は、0.19.2 バージョン (python3.9) を 0.16.2 バージョン (python3.8) に置き換えて問題を解決するものであるため、 scikitを通じてインストールする必要があります。image.whl ダウンロード アドレス。ただし、この方法ではPythonのバージョンも同時に切り替える必要があり、私はpython3.9を使っているのですが、公式サイトに対応するscikit-imageのバージョンが見つからなかったので、いくつかのバージョンを試しましたが失敗しました。- 新しいバージョンの scikit-image の場合、間違った名前でインポートされた可能性があります。次のコードから入手できます:Marching_cubes_lewiner:
from skimage import measure
verts, faces, _, _ = measure.marching_cubes_lewiner(volume, level)最後に、新しいバージョンの scikit-image に基づいて、プロジェクトは正常に実行され、mesh.ply + pc.ply ファイルが生成されます。
ステップ 3: 最初の 2 つのステップでは結果ファイルを正常に生成できましたが、システムには 2 つの警告が表示されます。これは使用には影響しませんが、問題を解決する必要があるだけです将measure.marching_cubes_lewiner 更换为 measure.marching_cubes。プロジェクトは正常に実行され、mesh.ply + pc.ply ファイルが生成されます。
4.2、MeshLab のインストール (3D モデル Mesh.ply を表示)
3D ジオメトリ処理システム (MeshLab): 概要 + インストール + チュートリアル
- MeshLab は、オープンソースの 3D 三角メッシュ編集および処理システムで、編集、クリーニング、修復、チェック、レンダリング、テクスチャおよびメッシュ変換を含む 3D メッシュの包括的な編集および処理を実行できます。
- さらに、3D デジタルツール/機器で生成された生データを処理する機能も備えており、3D モデル印刷機能を提供することで、ユーザーの工業化されたモデル作成を実現できます。
- MeshLab 公式 Web サイトのダウンロード アドレス: デフォルトの最新バージョン (複数のプラットフォームをサポート)
- Github公式サイトダウンロードアドレス：累積リリース版（マルチプラットフォーム対応）
5. コードの詳細説明
TSDF アルゴリズムの原理、導出プロセス、ソース コード分析: def __ init __()、def integrate()、def get_mesh(self)
注: CPU バージョンのソース コードを解析することによってのみ、TSDF の原理を深く理解することができます。GPU バージョンの主な内容: ボクセルがどのようにインデックス付けされ、マルチスレッドによって抽出されるか。
5.1、demo.py: メイン関数ファイル
"""
Fuse 1000 RGB-D images from the 7-scenes dataset into a TSDF voxel volume with 2cm resolution.
"""
import time
import cv2
import numpy as np
import fusion
if __name__ == "__main__":
# ======================================================================================================== #
# （1）估计体素体边界
# ======================================================================================================== #
print("Estimating voxel volume bounds...")
n_imgs = 1000 # 1.1、指定数据集中的RGB图像总个数
cam_intr = np.loadtxt("data/camera-intrinsics.txt", delimiter=' ') # 1.2、读取相机内参
vol_bnds = np.zeros((3, 2)) # 1.3、以米为单位指定xyz边界(min/max)。
for i in range(n_imgs):
depth_im = cv2.imread("data/frame-%06d.depth.png" % i, -1).astype(float) # 1.4、读取深度图像
depth_im /= 1000. # 单位为毫米。图像深度（depth）保存为16位PNG格式。
depth_im[depth_im == 65.535] = 0 # 将无效的图像深度设置为0(特定于7场景数据集)
cam_pose = np.loadtxt("data/frame-%06d.pose.txt" % i) # 1.5、读取相机位姿: 4x4刚性变换矩阵
view_frust_pts = fusion.get_view_frustum(depth_im, cam_intr, cam_pose) # 1.6、计算相机的视锥体和扩展凸包
vol_bnds[:, 0] = np.minimum(vol_bnds[:, 0], np.amin(view_frust_pts, axis=1))
vol_bnds[:, 1] = np.maximum(vol_bnds[:, 1], np.amax(view_frust_pts, axis=1))
# 视锥体是摄像机可见的空间，看上去像截掉顶部的金字塔。
# ======================================================================================================== #
# （2）RGB-D图像的TSDF体积融合
# ======================================================================================================== #
print("Initializing voxel volume...")
########## 函数: fusion.TSDFVolume ##########
tsdf_vol = fusion.TSDFVolume(vol_bnds, voxel_size=0.02) # 初始化体素大小=0.02m（即2cm）
# ======================================================================================================== #
# （3）循环RGB-D图像更新每个体素的TSDF值，并将它们融合在一起。
# ======================================================================================================== #
t0_elapse = time.time()
for i in range(n_imgs):
print("Fusing frame %d/%d" % (i+1, n_imgs))
color_image = cv2.cvtColor(cv2.imread("data/frame-%06d.color.jpg" % i), cv2.COLOR_BGR2RGB) # 读取彩色图像
depth_im = cv2.imread("data/frame-%06d.depth.png" % i, -1).astype(float) # 读取深度图像
depth_im /= 1000.
depth_im[depth_im == 65.535] = 0
cam_pose = np.loadtxt("data/frame-%06d.pose.txt" % i) # 读取相机位姿
########## 函数: fusion.integrate ##########
tsdf_vol.integrate(color_image, depth_im, cam_intr, cam_pose, obs_weight=1.) # 将观测结果整合到体素体中(假设颜色与深度对齐)
# ======================================================================================================== #
# （4）打印FPS，并输出.ply文件
# ======================================================================================================== #
# 4.1、打印平均FPS:（表示画面每秒传输帧数）
fps = n_imgs / (time.time() - t0_elapse)
print("Average FPS: {:.2f}".format(fps))
# 4.2、从体素体中获取3D网格，并保存为多边形.ply文件到磁盘（可以使用Meshlab查看）
print("Saving mesh to mesh.ply...")
verts, faces, norms, colors = tsdf_vol.get_mesh() # 使用marching cubes体素级重建方法计算网格
########## 函数: fusion.meshwrite ##########
fusion.meshwrite("mesh.ply", verts, faces, norms, colors)
# 4.3、从体素体积中获取点云，并保存为多边形.ply文件到磁盘（可以使用Meshlab查看）
print("Saving point cloud to pc.ply...")
point_cloud = tsdf_vol.get_point_cloud() # 从体素体中提取点云
########## 函数: fusion.pcwrite ##########
fusion.pcwrite("pc.ply", point_cloud)
5.2、fusion.py: 事前定義されたファイル
# Copyright (c) 2018 Andy Zeng
import numpy as np
from numba import njit, prange
from skimage import measure
# 默认使用GPU，若不使用需更改为: try 0; 否则需要安装pycuda.
try:
import pycuda.driver as cuda
import pycuda.autoinit
from pycuda.compiler import SourceModule
FUSION_GPU_MODE = 1
except Exception as err:
print('Warning: {}'.format(err))
print('Failed to import PyCUDA. Running fusion in CPU mode.')
FUSION_GPU_MODE = 0
class TSDFVolume:
"""
Volumetric TSDF Fusion of RGB-D Images.（RGB-D图像的TSDF体积融合）
"""
def __init__(self, vol_bnds, voxel_size, use_gpu=False):
"""构造函数
Args:
vol_bnds (ndarray): 以米为单位指定XYZ边界(min/max)。形状为(3,2)的数组（固定值）。
voxel_size (float): 以米为单位的体素大小（可自定义）。备注：体素越小，最终构成模型的体素个数越多，但运行速度越慢。
use_gpu=False: 是否使用GPU。若使用，需安装pycuda.
"""
# （1）将点云分布边界转换成numpy数组
vol_bnds = np.asarray(vol_bnds) # 数据类型转换: array转asarray. 区别在于是否共享内存
assert vol_bnds.shape == (3, 2), "[!] `vol_bnds` should be of shape (3, 2)."
# （2）定义体素体参数
self._vol_bnds = vol_bnds # 体素体xyz边界
self._voxel_size = float(voxel_size) # 体素体每个立方体边长
self._trunc_margin = 5 * self._voxel_size # 截断距离，设置为体素边长的5倍
self._color_const = 256 * 256 # 辅助参数，用于提取rgb值
# （3）调整体积边界以及索引顺序（order='C': 表示最后一个索引变化最快）
self._vol_dim = np.ceil((self._vol_bnds[:, 1]-self._vol_bnds[:, 0])/self._voxel_size).copy(order='C').astype(int)
self._vol_bnds[:, 1] = self._vol_bnds[:, 0]+self._vol_dim*self._voxel_size
self._vol_origin = self._vol_bnds[:, 0].copy(order='C').astype(np.float32)
# （4）打印TSDF地图的尺寸: [L, W, H]
print("Voxel volume size: {} x {} x {} - # points: {:,}" .format(self._vol_dim[0], self._vol_dim[1], self._vol_dim[2],
self._vol_dim[0]*self._vol_dim[1]*self._vol_dim[2]))
# 初始化保存体素体信息的容器
self._tsdf_vol_cpu = np.ones(self._vol_dim).astype(np.float32) # 用于保存每个体素栅格的tsdf值
self._weight_vol_cpu = np.zeros(self._vol_dim).astype(np.float32) # 用于保存每个体素栅格的权重值
self._color_vol_cpu = np.zeros(self._vol_dim).astype(np.float32) # 用于保存每个体素栅格的颜色值(将rgb三个值压缩成一个float32值表示)
self.gpu_mode = use_gpu and FUSION_GPU_MODE
# 将体素体积复制到GPU
if self.gpu_mode:
self._tsdf_vol_gpu = cuda.mem_alloc(self._tsdf_vol_cpu.nbytes)
cuda.memcpy_htod(self._tsdf_vol_gpu, self._tsdf_vol_cpu)
self._weight_vol_gpu = cuda.mem_alloc(self._weight_vol_cpu.nbytes)
cuda.memcpy_htod(self._weight_vol_gpu, self._weight_vol_cpu)
self._color_vol_gpu = cuda.mem_alloc(self._color_vol_cpu.nbytes)
cuda.memcpy_htod(self._color_vol_gpu, self._color_vol_cpu)
# Cuda内核函数(c++) 功能: 在GPU下的体素是如何通过多线程去索引并提取
self._cuda_src_mod = SourceModule("""
__global__ void integrate(float * tsdf_vol,
float * weight_vol,
float * color_vol,
float * vol_dim,
float * vol_origin,
float * cam_intr,
float * cam_pose,
float * other_params,
float * color_im,
float * depth_im) {
// Get voxel index
int gpu_loop_idx = (int) other_params[0];
int max_threads_per_block = blockDim.x;
int block_idx = blockIdx.z*gridDim.y*gridDim.x+blockIdx.y*gridDim.x+blockIdx.x;
int voxel_idx = gpu_loop_idx*gridDim.x*gridDim.y*gridDim.z*max_threads_per_block
+block_idx*max_threads_per_block+threadIdx.x;
int vol_dim_x = (int) vol_dim[0];
int vol_dim_y = (int) vol_dim[1];
int vol_dim_z = (int) vol_dim[2];
if (voxel_idx > vol_dim_x*vol_dim_y*vol_dim_z)
return;
// Get voxel grid coordinates (note: be careful when casting)
float voxel_x = floorf(((float)voxel_idx)/((float)(vol_dim_y*vol_dim_z)));
float voxel_y = floorf(((float)(voxel_idx-((int)voxel_x)*vol_dim_y*vol_dim_z))/((float)vol_dim_z));
float voxel_z = (float)(voxel_idx-((int)voxel_x)*vol_dim_y*vol_dim_z-((int)voxel_y)*vol_dim_z);
// Voxel grid coordinates to world coordinates
float voxel_size = other_params[1];
float pt_x = vol_origin[0]+voxel_x*voxel_size;
float pt_y = vol_origin[1]+voxel_y*voxel_size;
float pt_z = vol_origin[2]+voxel_z*voxel_size;
// World coordinates to camera coordinates
float tmp_pt_x = pt_x-cam_pose[0*4+3];
float tmp_pt_y = pt_y-cam_pose[1*4+3];
float tmp_pt_z = pt_z-cam_pose[2*4+3];
float cam_pt_x = cam_pose[0*4+0]*tmp_pt_x+cam_pose[1*4+0]*tmp_pt_y+cam_pose[2*4+0]*tmp_pt_z;
float cam_pt_y = cam_pose[0*4+1]*tmp_pt_x+cam_pose[1*4+1]*tmp_pt_y+cam_pose[2*4+1]*tmp_pt_z;
float cam_pt_z = cam_pose[0*4+2]*tmp_pt_x+cam_pose[1*4+2]*tmp_pt_y+cam_pose[2*4+2]*tmp_pt_z;
// Camera coordinates to image pixels
int pixel_x = (int) roundf(cam_intr[0*3+0]*(cam_pt_x/cam_pt_z)+cam_intr[0*3+2]);
int pixel_y = (int) roundf(cam_intr[1*3+1]*(cam_pt_y/cam_pt_z)+cam_intr[1*3+2]);
// Skip if outside view frustum
int im_h = (int) other_params[2];
int im_w = (int) other_params[3];
if (pixel_x < 0 || pixel_x >= im_w || pixel_y < 0 || pixel_y >= im_h || cam_pt_z<0)
return;
// Skip invalid depth
float depth_value = depth_im[pixel_y*im_w+pixel_x];
if (depth_value == 0)
return;
// Integrate TSDF
float trunc_margin = other_params[4];
float depth_diff = depth_value-cam_pt_z;
if (depth_diff < -trunc_margin)
return;
float dist = fmin(1.0f,depth_diff/trunc_margin);
float w_old = weight_vol[voxel_idx];
float obs_weight = other_params[5];
float w_new = w_old + obs_weight;
weight_vol[voxel_idx] = w_new;
tsdf_vol[voxel_idx] = (tsdf_vol[voxel_idx]*w_old+obs_weight*dist)/w_new;
// Integrate color
float old_color = color_vol[voxel_idx];
float old_b = floorf(old_color/(256*256));
float old_g = floorf((old_color-old_b*256*256)/256);
float old_r = old_color-old_b*256*256-old_g*256;
float new_color = color_im[pixel_y*im_w+pixel_x];
float new_b = floorf(new_color/(256*256));
float new_g = floorf((new_color-new_b*256*256)/256);
float new_r = new_color-new_b*256*256-new_g*256;
new_b = fmin(roundf((old_b*w_old+obs_weight*new_b)/w_new),255.0f);
new_g = fmin(roundf((old_g*w_old+obs_weight*new_g)/w_new),255.0f);
new_r = fmin(roundf((old_r*w_old+obs_weight*new_r)/w_new),255.0f);
color_vol[voxel_idx] = new_b*256*256+new_g*256+new_r;
}""")
self._cuda_integrate = self._cuda_src_mod.get_function("integrate")
# 确定GPU上的块/网格大小
gpu_dev = cuda.Device(0)
self._max_gpu_threads_per_block = gpu_dev.MAX_THREADS_PER_BLOCK
n_blocks = int(np.ceil(float(np.prod(self._vol_dim))/float(self._max_gpu_threads_per_block)))
grid_dim_x = min(gpu_dev.MAX_GRID_DIM_X, int(np.floor(np.cbrt(n_blocks))))
grid_dim_y = min(gpu_dev.MAX_GRID_DIM_Y, int(np.floor(np.sqrt(n_blocks/grid_dim_x))))
grid_dim_z = min(gpu_dev.MAX_GRID_DIM_Z, int(np.ceil(float(n_blocks)/float(grid_dim_x*grid_dim_y))))
self._max_gpu_grid_dim = np.array([grid_dim_x, grid_dim_y, grid_dim_z]).astype(int)
self._n_gpu_loops = int(np.ceil(float(np.prod(self._vol_dim))/float(np.prod(self._max_gpu_grid_dim)*self._max_gpu_threads_per_block)))
else:
# 获取每个体素网格的坐标
xv, yv, zv = np.meshgrid(range(self._vol_dim[0]), range(self._vol_dim[1]), range(self._vol_dim[2]), indexing='ij')
self.vox_coords = np.concatenate([xv.reshape(1, -1), yv.reshape(1, -1), zv.reshape(1, -1)], axis=0).astype(int).T
@staticmethod
@njit(parallel=True)
def vox2world(vol_origin, vox_coords, vox_size):
"""
Convert voxel grid coordinates to world coordinates.（将体素网格坐标转换为世界坐标。）
"""
vol_origin = vol_origin.astype(np.float32)
vox_coords = vox_coords.astype(np.float32)
cam_pts = np.empty_like(vox_coords, dtype=np.float32)
for i in prange(vox_coords.shape[0]):
for j in range(3):
cam_pts[i, j] = vol_origin[j] + (vox_size * vox_coords[i, j])
return cam_pts
@staticmethod
@njit(parallel=True)
def cam2pix(cam_pts, intr):
"""
Convert camera coordinates to pixel coordinates.（将相机坐标转换为像素坐标。）
"""
intr = intr.astype(np.float32)
fx, fy = intr[0, 0], intr[1, 1]
cx, cy = intr[0, 2], intr[1, 2]
pix = np.empty((cam_pts.shape[0], 2), dtype=np.int64)
for i in prange(cam_pts.shape[0]):
pix[i, 0] = int(np.round((cam_pts[i, 0] * fx / cam_pts[i, 2]) + cx))
pix[i, 1] = int(np.round((cam_pts[i, 1] * fy / cam_pts[i, 2]) + cy))
return pix
@staticmethod
@njit(parallel=True)
def integrate_tsdf(tsdf_vol, dist, w_old, obs_weight):
"""
Integrate the TSDF volume.
"""
tsdf_vol_int = np.empty_like(tsdf_vol, dtype=np.float32)
w_new = np.empty_like(w_old, dtype=np.float32)
for i in prange(len(tsdf_vol)):
w_new[i] = w_old[i] + obs_weight
tsdf_vol_int[i] = (w_old[i] * tsdf_vol[i] + obs_weight * dist[i]) / w_new[i]
return tsdf_vol_int, w_new
def integrate(self, color_im, depth_im, cam_intr, cam_pose, obs_weight=1.):
"""Integrate an RGB-D frame into the TSDF volume.
Args:
color_im (ndarray): An RGB image of shape (H, W, 3).
depth_im (ndarray): A depth image of shape (H, W).
cam_intr (ndarray): The camera intrinsics matrix of shape (3, 3).
cam_pose (ndarray): The camera pose (i.e. extrinsics) of shape (4, 4).
obs_weight (float): The weight to assign for the current observation. A higher value
"""
im_h, im_w = depth_im.shape # 获取图像尺寸
# 将RGB彩色图像折叠成单通道图像（将rgb三个值表示的颜色通道信息转换成一个用float32表示的单通道信息）
color_im = color_im.astype(np.float32)
color_im = np.floor(color_im[..., 2]*self._color_const + color_im[..., 1]*256 + color_im[..., 0])
# 【GPU mode】: 集成体素体积(调用CUDA内核)
if self.gpu_mode:
for gpu_loop_idx in range(self._n_gpu_loops):
self._cuda_integrate(self._tsdf_vol_gpu,
self._weight_vol_gpu,
self._color_vol_gpu,
cuda.InOut(self._vol_dim.astype(np.float32)),
cuda.InOut(self._vol_origin.astype(np.float32)),
cuda.InOut(cam_intr.reshape(-1).astype(np.float32)),
cuda.InOut(cam_pose.reshape(-1).astype(np.float32)),
cuda.InOut(np.asarray([gpu_loop_idx, self._voxel_size, im_h, im_w, self._trunc_margin, obs_weight], np.float32)),
cuda.InOut(color_im.reshape(-1).astype(np.float32)),
cuda.InOut(depth_im.reshape(-1).astype(np.float32)),
block=(self._max_gpu_threads_per_block, 1, 1),
grid=(int(self._max_gpu_grid_dim[0]), int(self._max_gpu_grid_dim[1]), int(self._max_gpu_grid_dim[2]), )
)
# 【CPU mode】: 整合体素体积(矢量化实现)
else:
# 将体素网格坐标转换为像素坐标
cam_pts = self.vox2world(self._vol_origin, self.vox_coords, self._voxel_size) # 体素坐标系转换到世界坐标系
cam_pts = rigid_transform(cam_pts, np.linalg.inv(cam_pose)) # 世界坐标系转换到相机坐标系
pix_z = cam_pts[:, 2]
pix = self.cam2pix(cam_pts, cam_intr) # 相机坐标系转换到像素坐标系
pix_x, pix_y = pix[:, 0], pix[:, 1]
# 消除视界外的像素（移除像素边界之外的投影点）
valid_pix = np.logical_and(pix_x >= 0, np.logical_and(pix_x < im_w, np.logical_and(pix_y >= 0, np.logical_and(pix_y < im_h, pix_z > 0))))
depth_val = np.zeros(pix_x.shape)
depth_val[valid_pix] = depth_im[pix_y[valid_pix], pix_x[valid_pix]] # 获取体素(x,y)在深度图像中的值
# 更新每个体素网格的tsdf值及对应的权重
depth_diff = depth_val - pix_z # 计算SDF值
valid_pts = np.logical_and(depth_val > 0, depth_diff >= -self._trunc_margin) # 确定出有效深度值(即sdf值的值要大于负的截断值)
dist = np.minimum(1, depth_diff / self._trunc_margin) # 计算截断值
valid_vox_x = self.vox_coords[valid_pts, 0]
valid_vox_y = self.vox_coords[valid_pts, 1]
valid_vox_z = self.vox_coords[valid_pts, 2]
w_old = self._weight_vol_cpu[valid_vox_x, valid_vox_y, valid_vox_z] # 提取上个循环对应体素的权重
tsdf_vals = self._tsdf_vol_cpu[valid_vox_x, valid_vox_y, valid_vox_z] # 提取上个循环对应体素的tsdf值
valid_dist = dist[valid_pts]
tsdf_vol_new, w_new = self.integrate_tsdf(tsdf_vals, valid_dist, w_old, obs_weight) # 计算体素新的权重和tsdf值
self._weight_vol_cpu[valid_vox_x, valid_vox_y, valid_vox_z] = w_new # 将新的权值和tsdf值更新到体素信息容器中
self._tsdf_vol_cpu[valid_vox_x, valid_vox_y, valid_vox_z] = tsdf_vol_new
# 更新每个体素网格的颜色值（按照旧权重与新权重的加权，更新每个体素栅格的rgb值）
old_color = self._color_vol_cpu[valid_vox_x, valid_vox_y, valid_vox_z]
old_b = np.floor(old_color / self._color_const)
old_g = np.floor((old_color-old_b*self._color_const)/256)
old_r = old_color - old_b*self._color_const - old_g*256
new_color = color_im[pix_y[valid_pts], pix_x[valid_pts]]
new_b = np.floor(new_color / self._color_const)
new_g = np.floor((new_color - new_b*self._color_const) / 256)
new_r = new_color - new_b*self._color_const - new_g*256
new_b = np.minimum(255., np.round((w_old*old_b + obs_weight*new_b) / w_new))
new_g = np.minimum(255., np.round((w_old*old_g + obs_weight*new_g) / w_new))
new_r = np.minimum(255., np.round((w_old*old_r + obs_weight*new_r) / w_new))
self._color_vol_cpu[valid_vox_x, valid_vox_y, valid_vox_z] = new_b*self._color_const + new_g*256 + new_r
def get_volume(self):
if self.gpu_mode:
cuda.memcpy_dtoh(self._tsdf_vol_cpu, self._tsdf_vol_gpu)
cuda.memcpy_dtoh(self._color_vol_cpu, self._color_vol_gpu)
return self._tsdf_vol_cpu, self._color_vol_cpu
def get_point_cloud(self):
"""
Extract a point cloud from the voxel volume.（从体素体中提取点云。）
"""
tsdf_vol, color_vol = self.get_volume()
# Marching cubes
verts = measure.marching_cubes(tsdf_vol, level=0)[0]
verts_ind = np.round(verts).astype(int)
verts = verts*self._voxel_size + self._vol_origin
# Get vertex colors
rgb_vals = color_vol[verts_ind[:, 0], verts_ind[:, 1], verts_ind[:, 2]]
colors_b = np.floor(rgb_vals / self._color_const)
colors_g = np.floor((rgb_vals - colors_b*self._color_const) / 256)
colors_r = rgb_vals - colors_b*self._color_const - colors_g*256
colors = np.floor(np.asarray([colors_r, colors_g, colors_b])).T
colors = colors.astype(np.uint8)
pc = np.hstack([verts, colors])
return pc
def get_mesh(self):
"""
Compute a mesh from the voxel volume using marching cubes.（使用marching cubes体素级重建方法计算网格）
"""
tsdf_vol, color_vol = self.get_volume() # 获取体素栅格的tsdf值及对应的颜色值
# 直接使用scikit-image工具包中封装的Marching cubes算法接口提取等值面
verts, faces, norms, vals = measure.marching_cubes(tsdf_vol, level=0)
verts_ind = np.round(verts).astype(int)
verts = verts*self._voxel_size+self._vol_origin # voxel grid coordinates to world coordinates
# 为每个体素赋值颜色
rgb_vals = color_vol[verts_ind[:, 0], verts_ind[:, 1], verts_ind[:, 2]]
colors_b = np.floor(rgb_vals/self._color_const)
colors_g = np.floor((rgb_vals-colors_b*self._color_const)/256)
colors_r = rgb_vals-colors_b*self._color_const-colors_g*256
colors = np.floor(np.asarray([colors_r, colors_g, colors_b])).T
colors = colors.astype(np.uint8)
return verts, faces, norms, colors
def rigid_transform(xyz, transform):
"""
世界坐标系转换到相机坐标系: Applies a rigid transform to an (N, 3) pointcloud.（对(N, 3)点云应用刚性变换。）
"""
xyz_h = np.hstack([xyz, np.ones((len(xyz), 1), dtype=np.float32)])
xyz_t_h = np.dot(transform, xyz_h.T).T
return xyz_t_h[:, :3]
def get_view_frustum(depth_im, cam_intr, cam_pose):
"""
Get corners of 3D camera view frustum of depth image.（获取三维相机视角的深度图像）
"""
im_h = depth_im.shape[0]
im_w = depth_im.shape[1]
max_depth = np.max(depth_im)
view_frust_pts = np.array([(np.array([0, 0, 0, im_w, im_w])-cam_intr[0, 2]) * np.array([0, max_depth, max_depth, max_depth, max_depth])/cam_intr[0, 0],
(np.array([0, 0, im_h, 0, im_h])-cam_intr[1, 2]) * np.array([0, max_depth, max_depth, max_depth, max_depth])/cam_intr[1, 1],
np.array([0, max_depth, max_depth, max_depth, max_depth])
])
view_frust_pts = rigid_transform(view_frust_pts.T, cam_pose).T
return view_frust_pts
def meshwrite(filename, verts, faces, norms, colors):
"""
Save a 3D mesh to a polygon .ply file.（将3D网格保存为多边形.ply文件。）
"""
# Write header
ply_file = open(filename, 'w')
ply_file.write("ply\n")
ply_file.write("format ascii 1.0\n")
ply_file.write("element vertex %d\n" % (verts.shape[0]))
ply_file.write("property float x\n")
ply_file.write("property float y\n")
ply_file.write("property float z\n")
ply_file.write("property float nx\n")
ply_file.write("property float ny\n")
ply_file.write("property float nz\n")
ply_file.write("property uchar red\n")
ply_file.write("property uchar green\n")
ply_file.write("property uchar blue\n")
ply_file.write("element face %d\n" % (faces.shape[0]))
ply_file.write("property list uchar int vertex_index\n")
ply_file.write("end_header\n")
# Write vertex list
for i in range(verts.shape[0]):
ply_file.write("%f %f %f %f %f %f %d %d %d\n" % (verts[i, 0], verts[i, 1], verts[i, 2],
norms[i, 0], norms[i, 1], norms[i, 2],
colors[i, 0], colors[i, 1], colors[i, 2],))
# Write face list
for i in range(faces.shape[0]):
ply_file.write("3 %d %d %d\n" % (faces[i, 0], faces[i, 1], faces[i, 2]))
ply_file.close()
def pcwrite(filename, xyzrgb):
"""
Save a point cloud to a polygon .ply file.（保存点云到多边形.ply文件。）
"""
xyz = xyzrgb[:, :3]
rgb = xyzrgb[:, 3:].astype(np.uint8)
# Write header
ply_file = open(filename, 'w')
ply_file.write("ply\n")
ply_file.write("format ascii 1.0\n")
ply_file.write("element vertex %d\n" % (xyz.shape[0]))
ply_file.write("property float x\n")
ply_file.write("property float y\n")
ply_file.write("property float z\n")
ply_file.write("property uchar red\n")
ply_file.write("property uchar green\n")
ply_file.write("property uchar blue\n")
ply_file.write("end_header\n")
# Write vertex list
for i in range(xyz.shape[0]):
ply_file.write("%f %f %f %d %d %d\n" % (xyz[i, 0], xyz[i, 1], xyz[i, 2], rgb[i, 0], rgb[i, 1], rgb[i, 2],))