Kinect OpenNI で3次元ポイントクラウド表示 - TK's HP

TK's HP ホーム » スポンサー広告 » kinect » Kinect OpenNI で3次元ポイントクラウド表示

スポンサーサイト

上記の広告は1ヶ月以上更新のないブログに表示されています。
新しい記事を書く事で広告が消せます。

Kinect OpenNI で3次元ポイントクラウド表示

Kinectを3次元ポイントクラウド(Point Cloud)化できたので、報告。

こんな感じです。


ROSには勝てませんが、日本だとほとんどやっている人がほとんどいないので。


以下、解説です。
一応参考サイト
OpenNI : Depth in Meters
あと、ROSの出しているkinect用カメラドライバのプログラムを参考にしました。
(ちなみにROS上でOpenNIを使えば自動的にポイントクラウドに変換してくれます)

google groupのディスカッションによると

so the depthmap gives back the Z-value for each pixel in mm
(millimeters). Thus if you want to get the full 3D position (X,Y,Z) of a
pixel (u,v), then use following formula:

XnUInt64 F_;
XnDouble pixel_size_;

// get the focal length in mm (ZPS = zero plane distance)
depth_.GetIntProperty ("ZPD", F_)

// get the pixel size in mm ("ZPPS" = pixel size at zero plane)
depth_.GetRealProperty ("ZPPS", pixel_size_)

X = (u - 320) * depth_md_[k] * pixel_size_ * 0.001 / F_;
Y = (v - 240) * depth_md_[k] * pixel_size_ * 0.001 / F_;
Z = depth_md_[k] * 0.001; // from mm in meters!

I hope this helps you,

Suat

といっており、
Kinectから得られる距離画像から得られる距離はミリメートルである。
この距離画像からXYZに変換するには、↑のまねをすれば良いとのこと。

なので変換するクラスを作ってみた。
convert.hpp

// ポイントクラウド変換クラス
class DepthParam
{
public:
XnUInt64 focalLength;
XnDouble pixelSize;
int uSize, vSize;
int uCenter;
int vCenter;
void setParam( const xn::DepthGenerator &depth )
{
// get the focal length in mm (ZPS = zero plane distance)
depth.GetIntProperty ("ZPD", focalLength );
// get the pixel size in mm ("ZPPS" = pixel size at zero plane)
depth.GetRealProperty ("ZPPS", pixelSize );

}
void setParam( const xn::DepthMetaData &depthMD )
{
uSize = depthMD.XRes();
vSize = depthMD.YRes();
uCenter = uSize / 2;
vCenter = vSize / 2;

}
bool depth2point( const XnDepthPixel* depthMap, XnPoint3D *points )
{
float P2R; // 変換定数
P2R = pixelSize * ( 1280 / uSize ) / focalLength; // 変換定数の計算
//実際の素子が1280x1024を640x480に変換している為、使うときは2倍しないといけない

float dist;
for (int v = 0; v < vSize; ++v)
{
for (int u = 0; u < uSize; ++u)
{
dist = (*depthMap) * 0.001f; //mm -> m

#if 0
// カメラ座標系での座標変換
// x right
// y bottom
// z front
points->X = dist * P2R * ( u - uCenter );
points->Y = dist * P2R * ( v - vCenter );
points->Z = dist;
#else
// ロボット座標系での座標変換
// x front
// y left
// z top
points->X = dist;
points->Y = dist * P2R * ( uCenter - u );
points->Z = dist * P2R * ( vCenter - v );
#endif
++depthMap;
++points;
}
}
return true;
}

};

公開用に編集したので、エラーがでるかもしれません。

とりあえずこれで3次元ポイントクラウドへの変換ができます。
これくらいの機能ならOpenNIのフレームワークでありそうなものですが、現在はまで実装されていないようです。
実装されるまでの間使うといった感じでしょうか。


ちなみに座標系ですが、「カメラ座標系」というのがKinectの座標系です。
ディスプレイと一緒で、右がX軸、下がY軸、前方向がZ軸です。
(ただしボーンの座標は上がY軸なので注意!座標系が混じっているのはミドルウェアとしては不適切?)

しかし、私の場合はカメラではなくロボットを基準に考えており(ロボットは地面にXY平面を張る)ので
前がX軸、左がY軸、上方向がZ軸となっています。

また、OpenNIは読み込むXMLファイルで画像の向きをコントロールできるようになっており、
サンプルではデフォルトでカメラの画像がミラー状態になっています。
座標系を定義する上では邪魔でしかないので、XMLファイルを編集して<Mirror on="false">とします
関連記事
記事に間違いなどがあればすぐに訂正いたしますので、コメントしてください。
コメント
非公開コメント

管理人のみ閲覧できます

このコメントは管理人のみ閲覧できます

2012-12-27 17:55 │ from URL

Re: Z座標について

> OpenNI,XtionPROLIVEを勉強している初心者ですが質問があります。
> dist = (*depthMap) * 0.001f; //mm -> m
> points->Z = dist;
> で定義しているZ座標の値というのはカメラからの直線距離のような気がするのですが、カメラ座標系のZの値になっているのでしょうか?間違っていたらすいません。またこの値が直線距離の値だとしたら、どのようにしたらZ座標の値(深度)を得られるでしょうか?
> ご返答よろしくお願いします。

すみません、私も不思議に思ったところなので、質問を公開させていただきます。
推測が多分に含まれているので、間違っていてもご容赦ください。

結論から言うと、Kinectはカメラからの直線距離ではなく、ユークリッド座標での値を返すようです。
平面の板などを映したときに、平面がそのまま見えるので、簡単に確認できると思います。
(カメラからの直線距離だと平面が球面に見えるはず。)

これは、Kinectが三角測量方式の距離画像センサであることが理由だと思います。
Wikipedia-三角測量
http://ja.wikipedia.org/wiki/%E4%B8%89%E8%A7%92%E6%B8%AC%E9%87%8F
同様の手法でカメラからの直線距離も求めることができますが、平面が球面に見えてしまうなどの問題があり、処理が面倒になるため使用されなかったのだと思われます。

ちなみに、TOF方式の距離画像センサならカメラからの距離が返るかと思います(未確認)。

2012-12-27 20:32 │ from TKURL

トラックバック

http://tclip.blog.fc2.com/tb.php/106-7cdd772f

上記広告は1ヶ月以上更新のないブログに表示されています。新しい記事を書くことで広告を消せます。