点群の時代はまだ終わっていない

問題を解決するのを長く待っていると、誰かが代わりに解決してくれるかもしれません。少なくとも、シンクの皿についてはそう自分に言い聞かせています。ウィーンでの会議中、まさに私のためにそれをやってくれた別の研究者たちを見つけました。彼らは私が2年前に発表した研究を基に、ほぼすべてのベンチマークで私を凌駕しました。この記事は、彼らの研究に注目を集め、そして少し利己的に、彼らの研究を再実装する過程で私が学んだことについて語るために書いています。要するに、彼らは点群に対する衝突判定のためのデータ構造を作成し、非常に高速に動作する一方で、メモリと構築時間も非常に安価に抑えています。詳細に興味がない場合は、論文やオリジナルのC++実装に直接ジャンプできます。私は独自の最適化を施したRust実装も公開しており、ソースコードはGitHubに、パッケージはcrates.ioにあります。 Recapt A Franka Emika Pandaロボットとその球状化された衝突判定表現。 私は多くの時間をモーションプランニングについて考えて過ごしています。ロボットが開始状態から目標状態へ衝突のない動きを見つける方法です。モーションプランニング問題を解決する方法は無数にありますが、論文を十分に読めば、どれも似たように見えてきます。いくつかの設定をサンプリングし、それらが有効かどうかをテストし、すべて可能な設定上で大きな経路探索を行います。それらのアルゴリズムのすべてが設定検証を必要とします。ロボットの設定が与えられたとき、ロボットがその位置で世界のジオメトリと衝突するかどうかを判断します。ロボットはしばしば知覚された環境で動作するため、その世界のジオメトリは通常、点群として私たちに届きます。ロボットのジオメトリが球の集まりに単純化されている場合、問題を球体衝突判定にさらに単純化できます。任意の設定で、ロボット上の球体のいずれかが知覚された点群と衝突するかどうかを最小時間でチェックします。 問題提起: 点のリストと球のセットが与えられたとき、球のいずれかが最小時間で衝突するかどうかを判断します。 数年前、私はCAPT（Collision-Aware Point Tree）と呼ばれるデータ構造を提案しました。これは、点群に対する設定検証を非常に高速に行うように設計されています。要するに、球と点群間の衝突チェッカーです。これはk-dツリーのような最近傍探索構造ですが、構築時に余分な作業を行い、探索ツリーをバックトラックしないようにしています。その結果、バッチ並列探索アルゴリズムを備えたk-dツリーとなり、SIMDで高速化された分岐なしクエリをサポートします。CAPTの大きな問題は構築時間でした。密な点群は、バックトラックを避けるために多くの重複データを必要とします。点群が十分に密になると、CAPTの構築はO(N log N)にスケールし、これはユーザーが制御ループ周波数でプランニングを得たいという希望にとって壊滅的です。CAPTのデータレイアウトでは、空間のある領域を表す探索ツリーの各リーフが、点群の多くの点の重複コピーを格納する必要があります。それらの重複コピーはデータ構造のフットプリントを支配し始め、それが構築時間を膨張させます。 箱の中の思考 Via Chen and Yeh、ボクセルベースの衝突判定スキーム。 チン・チェンとツンタイ・イェーという2人のロボット工学研究者は、自分たちのためにCAPTの問題点を修正することにしました。そのために、彼らは最近傍探索ツリーを完全に捨てることから始めました。空間分割ツリーの代わりに、空間をボクセルのグリッドに分割でき、各ボクセルはそれらが含む点のリストを格納します。この利点は二重です。第一に、単純な算術演算でクエリ球がどのボクセルにあるかを伝えることができます。第二に、隣接するボクセルを見つけることが容易であるため、点を重複させる必要がありません。しかし、ワークスペースのすべてのボクセルを単純に格納しても機能しません。ワークスペースが各次元で100ボクセル長の場合、数千点の点しか含まないフィルタリングされた点群を記録するために、100万ボクセルの情報を格納する必要があるかもしれません。これを制御下に置くために、チェンとイェーは、各レイヤーが1つの次元でセグメント化された3層スパースツリーに、占有されたボクセルのみを疎に格納します。これにいくつかの軸配置バウンディングボックステストを組み合わせると、結果の構造は多層ボクセルテーブル、すなわちMVTになります。CAPTと同様に、MVTは単一命令複数データ（SIMD）並列処理を使用して並列化可能です。任意のボクセルに対して、衝突チェッカーは、ボクセルに含まれるすべての点との衝突について、一定の速度で大きなバッチチェックを実行できます。 平坦な層のパッチ 平坦なボードのように MVTのオリジナルの実装には、いくつかの厄介なC++の癖がありました。すなわち、ボクセルテーブルはテーブルの各行へのポインタのタペストリーを使用していました。これは一般的に少し狂気じみていただけでなく、メモリ管理を非常に困難にし、サイズ効率も良くありませんでした。オリジナルのC++実装には、多くの奇妙な手動プール管理があり、点群が大きすぎると壊滅的なクラッシュを引き起こします。 struct MVT { // ZLevelTable = uint32_t* を使用したボクセルインデックスへのポインタ // YLevelTable = uint32_t** を使用したzレベルテーブルへのポインタ // XLevelTable = uint32_t*** を使用したyレベルテーブルへのポインタ XLevelTable x_level_table; } Rustでの実装を容易にするために、私は少し単純化しました。すべてをBox<[]>でバックします。 struct Mvt { /// グリッドインデックスからボクセルデータをどこから取得するかを示す tables: Box<[u32]>, /// `points`内のボクセルリストを取得する場所を示す voxels: Box<[u32]>, /// 平坦化されたSoA、すべての点データの共有バッファ points: [Box<[f32]>; 3] // 他のフィールド... } struct Voxel { /// `points`内のこのボクセルに格納されている最初の点のインデックス offset: u32, /// ボクセル内の点の数 count: u32, /// 他のフィールド... } 検索ロジックは非常にシンプルになります。テーブルを使用して、どのボクセルに属するかを特定し、`voxels`でルックアップします。次に、ボクセルを使用して、衝突判定する必要がある点の範囲を特定し、最終的にそれらの点に対して総当たりチェックを実行します。 変更可能にする 検索ロジックを大幅に単純化するだけでなく、新しい検索構造により、各Voxelに独自のpointsフィールドを与えるだけで、MVTを変更可能にすることが簡単になります。これは、単一の大きなバッファを共有するのではなく、 // `points` は `Mvt` から削除されました struct Voxel { /// `points`内のこのボクセルに格納されている最初の点のインデックス offset: u32, /// ボクセル内の点の数 count: u32, /// このボクセル内の点のSoAバッファ points: [Vec<[f32]>; 3] // 他のフィールド... } 変更可能性を追加すると、サイズが約2倍、構築時間が約1.5倍になりますが、便利な機能です。MVTを単一使用構造として使用するユーザーのために良好なパフォーマンスを維持するために、実装を分割しました。変更不可能なデフォルトのMvtとMutableMvt構造の両方を記述しました。 大きなボール、大きな問題 Fetchロボットの球状化（左）と、それぞれ赤、緑、紫で示された半径。 MVTを構築するには、ボクセルの大きさを選択する必要があります。ボクセルが大きすぎると、衝突判定クエリは遠くの点を検索するのに無駄な時間がかかりますが、小さすぎると、クエリは多数の小さなボクセルに対して除外チェックを行う必要があります。しかし、いくつかの妥当な候補があります。各ロボットの球状化されたジオメトリ上で、ロボットの最大の球体、半径がr_maxのものを選択できます。あるいは、ロボットの移動リンクのみに限定し、ほとんどのロボットのベースリンクにある大きな球体をスキップして、r_linkを選択することもできます。最後に、ロボットのバウンディングボリューム階層を見て、衝突判定に使用される最大の球体のサイズであるr_bvを選択できます。 クエリ速度のボクセル幅によるスケーリング。各曲線は、X軸上のボクセル幅で生成されたMVTの平均クエリ時間を示しており、ロボットごとに分離されています。r_max、r_link、r_bvはそれぞれ●、■、▲でマークされています。 オリジナルのMVT論文では、主にクエリ時間について一般的に言及し、パフォーマンスが良いと主張して、r_max = 1.0の使用を推奨していました。