OpenNIを利用したアプリ：跳ねるボール

はじめに

OpenNIとXtion Pro Liveを使って、こんなアプリを作りました。

1. Xtion Pro Liveから取り込んだデプスデータから３次元点群（ポイントクラウド）を作成する。
2. Point Cloud Library(PCL)を用いて、各点の法線ベクトルを算出する。
3. 仮想ボールを投入し、環境内の障害物と衝突する様子をシミュレーションする。

法線ベクトルの導出は、一度だけ行います。その後のフレームでは同じデータを使います。描画はOpenGLで行います。

開発環境

1. Mac OS X 10.7.4
2. プロセッサ：3.06 GHz Intel Core 2 Duo
3. メモリ：4GB
4. Xcode4.3.3

主要ライブラリ

1. boost-1.50.0
2. pcl-1.6.0
3. OpenNI-Bin-Dev-MacOSX-v1.5.4.0

ソース

こちらです。自分のマシーンでしか動作確認していません。

コンパイルオプション

Header Search Path:

パス	目的
/usr/include/ni/	OpenNIヘッダー
/opt/local/include/	boostヘッダー
/usr/local/include/pcl-1.6/	PCLヘッダー
/opt/local/include/eigen3/	eigenヘッダー(PCLがこれに依存している）
/opt/local/include/vtk-5.10/	vtkヘッダー(PCLがこれに依存している）

Library Search Path:

パス	目的
/opt/local/lib	boostライブラリ
/usr/local/lib	PCLライブラリ

その他、OpenGLを使用するため、-framework OpenGLなどを設定しています。詳細はソースをご覧ください。

使い方

実行ファイ名は、OpenNiViewerです。引数なしで実行すると以下を出力します。 --configにSamplesConfig.xmlを指定し実行すると、窓が開き、Xtion Pro Liveの映像が映し出されます。SamplesConfig.xmlはソースに同封しました。--width/--heightの値はデフォルト値でしか動作確認していません。 窓にフォーカスがある状態で、キーボードから「p」を打つと、法線ベクトルの計算が始まります。 "Normals is now being extracted."は計算中、"Normals has been extracted."は計算終了を表します。続いて、「i」を打つと、環境内にボールが投入されます。ボールと障害物の相互作用がリアルタイムで描画されます。 「esc」を打つと終了します。

ソースの概要

OpenGLの視点

以下のように設定しています。本アプリの長さの単位は全てmmです。 8行目の引数は使用する環境に合わせる必要があります。

ポイントクラウドへの変換

OpenNiDevice::create_cloudで行います。カラー画像描画のためのポイントクラウド（cloud_）と法線ベクトル算出用のポイントクラウド（cloud_xyz_）の２種類を生成しています。 26,27,28行目でRGB値を設定しています。33行目でデプスから３次元座標への変換を行っています。34行目で法線用のデータを設定しています。

カラー画像の描画

カラー画像用のポイントクラウド（cloud_）は、OpenGLのglInterleavedArraysに以下（5行目）のように渡されます。

法線ベクトルの計算

法線算出用ポイントクラウド（cloud_xyz_）はOpenNiDevice::calculate_normalsで処理されます。 6行目のinitializeの内容は以下の通りです。 9行目で近傍探索の範囲を指定しています。この引数は環境に合わせて変える必要があります。この範囲内にある点から平面を導出し、その法線ベクトルを求めます。 法線ベクトルの計算は「p」を打つと一度だけ行います。Xtion Pro Liveを動かした場合は、再度「p」を打つ必要があります。

ボールの配置

ボールはOpenNiViewer::register_ballsで配置しています。 25行目でボールの半径を指定します。31行目で初期位置を、32行目で初期速度を、33行目で加速度を設定します。 これらも環境に合わせて変える必要があります。

壁の配置

画面手前に壁を設定しています。環境に合わせてパラメータを変える必要があります。

法線ベクトルの検索

毎フレーブごとに、ボール近傍にある法線ベクトルを検索する必要があります。この検索には、kd-treeを使います。 8行目のkdtree_はpcl::KdTreeFLANN<pcl::PointXYZ> のオブジェクトです。このクラスは、近傍点の探索に失敗するとnanを返します。なので、12行目でこれを見ています（このあたりの実装はもっと上手いやり方があるかもしれません）。

Derivation of Normal Vectors from Point Cloud

In this page, I describe how to derive normal vectors from a point cloud. A simple explanation is provided in the tutorial of the Point Cloud Library(PCL). According to the page, there are two approaches to calculate normal vectors from a point cloud:

1. Determine a surface from a point cloud, and calculate a normal vector of a tangent plane.
2. Calculate a normal vector from a point cloud directly.

The tutorial focuses on the latter case, and introduces one of the most simple methods in the case:

1. Consider a query point in a point cloud.
2. Consider k points in the vicinity of the query point.
3. Determine a plane by means of a least-square plane fitting with the query point and the k neighbors.
4. Extract a normal vector of the plane.

The tutorial tells us that the problem results in an analysis of the eigenvectors and eigenvalues of a covariance matrix created from the nearest neighbors of the query point, and shows only the equation of the eigenvalue problem. I'm interested in the deriving process of the equation. In this page, I give you a detailed description about it.
A point on a plane satisfies the following equation,
,
where is a normal vector of the plane, and is a constant term. The sum of squared residuals that we have to minimize is
,
where indicates an element of a set of neighbors of the query point. An equation of a plane multiplied by a constant represents the same plane as the original equation. To eliminate this indefiniteness, we set the condition that the norm of the normal vector is 1. The condition is introduced into the sum of squared residuals by using the Lagrange multiplier as
.
First, we differentiate it partially with respect to and get
.
The equation is rewritten as
,
where is a centroid of the neighbors. By substituting this formula into the equation , we obtain
.
Differentiating it partially with respect to yields
.
The deformation of the equation reduces to

.
Now we have the equation of the eigenvalue problem of the covariance matrix created from the nearest neighbors of the query point.

This page tells us that the eigenvector corresponding to the smallest eigenvalue approximates the normal vector of the plane. We can confirm the fact as follows.
The sum of squared residuals is
,
where is a set of nearest neighbors, and is a centroid of them.
Using , we can rewrite as, \begin{eqnarray} F(\vec{n})&=&\vec{n}^{T}\;Q\;\vec{n} \nonumber \\ &=&\lambda\;\vec{n}^{T}\cdot\vec{n} \nonumber \\ &=&\lambda \nonumber \end{eqnarray} We used the condition that the norm of the vector is 1.
So we can now understand that in order to minimize , we have to select the smallest eigenvalue.

Point Cloudからの法線導出2

ここの続きです。固有値方程式から求められる3つの固有値のうち、最小の固有値に対応する固有ベクトルが平面の法線ベクトルであると、ここに書いてあります。これを確かめてみます。

最小自乗式は次式で与えられました。 \begin{equation} F(\vec{n})=\sum\limits_{i}\;\|\vec{n}^{T}\cdot\left(\vec{p}_{i}-\vec{p}_{a}\right)\|^2 \label{least} \end{equation} ここで、$\vec{p}_{i}$ は近傍点群、$\vec{p}_{a}$ は近傍点群の重心を表します。 そして、解くべき固有値方程式は次式でした。 \begin{equation} Q\;\vec{n}=\lambda\;\vec{n} \label{eigen} \end{equation} ここで、$Q$ は次式で定義されます。 \begin{equation} Q=\sum\limits_{i}\left(\vec{p}_{i}-\vec{p}_{a}\right)^{\;} \left(\vec{p}_{i}-\vec{p}_{a}\right)^{T} \nonumber \end{equation} $Q$ を使って、$F$ を書き直すと \begin{eqnarray} F(\vec{n})&=&\vec{n}^{T}\;Q\;\vec{n}\nonumber \\ &=&\lambda\;\vec{n}^{T}\cdot\vec{n} \nonumber \\ &=&\lambda \nonumber \end{eqnarray} となります。ここで、$\vec{n}$ の長さが1であることを使いました。 以上から $F$ を最小にするのは、3つの固有値のうち最小の固有値であることが分ります。

　ところで、ここまでの議論では、式(\ref{least})を最小にすることにより法線を求めてきましたが、 別の見方をすることができます。すなわち、近傍点群 $\vec{p}_{i}$ の重心を原点に取り直し、この点群に対し 主成分分析を行うと固有値方程式(\ref{eigen})に帰着します。そして、一番寄与の小さな向きが法線であるとみなします。

OpenNI Viewer

Viewer applications residing in the sample directory of the OpenNI package do not visualize a 3D point cloud, but display a 2D projection image. I implemented the viewer which visualizes the 3D point cloud with use of the OpenNI and OpenGL on Xcode4.3.3. The compile options are as follows:

1. Other Linker Flags: /usr/lib/libOpenNI.dylib -framework OpenGL -framework GLUT -lboost_program_options
2. Header Search Paths: /usr/include/ni /opt/local/include
3. Library Search Paths: /opt/local/lib

The paths /opt/local/include and /opt/local/lib are for using the boost libraries. When running the executable file without any arguments, the following output is displayed:
To run the viewer, you have to pass the path to the SamplesConfig.xml. I verified my viewer with only default parameters. My source code is here. I summarized the installation procedures of the OpenNI here.

Bouncing Ball

I visualized bouncing balls using OpenGL. See a figure below.

I placed walls at front, back, left, right, and floor. Three balls repeatedly collide with walls and each other. Their kinetic energies are conserved(i.e. completely-elastic collision).
I implemented it in C++ with the OpenGL and boost libraries on Xcode4.3.3. The compile option is as follows:

1. Other Linker Flags: -framework OpenGL -framework GLUT
2. Header Search Paths: /opt/local/include

The path /opt/local/include is for including the boost header files. My source code is here.

Point Cloudからの法線導出

Point Cloud（点群）の各点の法線ベクトルを求めたい。Point Cloud Library(PCL)のチュートリアルに簡単な説明がある。それによると、点群から法線を求める方法には2つある。

1. 点群から曲面を求め、その曲面から各点の法線を計算する。
2. 点群から直接法線を計算する。

チュートリアルでは後者を取り上げ、その中で最も簡単な方法として以下を紹介している。

1. ある点Aを考える。
2. 点Aの近傍k個の点を考える。
3. 点Aと近傍k個の点を使い、最小自乗法により平面を決定する。
4. その平面の法線ベクトルを求める。

そして、この問題は、近傍点群から構成される共分散行列の固有値問題に帰着すると結ばれ、その固有値問題の数式だけが書かれていた。 導出過程が気になります。以下これを導いてみます。

平面上の点 は次式を満たします。

ここで、 は平面の法線ベクトル、 は定数項です。 従って、最小にすべき自乗式は以下のようになります。

ここで、 は近傍点群を表します。平面の式には定数倍だけの不定性があります。 これを取り除くため、法線ベクトルの大きさを1に制限します。この条件をLagrangeの未定乗数により最小自乗式に導入します。

最初に で偏微分します。

変形して

を得ます。ここで、 は近傍点群の重心を表します。これを最初の に代入すると以下を得ます。

これを で偏微分します。

これを変形すると


となります。近傍点群から構成される共分散行列の固有値問題に帰着しました。追記

余談：数式は、Google Chart APIを使ってTexで記述しました。ベースラインがずれているのが気になります。

Point Cloud Libraryの再コンパイル

macportsのboostのバージョンが1.50.0に上がって以降、Point Cloud Library(PCL)のio周りでエラーが発生するようになった。調べてみると同じ問題についてここでも議論されていた。pcl-1.5はboost::filesystemのversion2を使って実装されている。boost-1.50.0以降、version2がサポートされなくなったことが原因らしい。すでに修正されていると書かれているので、pcl-1.6のソースをここからダウンロードし、コンパイルを行った。 make insallを行う前にpcl-1.5の後始末をしておく。 io周りのエラーは解消した。

跳ねるボール

運動方程式にしたがって跳ねるボールを視覚化してみました（下図）。

床、奥、手前、左右に壁を設定してあります。３つのボールが壁やお互い同士と衝突を繰り返します。運動エネルギーは衝突の前後で保存されます（完全弾性衝突）。実装環境はXcode4.3.3、使用したライブラリは、OpenGL、boostです。 コンパイルオプションは以下の通り。

1. Other Linker Flags: -framework OpenGL -framework GLUT
2. Header Search Paths: /opt/local/include

/opt/local/includeはboostをつかうための設定です。 ソースはこちら。

OpenNIのViewer

OpenNIのサンプルにあるViewerアプリは、３次元空間内の点を２次元平面上に射影し、それをテキスチャとして、画面に貼付けている。３次元空間内の点のまま表示できるViewerを作成した。実装環境は、Xcode4.3.3である。
コンパイルオプションは以下の通り。

1. Other Linker Flags: /usr/lib/libOpenNI.dylib -framework OpenGL -framework GLUT -lboost_program_options
2. Header Search Paths: /usr/include/ni /opt/local/include
3. Library Search Paths: /opt/local/lib

/opt/local/includeと/opt/local/libはboostを使うための設定である。実行ファイルを引数なしで実行すると以下のコメントが出力される。
--configにSamplesConfig.xmlを指定して実行する。それ以外の引数はデフォルト値のみ動作確認した。ソースはこちらです。 OpenNiViewer.tar.gz OpenNI等のインストール方法は、こちらにまとめてあります。