はじめに
Point Cloud Libarary（PCL）の勉強を始めました。
- PCL - Point Cloud Library (PCL)
- 【シリーズ「PCLを触ってみよう!」第一回】3D点群処理ライブラリ「Point Cloud Library」の概要 | DERiVE ブログ & メルマガ
- Point Cloud Library - Wikipedia
PCL は 3 次元の点群（ポイントクラウド）を扱うための大規模な C++、BSD ライセンスなライブラリです。Kinect から得られるデータはデプス画像としてのデータですが、PCL ではこの画素を 3 次元空間にプロットし、フィルタリング、特徴推定、表面再構成、イメージレジストレーション、モデルフィッティング、セグメンテーションなどを行うための様々なアルゴリズムを利用することが出来ます。開発に携わっている会社のリストにはインテルやトヨタ、NVIDIA、資金提供元のリストには Google 等、名だたる企業や機関が名を連ねています。
個人的には、2次元的な処理なら OpenCV、3次元的な処理なら PCL と解釈しています。目標としては PCL を Unity と組み合わせて、バーチャルな物体と実世界の物体とのインタラクションを目指したいと考えています。
環境
- Windows 8.1
- Visual Studio Express 2013
- Xtion PRO LIVE
環境構築
PCL は公式で v1.6.0 の VS2010 による Prebuild バイナリを配布しています。
これを利用すれば簡単に環境構築が可能です。しかしながら現在（2014/05/11）の PCL の最新安定版は v1.7.1 であり、3rd Party のライブラリも最新のもの（たとえば Boost は v1.55.0）を使いたいです。開発環境も C++11 の機能が潤沢に使える VS2013 を使いたいところです。素晴らしいことに Summary?Blog 様で、最新の v1.7.2 及び 3rd Party のライブラリ群を VS2013 によってビルドした Prebuild バイナリが公開されています。
更に、4/23 でクローズしてしまった OpenNI もアーカイブして下さっています。
ありがとうございます！PCL と OpenNI2 をインストールし、VS2013 でプロジェクトを作成、Include ディレクトリと Library ディレクトリをポチポチ設定すれば準備完了です。
なお、Mac では brew で以下のリポジトリをタップすれば 1.7.0 が導入可能です（が、私の環境では CloudViewer の表示でエラーが出て不明なのでうまく行ってません...）。
PCL の学習
日本語では、DERiVE 様により大変分かりやすい「PCL を触ってみよう！」シリーズが公開されています。
- 【シリーズ「PCLを触ってみよう!」第一回】3D点群処理ライブラリ「Point Cloud Library」の概要 | DERiVE ブログ & メルマガ
- PCLを触ってみよう！ | DERiVE ブログ & メルマガ
こちらを参考にさせて頂いて今勉強しています。また公式にも豊富なチュートリアルが用意されています。
Grabber を使用しない点群の表示
PCL には OpenNIGrabber という OpenNI を利用した点群を CloudViewer（ポイントクラウドを表示する便利なビューワ）で表示するものが用意されています。
- 【シリーズ】「PCLを触ってみよう!」 第４回：OpenNIによるKinectからのデータ取得とその保存 | DERiVE ブログ & メルマガ
- 最小限のPCL+OpenNIコード
- Point Cloud Library (PCL): pcl::OpenNIGrabber Class Reference
しかしながらビルド時に OpenNI のパスが通ってないと生成されないため、上述の v1.7.2 の Prebuild バイナリにはこれが含まれていません。しかしながら OpenNIGrabber は基本的には Boost.Signals2 を利用して簡単に PointCloud を受け取れるようにしているものなので、自前で Grabber を作成したり、Grabber を利用しなくとも PointCloud を自前で用意して CloudViewer に食わせてあげれば問題ありません。表示するための最小限の処理はこんな感じです。
int main() { pcl::visualization::CloudViewer viewer("OpenNI Viewer"); while (1) { boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB>> cloud(new pcl::PointCloud<pcl::PointXYZRGB>()); // ...（PointCloud に PointXYZRGB をつめる） viewer.showCloud(cloud); } }
つめるのに便利なクラスとかを適当に作って PointCloud を詰めていけば良いわけです。まず、Grabber を使用しない例を作ってみます。
main.cpp
#include <pcl/visualization/cloud_viewer.h> #include <pcl/io/pcd_io.h> #include <boost/make_shared.hpp> #include "xtion.h" const int WIDTH = 640, HEIGHT = 480, FPS = 30; int main() { // CloudViewer を生成 // （表示は別スレッドで実行される） pcl::visualization::CloudViewer viewer("OpenNI Viewer"); // Xtion からデータを取ってくるクラスのインスタンス（後述） Xtion sensorColor(openni::SensorType::SENSOR_COLOR, WIDTH, HEIGHT, FPS); Xtion sensorDepth(openni::SensorType::SENSOR_DEPTH, WIDTH, HEIGHT, FPS); // 最新のポイントクラウドを表示し続ける for (int i = 0; ; ++i) { // CloudViewer に与える PointCloud boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB>> cloud(new pcl::PointCloud<pcl::PointXYZRGB>()); // ポイントクラウドの大きさをセット cloud->width = WIDTH; cloud->height = HEIGHT; cloud->is_dense = false; cloud->points.resize(cloud->height * cloud->width); // 最新の RGB / デプス情報を取ってくる const auto color = sensorColor.getData<openni::RGB888Pixel>(); const auto depth = sensorDepth.getData<openni::DepthPixel>(); const int resX = sensorColor.getResolutionX(); // データを1つずつみてポイントクラウドに詰めていく for (int j = 0; j < HEIGHT; ++j) { for (int i = 0; i < WIDTH; ++i) { // convertDepthToWorld を利用すると、実際の世界の座標に変換してくれる const auto z = depth[j * resX + i]; float wx, wy, wz; openni::CoordinateConverter::convertDepthToWorld(sensorDepth.getStream(), i, j, z, &wx, &wy, &wz); // 実際の世界の座標を Point につめる pcl::PointXYZRGB point; const float millimeterToMeter = 0.001f; point.x = wx * millimeterToMeter; point.y = wy * millimeterToMeter; point.z = wz * millimeterToMeter; // RGB カメラとデプスカメラの位置がずれているので適当にオフセットして色を Point につめる const int offsetX = 10; const int index = j * resX + (i + offsetX < WIDTH ? i + offsetX : WIDTH); const auto rgb = color[index]; point.r = rgb.r; point.g = rgb.g; point.b = rgb.b; cloud->push_back(point); } } // CloudViewer で点群を表示 viewer.showCloud(cloud); // ESC キーで終了 if (GetKeyState(VK_ESCAPE) < 0) { break; } } }
xtion.h
#include <OpenNI.h> // OpenNI2 で Xtion からデータを取ってくるクラス class Xtion { public: Xtion(openni::SensorType type, int width, int height, int fps); ~Xtion(); template <typename T> const T* getData() { openni::VideoFrameRef frame; videoStream_.readFrame(&frame); if (!frame.isValid()) { return nullptr; } return static_cast<const T*>(frame.getData()); } int getResolutionX() const; const openni::VideoStream& getStream() const; private: static void Initialize(); static void Shutdown(); void createStream(); void start(); static openni::Device Device; static int Count; const openni::SensorType sensorType_; const int width_, height_, fps_; openni::VideoStream videoStream_; };
xtion.cpp
#include "xtion.h" #include <exception> openni::Device Xtion::Device; int Xtion::Count = 0; Xtion::Xtion(openni::SensorType type, int width, int height, int fps) : sensorType_(type), width_(width), height_(height), fps_(fps) { Initialize(); createStream(); start(); } Xtion::~Xtion() { Shutdown(); } int Xtion::getResolutionX() const { return videoStream_.getVideoMode().getResolutionX(); } const openni::VideoStream& Xtion::getStream() const { return videoStream_; } void Xtion::Initialize() { // 初回だけ OpenNI の初期化 if (Count == 0) { openni::OpenNI::initialize(); if (Device.open(openni::ANY_DEVICE) != openni::STATUS_OK) { throw std::exception(openni::OpenNI::getExtendedError()); } } ++Count; } void Xtion::Shutdown() { --Count; // インスタンスがいなくなったら OpenNI の終了 if (Count == 0) { openni::OpenNI::shutdown(); } } void Xtion::createStream() { videoStream_.create(Device, sensorType_); auto mode = videoStream_.getVideoMode(); mode.setResolution(width_, height_); mode.setFps(fps_); videoStream_.setVideoMode(mode); } void Xtion::start() { videoStream_.start(); }
openni::CoordinateConverter::convertDepthToWorld で実世界の位置に変換できるのが便利です。
結果
手元に持ったカメラの映像を真上から見るみたいなのとか出来て面白いですね。
自前の Grabber を作成
参考：
先ほど作った Xtion クラスをそのまま利用して書いて、main.cpp を書き換えてみます。
main.cpp
#define _SCL_SECURE_NO_WARNINGS #include <boost/thread.hpp> #include <pcl/io/grabber.h> #include <pcl/visualization/cloud_viewer.h> #include <Windows.h> #include "xtion.h" class XtionGrabber : public pcl::Grabber { public: typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloud; typedef void (PointCloudSignalFuncType)(const PointCloud::ConstPtr&); XtionGrabber(int width, int height, int fps) : Grabber(), isRunning_(false), width_(width), height_(height), fps_(fps) { signal_ = createSignal<PointCloudSignalFuncType>(); } void start() override { isRunning_ = true; thread_ = boost::thread(&XtionGrabber::imageDepthThreadFunc, this); } void stop() override { boost::unique_lock<boost::mutex> lock(mutex_); isRunning_ = false; } bool isRunning() const override { boost::unique_lock<boost::mutex> lock(mutex_); return isRunning_; } std::string getName() const override { return "XtionGrabber"; } float getFramesPerSecond() const override { return fps_; } private: void imageDepthThreadFunc() { Xtion sensorColor(openni::SensorType::SENSOR_COLOR, 640, 480, 30); Xtion sensorDepth(openni::SensorType::SENSOR_DEPTH, 640, 480, 30); for (;;) { boost::unique_lock<boost::mutex> lock(mutex_); if (!isRunning_) return; if (signal_->num_slots() > 0) { (*signal_)(convertToXYZRGBPointCloud(sensorColor, sensorDepth)); } } } PointCloud::ConstPtr convertToXYZRGBPointCloud(Xtion& sensorColor, Xtion& sensorDepth) { boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB>> cloud(new pcl::PointCloud<pcl::PointXYZRGB>()); cloud->width = width_; cloud->height = height_; cloud->is_dense = false; cloud->points.resize(cloud->height * cloud->width); const auto color = sensorColor.getData<openni::RGB888Pixel>(); const auto depth = sensorDepth.getData<openni::DepthPixel>(); const int resX = sensorColor.getResolutionX(); for (int j = 0; j < height_; ++j) { for (int i = 0; i < width_; ++i) { const auto z = depth[j * resX + i]; float wx, wy, wz; openni::CoordinateConverter::convertDepthToWorld(sensorDepth.getStream(), i, j, z, &wx, &wy, &wz); pcl::PointXYZRGB point; const float millimeterToMeter = 0.001f; point.x = wx * millimeterToMeter; point.y = wy * millimeterToMeter; point.z = wz * millimeterToMeter; const int offsetX = 10; const int index = j * resX + (i + offsetX < width_ ? i + offsetX : width_); const auto rgb = color[index]; point.r = rgb.r; point.g = rgb.g; point.b = rgb.b; cloud->push_back(point); } } return cloud; } bool isRunning_; const int width_, height_; const int fps_; openni::VideoStream stream_; boost::signals2::signal<PointCloudSignalFuncType>* signal_; boost::thread thread_; mutable boost::mutex mutex_; }; int main() { pcl::visualization::CloudViewer viewer("OpenNI Viewer"); boost::function<XtionGrabber::PointCloudSignalFuncType> callback = [&](const XtionGrabber::PointCloud::ConstPtr& cloud) { if (!viewer.wasStopped()) { viewer.showCloud(cloud); } }; try { auto grabber = std::make_unique<XtionGrabber>(640, 480, 30); grabber->registerCallback(callback); grabber->start(); while (grabber->isRunning()) { Sleep(0); } grabber->stop(); } catch (const std::exception& e) { std::cerr << e.what() << std::endl; return EXIT_FAILURE; } return 0; }
Grabber は先程も触れましたが、Boost.Signals2 を使った実装を行っています。Boost.Signals2 は Qt のシグナル / スロットのような仕組みを実現するライブラリです。1 つのイベントに対して複数のイベントハンドラを登録できます。Qt の方は独自の C++ 拡張なのに対し、こちらは純粋な C++ で出来るわけですね。
独自の Grabber 作成するには、pcl::Grabber を継承したクラスを作成、純粋仮想関数をひと通りオーバーライドして（↑でいう override キーワードがついたもの）、後はシグナルを通じて PointCloud をスロット達に通知する仕組みを作れば良い形です。Grabber 用のシグナルを作成するには、pcl::Grabber::createSignal() を呼び出します。これはテンプレート引数で与えられた型情報をキーとしてシグナルを作成し、内部に登録します。
スロットの作成とその接続は registerCallback() で、クラスの外側から行います。
つまり、流れとしては、
- Grabber の中で適当な関数の型を元に createSignal してシグナルを作成（コンストラクタ内）
- これに同じ関数型のスロットを Boost.Function で作成して registerCallback で登録（main 関数内）
- そしてデータ更新のタイミングでシグナルを発火（Xtion::imageDepthThreadFunc 内）
となります。ポイントクラウドに対する処理は先ほどと同様です。
上記例では PointXYZRGB 群しか返さない形ですが、Grabber の利点の一つは、欲しい型のスロットが登録された時にのみ内部的な処理（ここでいうところの convertToXYZRGBPointCloud）を行える点だと思います。なのでライブラリとして整備されてると便利ですが、自前で作るのはちょっと労力が要るので、アレコレ実験したい時は Grabber を使わない最初の実装のほうがシンプルで良いかなぁと思いました。まぁそもそもビルドして OpenNIGrabber 使えよ、というのが一番正しいと思います。。
なお、冒頭の define は Boost.Signals2 関連の C4996 を抑制するためにつけています。
おわりに
PCL は難易度は高そうなので地道にやっていければと思います。
