対話環境でPoint Cloud Library (PCL) を使いたい

新年はじめての記事ということで、少し遅いですが、あけましておめでとうございます。PCLを対話環境で使いたかったので、お正月の間にPCLのラッパーを作りました¹。なぜ作ったのか、どうやって作ったのか、少し整理して書いてみようと思います。

Point Cloud Library (PCL) とは

http://www.pointclouds.org/

問題

PCL はboost、Eigenに依存している、かつtemplateを多く使用しているため、PCLを使用したプロジェクトのコンパイル時間は非常に長くなるという問題があります。twitterで [PCL コンパイル] として検索すると、例えば以下の様なツイートが見つかりますが、完全に同意です。

PCLリンクしてるコードのコンパイルに一分半くらいかかる。つらい

— がらえもん(プログラム書く (@garaemon_coder) August 14, 2015

PCLはC++だしコンパイル遅いしで色々めんどくさい

— 動かないで点P (@initial_D_0601) August 25, 2015

PCLを使うプロジェクトのコンパイル時間かかりすぎて辛いわ

— kato tetsuro (@tkato_) November 6, 2015

boostへの依存関係が必須かどうかについては疑問が残りますが、点群処理ではパフォーマンスが求められることが多いと思われるので、C++で書かれていることは合理的に思います。とはいえ、コンパイル時間が長いのは試行錯誤するにはつらいです。

ではどうするか

試行錯誤のサイクルを速く回せるようにすることは僕にとって非常に重要だったのと、 C++で書かなければいけないという制約もなかった（※組み込み用途ではない）ので、対話的にPCLを使うために、僕は動的型付け言語でラッパーを作ることにしました。

参考までに、対話環境を使うことによるメリットは、下記スライドが参考になります。PCLの紹介もされています²。

何で書くか

世の中には色んなプログラミング言語があります。C++ライブラリのラッパー作るぞとなったとき、僕にとって選択肢は、

• Python
• Julia

の二択でした。それぞれ、以下のプロジェクトに頼れば templateを多用したライブラリのラップができそうだと思いました。

• Cython
• Cxx.jl

pythonに関しては、すでに cythonで書かれた strawlab/python-pcl というラッパーがあります。しかし、

• 現在あまりメンテされていない
• サポートされている機能も多くはない
• templateを多用したライブラリのラップをcythonで十分にできるかどうか自信がなかった ³
• Juliaは関数や型がパラメータを持てるため、templateを多用したライブラリのラップが簡単にできそうだと思った（i.e. pcl::PointCloud<T> は PointCloud{T} と書ける⁴）
• Cxx.jl を使えば JITライクに C++ を使える（試行錯誤できる）し、Juliaのほうがいいかな

といった理由から、Juliaで書くことにしました。

成果物

https://github.com/r9y9/PCL.jl

StatisticalOutlierRemovalのデモ | nbviewer こんな感じで、jupyter上で試行錯誤できるようになりましたとさ⁵。strawlab/python-pcl よりも多くのことができると思います。⁶

PCLは非常に大きなライブラリのため、全ての機能をラップするつもりはありませんが、今後必要に応じて機能を追加するかもしれません。

適当なスクショ

PCL.jl で、少なくとも最低限以下はできますということで。ソースコードは r9y9/PCL.jl/examples にあります。

PCLVisualizer

3D Object Recognition based on Correspondence Grouping

Hypothesis Verification for 3D Object Recognition

Extracting indices from a PointCloud

Kinect v2で遊ぶ

画質低い & クロップが適当で一部しか見えませんが、諸々の処理を含めて fpsは15くらいでしょうか。depthとrgb imageのregistration、その結果の点群への変換に関しては、20~30fps程度でした 測りなおしたら平均40fpsくらいはでてました。real-timeで点群を処理するようなアプリケーションを書く場合は、現実的にはC++で書くことになるかと思います。

余談

Kinect v2 から得たデータを点群に変換するのに、Juliaではパフォーマンスを出すのに苦労したのですが、結果面白い（キモい？）コードができたので、少し話はそれますが簡単に紹介しておきたいと思います。

Depthとcolorを点群に変換する関数

まず、コードを以下に示します。

function getPointCloudXYZRGB(registration, undistorted, registered)
    w = width(undistorted)
    h = height(undistorted)
    cloud = pcl.PointCloud{pcl.PointXYZRGB}(w, h)
    icxx"$(cloud.handle)->is_dense = false;"
    pointsptr = icxx"&$(cloud.handle)->points[0];"
    icxx"""
    for (size_t ri = 0; ri < $h; ++ri) {
        for (size_t ci = 0; ci < $w; ++ci) {
            auto p = $(pointsptr) + $w * ri + ci;
            $(registration)->getPointXYZRGB($(undistorted.handle),
                $(registered.handle), ri, ci, p->x, p->y, p->z, p->rgb);
        }
    }
    """
    cloud
end

r9y9/PCL.jl/examples/libfreenect2_grabbar.jl#L12-L29

syntax highlightとは何だったのか、と言いたくなるようなコードですが、performance heavy な部分は icxx"""...""" という形で、C++ で記述しています。Juliaのコード中で、こんなに自由にC++を使えるなんて、何というかキモいけど書いていて楽しいです。

なお、最初に書いたコードは、以下の様な感じでした。

function getPointCloudXYZRGB(registration, undistorted, registered)
    w = width(undistorted)
    h = height(undistorted)
    cloud = pcl.PointCloud{pcl.PointXYZRGB}(w, h)
    icxx"$(cloud.handle)->is_dense = true;"
    pointsptr = icxx"&$(cloud.handle)->points[0];"
    for ri in 0:h-1
        for ci in 0:w-1
            p = icxx"$(pointsptr) + $w * $ri + $ci;"
            x,y,z,r,g,b = getPointXYZRGB(registration, undistorted,
                registered, ri, ci)
            isnan(z) && icxx"$(cloud.handle)->is_dense = false;"
            icxx"""
            $p->x = $x;
            $p->y = $y;
            $p->z = $z;
            $p->r = $r;
            $p->g = $g;
            $p->b = $b;
            """
        end
    end
    cloud
end

r9y9/PCL.jl/examples/libfreenect2_grabbar.jl#L12-L29

このコードだと、forループの中でJulia関数の呼びだしが発生するため、実は重たい処理になっています。このコードだと、確かfps 3 とかそのくらいでした。関数呼び出しがボトルネックだと気づいて、icxx"""...""" でくるんで（一つの関数にすることで）高速化を図った次第です。

雑記

以下、僕のmacbook proで tic(); using PCL; toc() をした結果：

julia> tic(); using PCL; toc()
INFO: vtk include directory found: /usr/local/include/vtk-6.3
INFO: Loading Cxx.jl...
INFO: dlopen...
INFO: vtk version: 6.3.0
INFO: Including headers from system path: /usr/local/include
INFO: pcl_version: 1.8
INFO: Include pcl top-level headers
  1.053026 seconds (91 allocations: 4.266 KB)
INFO: Include pcl::common headers
  5.433219 seconds (91 allocations: 4.078 KB)
INFO: adding vtk and visualization module headers
INFO: Include pcl::io headers
  0.389614 seconds (195 allocations: 11.034 KB)
INFO: Include pcl::registration headers
  1.428106 seconds (195 allocations: 11.065 KB)
INFO: Include pcl::recognition headers
  1.154518 seconds (136 allocations: 6.141 KB)
INFO: Include pcl::features headers
  0.033937 seconds (181 allocations: 8.094 KB)
INFO: Include pcl::filters headers
  0.070545 seconds (316 allocations: 14.125 KB)
INFO: Include pcl::kdtree headers
  0.022809 seconds (91 allocations: 4.078 KB)
INFO: Include pcl::sample_consensus headers
  0.014600 seconds (91 allocations: 4.141 KB)
INFO: Include pcl::segmentation headers
  0.010710 seconds (46 allocations: 2.094 KB)
INFO: FLANN version: 1.8.4
elapsed time: 39.194405845 seconds
39.194405845

r9y9/PCL.jl/src/PCL.jl#L90-L101 pcl/pcl_base.h. pcl/common/common_headers.h 当たりのパースに大分時間かかってますね、、。まぁ一度ロードしてしまえば、Juliaのプロセスをkillしないかぎり問題ないのですが。開発中は、頻繁にreloadする必要があって、辛かったです。

ロード時間が長い問題は、Cxx.jlにプリコンパイル（Keno/Cxx.jl/issues/181）がサポートされれば、改善するかもしれません。

さいごに

PCLを対話環境で使えるようになりました。快適です。また今回のラッピングを通して、PCLとは関係ありませんが、Cxx.jl でできないことはほぼないという所感を持ちました。C++ の対話環境（REPL）も付いているので、最強すぎますね。Cythonでもできるぞってことであれば、教えて下さい。僕もpythonから使えるのであれば使いたいです（でも作るのは面倒過ぎる気がするので手を出せない）。

僕にとって快適な環境はできましたが、Cxx.jl のビルドはかなり面倒なので（Juliaの開発版も必要ですし…）、きっと誰も使わないんだろうなー、、、

参考

• Keno/Cxx.jl
• Cxx.jl を使ってみた感想 + OpenCV.jl, Libfreenect2.jl の紹介