この記事は、NTTドコモ R&D Advent Calendar 2021 25日目の記事です。
今年もNTTドコモ R&D Advent Calendarにお付き合いいただき、ありがとうございます。いよいよ12/25ということで、最後の記事になります。
はじめまして(orお久しぶりです)、ドコモの北出と申します。
昨年は都会の車窓からフォトグラメトリ 〜LiDAR無しスマホでもできる！3Dスキャン〜 - Qiitaというタイトルで、LiDARを使わないことをウリにした記事を書かせていただきましたが、今年は完全に手のひらを返してLiDARフル活用で記事を書きたいと思います。
こんな感じの(点群的には密だけど人口密度としては疎な)3Dモデルを作っていきます。
点群の色合いでクリスマス感を出してみました(ちょっと無理があるか)。
何をする記事？
とっても高級なLiDARを使って自社ビル（横須賀のR&Dセンタ）をスキャンします。
得られる(といいな)情報としてはおおよそ下記の通りです。
- Velodyne LiDAR（VLS-128）の特徴
- Open3D 0.14のコンパイル方法
- Open3D+GPUを使ったMulti-Scale ICPの動作
- LiDARデータ統合による3Dモデル作成
ちなみに、VLS-128は個人で購入するにはなかなか厳しい価格感ですが、宝くじでも当たればお家に一台欲しいなぁと思いました。
VLS-128とは？
VLS-128は、Velodyne社が開発した回転式LiDARのうち、最高級のモデルになります。Alpha Primeとも呼ばれます。この別名は、設定ファイルを探す際に地味に重要になってきます。
回転式のLiDARでは、垂直方向に16~128本のレーザー光を発射し、LiDARに戻ってくるまでの時間を測定することで三次元距離測定を実現します(ToF方式)。
垂直レーザーの本数は、モデルによって異なりますが、今回使うLiDAR「VLS-128」は名前の通り128本の垂直レーザを回転させ、スキャンを行います。
おおよそのスペックは下記の通りです。
- レーザー＋検出器：128本(垂直)
- FOV：水平360度、垂直+15度~-25度
- 測定(回転)周期：5〜10Hz
- 最大距離：300m
日本では、株式会社アルゴ様より購入ができます。
点群データの収集
前項で紹介したVLS-128を使って、実空間をスキャンしていきます。
今回は、ドコモR＆Dセンタの端から端まで歩いてスキャンすることにしました。
横須賀にある（のどかな？）拠点です。
機材
利用するLiDARに比べて、突然低コストな感じになります。
カインズで買ったコンテナ台車に下記のような物品を詰め込んで、手押し走行を行いました。不安定でヒヤヒヤしました。
- VLS-128(with 三脚)
- Anker PowerHouse II 400
- Macbook Pro(M1)
VLS-128はには100V電源がついてきましたので、100V出力可能なバッテリーを使っています。
データの保存PCとしてMBPを用いました。VLS-128とはイーサネットケーブル直結です。
ちなみに、VLS-128は車載を前提とした作りになっているようで、裏面にはM8のネジ穴が4つあります。今回はこれを利用して、1/4インチナットマウンターを3Dプリンタで自作して無理やり三脚にマウントしました。
VeloViewを使ったキャプチャ
Velodyneを使ったデータ収集は色々な方法があると思いますが、今回は可視化と保存含めて最もお手軽と思われるVeloViewを使うことにしました。
VeloView公式サイトより、4.1以降のバージョンをダウンロードしてきて利用します。macOS12,Windows10,Ubuntuそれぞれpcap形式でのキャプチャが動作しました。
※pcapファイルは、イーサネット経由でVelodyneセンサから入ってくるセンサストリームをdumpしたファイルです。ROSを経由したrosbag形式でもキャプチャ可能ですが、純粋にセンサストリームを保存する用途であればこのpcapがおすすめです。
センサストリームはこんな感じで可視化できます。
intensity(信号強度)で色を付けるモードにすると、材質によって見え方が変わって楽しい感じです。
Tools -> Recordを選択すると、pcap形式でデータが蓄積されていきます。
30分ほどで10GB弱のファイルになるので、ストレージが強めのPCを使うのが良さそうです(そのほかのスペックはM1なMacboocProでも問題ありませんでした)。
点群処理環境環境準備
キャプチャされた3Dな点群を扱うには、Open3Dが便利です。難しいことを考えずに使いたい方は、こちらのGetting Startedにある通り、下記コマンドで一発です。
pip install open3d
M1なMacでもRTXなUbuntuでも問題なく動きました。
以下Open3Dのコンパイルは、下記のような人におすすめです。
- Open3D-MLを使いたい
- KinectやRealSenseなどのデバイスを使いたい
- C++なサンプルをビルドしたい
純粋な点群処理だけであれば読み飛ばしていただいてOKです。
Open3Dのコンパイル
基本的な手順は、特にOpen3D-MLに関するところはこちらのドキュメントを参考にしてください。
前提環境
Pytorch1.8.1+CUDA11.1で動作を確認しました。
（私の方では、現時点ではTensorflow系の機能をONにできていません）
ただし、公式ドキュメントに記載されているPytorchのバージョンが異なるため、注意が必要です。
（細かいところで動作不良があるかもしれません。）
また、CXX ABIのバージョンについても注意が必要です。
これまたドキュメントにある通り、pipで取得可能なPytorchやTensorflowは古いABIにてコンパイルされているので、今回のOpen3Dコンパイルにおいても-DGLIBCXX_USE_CXX11_ABI=OFFオプションをつけてあげる必要があります。
もしくは、Open3D公式からビルド済みPytorchなどが配布されているので、そちらを使ってABIバージョンを合わせる方法もあります。
ソースからOpen3Dをコンパイル
下記手順に従って作業を進めます。おおよそ下記のようなセッテイングです。
- Python及びJupyterモジュール有効化
- CUDA有効化
- サンプルのコンパイル
- GPU機能有効化
- Open3D-ML有効化
- Pytorchモジュール有効化(基本的にはOpen3D-MLとセット)
cd /usr/local
sudo git clone --recursive https://github.com/intel-isl/Open3D
sudo git clone https://github.com/intel-isl/Open3D-ML.git
sudo chown -R ${USER} /usr/local/Open3D
cd Open3D
mkdir build
cd build
cmake -DCMAKE_INSTALL_PREFIX=/data/workspace/open3d_build \
-DPYTHON_EXECUTABLE=$(which python3) \
-DBUILD_PYTHON_MODULE=ON \
-DBUILD_SHARED_LIBS=ON \
-DBUILD_EXAMPLES=ON \
-DBUILD_UNIT_TESTS=OFF \
-DBUILD_BENCHMARKS=ON \
-DBUILD_CUDA_MODULE=ON \
-DBUILD_COMMON_CUDA_ARCHS=ON \
-DBUILD_CACHED_CUDA_MANAGER=ON \
-DBUILD_GUI=ON \
-DBUILD_JUPYTER_EXTENSION=ON \
-DWITH_OPENMP=ON \
-DWITH_IPPICV=ON \
-DENABLE_HEADLESS_RENDERING=OFF \
-DSTATIC_WINDOWS_RUNTIME=OFF \
-DGLIBCXX_USE_CXX11_ABI=OFF \
-DUSE_BLAS=ON \
-DBUILD_FILAMENT_FROM_SOURCE=OFF \
-DWITH_FAISS=OFF \
-DBUILD_LIBREALSENSE=ON \
-DUSE_SYSTEM_LIBREALSENSE=ON \
-DBUILD_AZURE_KINECT=ON \
-DBUILD_TENSORFLOW_OPS=OFF \
-DBUILD_PYTORCH_OPS=ON \
-DBUNDLE_OPEN3D_ML=ON \
-DOPEN3D_ML_ROOT=../../Open3D-ML \
..
make -j$(nproc)
sudo make install
sudo make install-pip-package
点群の結合
今回キーとなるのはmulti-scale ICPというアルゴリズムです。
ICPは点群同士の位置合わせを行う代表的なアルゴリズムとなりますが、multi-scale ICPとしては密な点群の位置合わせを扱えるという特徴があります。
Open3DのPipeline(Tensor)から扱うことで、cuda処理を可能としています。
流れは以下の通りです。
- Velodyne pcapデータのフレーム毎読み出し
- 直前の点群との位置合わせ(multi-scale ICP)
- うまくフィットしない場合、全体の点群との位置合わせを実施
- 点群のマージ、保存
次のようなソースコードで、実際のpcapデータを使って点群を繋ぎ合わせ、3Dモデルを作っていきます。
あいだに入っているコメント(コードブロック外)を抜いてコピペすれば、動かせると思います。
#coding:utf-8
from tqdm import tqdm
import numpy as np
import argparse
import os
import open3d as o3d
import datetime
import time
import velodyne_decoder as vd
import copy
import math
点群のデコードには、velodyne_decorderを利用します。
Open3Dについても正しくimportできるか確認します。
parser = argparse.ArgumentParser(
prog='pcap_multi-scale_ICP',
usage='create pointcloud from pcap',
description='',
epilog='end',
add_help=True,
)
parser.add_argument('-i', '--input')
parser.add_argument('-o', '--output')
parser.add_argument('-s', '--start', type=float)
parser.add_argument('-e', '--end', type=float)
parser.add_argument('-m', '--icp_interval', type=float)
parser.add_argument('-v', '--voxel_size', type=float)
parser.add_argument('-c', '--cuda', action='store_true')
parser.add_argument('--save_interval', type=int)
parser.add_argument('--merge_interval', type=int)
args = parser.parse_args()
voxel_resol = 0.1
icp_interval = 0.1
save_interval = 100
merge_interval = 10
outdir = os.path.dirname(args.input) + '/' + os.path.splitext(os.path.basename(args.input))[0] + '/integration/'
start_time = 0.0
end_time = time.time()
if args.output:
outdir = args.output
if not os.path.isdir(outdir):
os.makedirs(outdir)
if args.voxel_size:
voxel_resol = args.voxel_size
if args.start:
start_time = args.start
if args.end:
end_time = args.end
print(str(start_time) + " to " + str(end_time))
if args.icp_interval:
icp_interval = args.icp_interval
if args.save_interval:
save_interval = args.save_interval
if args.merge_interval:
merge_interval = args.merge_interval
入力はpcapファイル、出力は点群(ply)ファイルとなります。また、Open3DのGUI画面についても、スクリーンショットとして保存することにしました。
各フレームのマージは毎回行うのではなく、merge_intervalで指定した頻度で、voxel_resolで指定した解像度でボクセル化した上で繋ぎ合わせることにしました。
また、メモリーいっぱいでプログラムが落ちたりもするので(最初からやり直しで悲しい)、save_intervalで指定した頻度でplyファイルに保存することにします。
boundary = {'minX':-30.0, 'minY':-30.0, 'minZ':-5.0, 'maxX':30.0, 'maxY':30.0, 'maxZ':20.0}
def maxDist():
minXYZ_len = math.sqrt(boundary['minX']**2 + boundary['minY']**2 + boundary['minZ']**2)
maxXYZ_len = math.sqrt(boundary['maxX']**2 + boundary['maxY']**2 + boundary['maxZ']**2)
return max(minXYZ_len, maxXYZ_len)
VLS-128は最大300ｍの測位が可能ですが、点群を結合していくためには大きすぎるため点群を球状のエリアでクロップすることにしました。boundaryで指定した範囲のうち最長の長さを半径として、点群を切り取っていきます。
def process_m_ICP(s_pcd, t_pcd):
#ICP process
# Setting Verbosity to Debug, helps in fine-tuning the performance.
# o3d.utility.set_verbosity_level(o3d.utility.VerbosityLevel.Debug)
s = time.time()
source_pcd = o3d.t.geometry.PointCloud.from_legacy(s_pcd)
source_pcd.estimate_normals()
target_pcd = o3d.t.geometry.PointCloud.from_legacy(t_pcd)
target_pcd.estimate_normals()
if args.cuda:
source_pcd = source_pcd.cuda(0)
target_pcd = target_pcd.cuda(0)
registration_ms_icp = o3d.t.pipelines.registration.multi_scale_icp(source_pcd,target_pcd, voxel_sizes,
criteria_list,
max_correspondence_distances,
init_source_to_target, estimation,
save_loss_log)
ms_icp_time = time.time() - s
print("Time taken by Multi-Scale ICP: ", ms_icp_time)
print("Inlier Fitness: ", registration_ms_icp.fitness)
print("Inlier RMSE: ", registration_ms_icp.inlier_rmse)
return registration_ms_icp
こちらのチュートリアルほぼそのままなのですが、cpu側で作成した点群をcuda処理を行う改造をしております。
config = vd.Config()
config.model = 'VLS-128'
config.rpm = 600
config.calibration_file = 'calibrations/Alpha Prime.yml'
config.timestamp_first_packet = start_time
config.max_range = maxDist()
pcap_file = args.input
cloud_arrays = []
velodyne_decoder側の設定になります。
config.modelをVLS-128に設定するだけではcalibration_fileが読み込めなかったため、こちらにあるAlpha Prime.ymlを指定しました。
なお、処理を行うタイムスタンプをconfig.timestamp_first_packetで指定してみましたが、これはうまく動作しませんでしたので、別で処理をしています(後述)。
if args.cuda:
device = o3d.core.Device("CUDA:0")
print('Using CUDA')
else:
device = o3d.core.Device("CPU:0")
vis = o3d.visualization.Visualizer()
vis.create_window()
pcd = o3d.geometry.PointCloud()
geom_added = False
pcd_prev = None
## ICP settings
voxel_sizes = o3d.utility.DoubleVector([0.1, 0.05, 0.025])
# List of Convergence-Criteria for Multi-Scale ICP:
criteria_list = [
o3d.t.pipelines.registration.ICPConvergenceCriteria(relative_fitness=0.0001,
relative_rmse=0.0001,
max_iteration=50),
o3d.t.pipelines.registration.ICPConvergenceCriteria(0.00001, 0.00001, 15),
o3d.t.pipelines.registration.ICPConvergenceCriteria(0.000001, 0.000001, 10)
]
# `max_correspondence_distances` for Multi-Scale ICP (o3d.utility.DoubleVector):
max_correspondence_distances = o3d.utility.DoubleVector([0.3, 0.14, 0.07])
# Initial alignment or source to target transform.
init_source_to_target = o3d.core.Tensor.eye(4, o3d.core.Dtype.Float32)
# Select the `Estimation Method`, and `Robust Kernel` (for outlier-rejection).
estimation = o3d.t.pipelines.registration.TransformationEstimationPointToPlane()
# Save iteration wise `fitness`, `inlier_rmse`, etc. to analyse and tune result.
ここも、サンプルそのままです。
なお、大規模な点群でうまくマッチしないケースがあったので、max_iterationを50まで増やしています。
また、multi-scale ICPでは三段階の解像度（voxel_sizes）でマッチング処理を行いますが、この解像度を上げすぎると時間がかかるだけではなくメモリー不足で落ちてしまいます。。
save_loss_log = True
stamp_prev = 0
trans_prev = None
trans_list = []
pcd_prev = None
count = 0
prev_save_count = 1
prev_merge_count = 1
fitness_threshold = 0.65
for stamp, points in vd.read_pcap(pcap_file, config):
if stamp < start_time:
continue
if stamp > end_time:
break
str_timestamp = str(datetime.datetime.fromtimestamp(stamp)).replace(' ', '-')
pcd_cur = o3d.geometry.PointCloud()
if stamp-stamp_prev >= icp_interval:
pcd_cur.points = o3d.utility.Vector3dVector(points[:,:3])
else:
continue
指定されたタイムスタンプ内でVelodyne点群をOpen3D形式の点群に変換します。
registration_ms_icp = None
if pcd_prev is None or pcd is None:
pcd_prev = pcd_cur
pcd = pcd_cur
else:
print(stamp)
registration_ms_icp = process_m_ICP(pcd_prev, pcd_cur)
stamp_prev = stamp
if registration_ms_icp.fitness < fitness_threshold:
print("global registration")
registration_ms_icp = process_m_ICP(pcd, pcd_cur)
pcd_prev = copy.deepcopy(pcd_cur)
一定周期でmulti-scale ICPを行います。一致度が一定(ここでは0.65としました)を下回った時、点群全体との位置合わせを行います。
if registration_ms_icp is not None:
pcd.transform(o3d.core.Tensor.numpy(registration_ms_icp.transformation))
if prev_merge_count > merge_interval:
pcd_cur_np = np.asarray(pcd_cur.points)
pcd_global_np = np.asarray(pcd.points)
pcd_tmp_np = np.concatenate((pcd_cur_np,pcd_global_np), axis=0)
pcd.points = o3d.utility.Vector3dVector(pcd_tmp_np)
pcd.points = pcd.voxel_down_sample(voxel_size=voxel_resol).points
print("Merge pointcloud")
print(pcd)
prev_merge_count = 0
else:
prev_merge_count += 1
multi-scale ICPの結果を用いて、点群の位置や向きを変えていきます。
また、一定周期でダウンサンプルした上で、全体の点群にマージしていきます。
if prev_save_count > save_interval:
o3d.io.write_point_cloud(outdir + '/' + os.path.splitext(os.path.basename(args.input))[0] + '.ply', pcd)
prev_save_count = 0
else:
prev_save_count += 1
if geom_added == False:
vis.add_geometry(pcd)
geom_added = True
vis.update_geometry(pcd)
vis.poll_events()
vis.update_renderer()
time.sleep(0.01)
vis.capture_screen_image(outdir + '/' + 'screen_{:07d}'.format(count) + '.jpg', True)
count += 1
点群の保存とともに、スクリーンショットも保存していきます。
ちなみに、環境によってはcapture_screen_imageをupdate_rendererの直後に呼び出すと真っ白な画像しか保存されませんでしたので、sleepを入れています。
結果
上記のプログラムで処理した、点群位置合わせの様子です。
Z軸方向の高さで色をつけています。
点群の位置合わせやSLAM処理では、Loop Closureや最適化処理を行うのが一般的ですが、今回のようなICPベースの手法のみでもそれなりの完成度に見えます。
VLS-128の点群が非常に高密度であったことも影響していると思われます。
所々段差で台車を持ち上げたのですが、しっかり追従できていますね。
タイトルで「ぼっち」と書かせていただいたように、この点群には人や車がほとんど写っておりません。
(ドコモR＆Dセンタ拠点に人がいなかったのもありますが)点群の結合処理の段階でダウンサンプルを行なっている都合上で、動くものが消えたようです。
まとめ
Open3Dを使って、Velodyne LiDAR点群を繋ぎ合わせ、3Dモデルを作っていきました。
今回は単色の点群をもとにしてモデルを作りましたが、機会があれば今後カメラとの組み合わせによるカラー点群や点群認識なども紹介したいと思います。
それでは、良いお年を！！