かみやんの技術者ブログ

主にプログラムの話です

Kinect買ったよ！

技術


やー、すっかりTwitterを使うようになってブログの更新が止まりました。他の人もそういっていたので、そういうもんなんでしょう。と言う訳で、@kamiyan 上で会いましょう〜。

そう。ブログに書かなきゃってのはKinectですよ！もうロボ野郎とか、Make野郎とか、電子工作野郎には必須でしょ！！
KinectのPC用ドライバがオープンソースで出たと聞いて、12月の5日に買ってきましたよ。アキバ、ヨドバシで13,000円ほどだったでしょうか？もちろんXbox360は持っていないし、たぶん買わないけど。
注意事項は、Xbox360+Kinectセットが売っているが、そっちだとPCでつなぐときに必要なACアダプターが入っていないらしいので、Kinectのみの方を買うこと。もうちょっと詳しく説明すると、Kinectは、付属のACアダプターの定格をみると、12V-1.08Aの電源が要る模様。Xbox360+KinectセットのKinectはUSBバスパワーのみで動くらしいが、普通 USBバスパワーは5Vなので、5Vのバスパワーで同じ電力を得ようと思ったら2.5Aぐらい電流が取り出せないといけない。普通PCのUSBポートは、500mAとか1Aぐらいが限度ではないかな（詳しくないけど）。なので、PCでつなぐ場合はAC電源が必要。で、Xbox360+KinectセットのXbox360は新しいXboxらしくバスパワーの電力が大きいのでしょう。そして旧型のXbox360の人は、Kinect単体を買ってね！（単体版は、ACアダプタ付き）という訳だ。

で、買ってきて半日ほどで、ちょっとコードを書きました。そのときのメモ。
＜2010/12/5＞

って感じです。

まずは、自分をポリゴン化してみよう〜！


上図、最初のバージョン。ポリゴンとポリゴンの隙間が多い。

上図、隙間を減らしたつもり。左手と顔の間がレインボー。

上図、隙間が減った。

上図、Depthをみて壁を除去。

KinectはRGBとDepthの4要素が、640x480ピクセル分来る。そして上記は、1ピクセルを2つの三角形で描画しているので、61万ポリゴン。61万ポリゴンがあっさり動くのは、元3D野郎として感慨深い。久しぶりにOpenGL使ったが、18年前に使ったレベルから知識が変わっていない辺りが悲しい（苦笑）。

Kinectの仕組み

Kinectは、なんで3次元スキャンができているのか。
ステレオビジョンだと2つのカメラで撮影して、特徴点を抽出して、特徴点マッチングをして、視差から三角測量で奥行きを求める。ステレオビジョンが精度が悪い理由の一つとして、特徴点マッチングでマッチしないことがある。ということと、特徴点抽出で、色が単一などの場所は特徴点がなく奥行きが分からないという２つがメイン。
そこで、Kinectでは、赤外線レーザーで特徴のあるパターンを照射。赤外線カメラでパターンを認識。パターン照射の発射位置と赤外線カメラの位置が10cmほど水平にずれているので、この視差（左目が照射、右目がカメラみたいな感じ）で、三角測量で奥行きを求めていると思われる。これにより特徴点マッチングは簡単でミスが少ない。また、カラーのカメラで認識でないので、色が単一でも関係がない。
すばらしいアイデアだ。
あとは、Kinect内にGPU相当のプロセッサが入っているのか、特徴点マッチングと、奥行き計算もハードウェアで計算されているっぽい。

Kinectで夢が膨らむ

やー、Kinectはすごい。つくばチャレンジでも来年はKinect積むロボットが増えるんじゃないでしょうか。なんせ、現在、つくばチャレンジで障害物センサーとして人気なのが北陽電機のレーザーレンジファインダ(LRF) Top-URGとかですが、これは定価が40万。スキャン方向は1次元で、得られるデータは2次元。
いっぽうKinectは、スキャンが2次元で、得られるデータは3次元。定価が15,000円ほど。
Kinectを使えば障害物回避ができそう。
ただし、Top-URGは、距離が30m飛んで、30m先でも誤差10mm以下という驚異の精度なのに対して、Kinectは恐らくセンシング範囲が5mとか10mとか（屋内で遊ぶ用だからね）であろうと思われるし、誤差は、ステレオビジョンと似た仕組みと思われるので遠いところほど誤差がでるのではないかな。ま、でも近く3m以下とかさえ分かれば障害物回避できるので、ロボットの価格がぐっと下げられる。

Kinectをつかったモーションキャプチャとか、Kinectをルンバに積んだとか、Kinectをクアッドコプター（AR Droneみたいなやつ）に積んだとか、どんどんニュースが届きます。
楽しい時代になりました。

KinectApp Source Code

僕がさくっと半日で書いたKinectApp.hとKinectApp.cppを公開します。BSDライセンスで公開します。
プロジェクトファイルは、こちら：http://www.asahi-net.or.jp/~qs7e-kmy/robot/KinectTest20101218.zip

KinectApp.h

#ifndef __KINECT_APP_H__
#define __KINECT_APP_H__



#define DEG2RAD(x)  ((x)*M_PI/180.0)

class KinectApp: public Kinect::KinectListener
{
private:
	static const int KINECT_W=Kinect::KINECT_DEPTH_WIDTH;
	static const int KINECT_H=Kinect::KINECT_DEPTH_HEIGHT;
	static const char FLAG_POINT=(1<<0);//点あり
	static const char FLAG_RIGHT=(1<<1);//右エッジあり
	static const char FLAG_DOWN=(1<<2);//下エッジあり
	Kinect::KinectFinder* mKinectFinder;//KinectFinder
	Kinect::Kinect* mKinect;//Kinect
	float mYaw;//カメラヨー角(degree)
	float mPitch;//カメラピッチ角(degree)
	float mDistance;//カメラ距離
	unsigned short mDepthBuffer[KINECT_W * KINECT_H];//depth buffer
	unsigned char mColorBuffer[Kinect::KINECT_COLOR_WIDTH * Kinect::KINECT_COLOR_HEIGHT * 3];//color buffer
	float mFar;//カットする後ろの距離
	float mDepthFact;//奥行き方向のスケール

	//コンストラクタ
	KinectApp(){
		mKinectFinder=NULL;
		mKinect=NULL;
		mYaw=0.0f;
		mPitch=0.0f;
		mDistance=400.0f;
		memset(mDepthBuffer,0,sizeof(mDepthBuffer));
		memset(mColorBuffer,0,sizeof(mColorBuffer));
		mDepthFact=600.f;
		mFar=mDepthFact/4.0f;
	}
public:
	//インスタンス取得（シングルトン）
	static KinectApp* GetApp();

	//デストラクタ
	~KinectApp(){
		if(mKinect!=NULL){
			mKinect->SetLedMode(Kinect::Led_Off);
			mKinect->RemoveListener(this);
			//delete mKinect;//mKinectはKinectFinderが削除するのでアプリは削除しない
		}
		mKinect=NULL;
		if(mKinectFinder!=NULL){
			delete mKinectFinder;
		}
	}

	//Kinectの初期化
	bool InitKinect(){
		mKinectFinder=new Kinect::KinectFinder();
		if(mKinectFinder==NULL){
			printf("error init KinectFinder...\n");
			return false;
		}

		if (mKinectFinder->GetKinectCount() < 1)
		{
			printf("Unable to find Kinect devices... Is one connected?\n");
			return false;
		}

		mKinect = mKinectFinder->GetKinect();
		if (mKinect == NULL) {
			printf("error getting Kinect...\n");
			return false;
		}
	
		// register the listener with the kinect. Make sure you remove the 
		// listener before deleting the instance! A good place to unregister 
		// would be your listener destructor.
		mKinect->AddListener(this);

		// SetMotorPosition accepts 0 to 1 range
		mKinect->SetMotorPosition(1);
	
		// Led mode ranges from 0 to 7, see the header for possible values
		mKinect->SetLedMode(Kinect::Led_Yellow);
	
		// Grab 10 accelerometer values from the kinect
		float x,y,z;
		for (int i =0 ;i<10;i++){
			if (mKinect->GetAcceleroData(&x,&y,&z))
			{
				printf("accelerometer reports: %f,%f,%f\n", x,y,z);
			}
			Sleep(5);
		}
		return true;//success
	}

	//glut 描画イベント
	static void glutOnDraw();
	//glut キーイベント
	static void glutOnKeyPress(unsigned char key, int x, int y);

	//OpenGLの初期化
	bool InitDisplay(int argc, char **argv){
		GLfloat light_diffuse[] = {1.0f, 1.0f, 1.0f, 1.0f};  /* Red diffuse light. */
		GLfloat light_position[] = {1.0f, 1.0f, 10.0f, 0.0f};  /* Infinite light location. */
		GLfloat lightAmbient[]  = {0.2f, 0.2f, 0.2f, 1.0};
		
		glutInit(&argc, argv);
		glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);
		glutCreateWindow("KinectApp");
		glutReshapeWindow(800,600);
		glutKeyboardFunc(KinectApp::glutOnKeyPress);
		glutDisplayFunc(KinectApp::glutOnDraw);

		/* Enable a single OpenGL light. */
		glLightfv(GL_LIGHT0, GL_AMBIENT, lightAmbient);
		glLightfv(GL_LIGHT0, GL_DIFFUSE, light_diffuse);
		glLightfv(GL_LIGHT0, GL_POSITION, light_position);
		glEnable(GL_LIGHT0);
		glEnable(GL_LIGHTING);

		/* Use depth buffering for hidden surface elimination. */
		glEnable(GL_DEPTH_TEST);

		/* Setup the view of the cube. */
		glMatrixMode(GL_PROJECTION);
		gluPerspective( /* field of view in degree */ 90.0,
		/* aspect ratio */ 800.f/600.f,
		/* Z near */ 1.0, /* Z far */ 1000.0f);
		glMatrixMode(GL_MODELVIEW);
		gluLookAt(0.0, 0.0, mDistance,  /* eye is at (0,0,5) */
			0.0, 0.0, 0.0,      /* center is at (0,0,0) */
			0.0, 1.0, 0.);      /* up is in positive Y direction */
		return true;//success
	}

	//Kinect切断イベント
	virtual void KinectDisconnected(Kinect::Kinect* kinect) {
		printf("Kinect disconnected!\n");
	}

	//デプスバッファの最小値最大値を求める
	void calculateMinMaxDepth(unsigned short* oMin, unsigned short* oMax){
		unsigned short min=65535;
		unsigned short max=0;
		for(int j=0;j<KINECT_H;j++){
			for(int i=0;i<KINECT_W;i++){
				unsigned short d=mDepthBuffer[j*KINECT_W+i];
				if(d<min){
					min=d;
				}
				if(d>max){
					max=d;
				}
			}
		}
		*oMin=min;
		*oMax=max;
	}
		
	// 距離データ受信イベント
	virtual void DepthReceived(Kinect::Kinect* kinect) {
		kinect->ParseDepthBuffer();									
		// kinect->mDepthBufferが今だけ有効
		memcpy(this->mDepthBuffer,kinect->mDepthBuffer,sizeof(this->mDepthBuffer));
		glutPostRedisplay();
	}
		
	// カラーデータ受信イベント
	virtual void ColorReceived(Kinect::Kinect* kinect) {
		kinect->ParseColorBuffer();
		// kinect->mColorBufferが今だけ有効
		memcpy(this->mColorBuffer,kinect->mColorBuffer,sizeof(this->mColorBuffer));
	}
		
	//音声受信イベント
	virtual void AudioReceived(Kinect::Kinect* kinect) {
	}

	//キーイベント
	void OnKeyPress(unsigned key){
		KinectApp* app = KinectApp::GetApp();
		printf("key=%d\n",key);
		switch (key) {
			case 27://ESCキーのとき
				delete app;
				exit(0);
			case 97://a
				mYaw+=10.0f;
				glutPostRedisplay();
				break;
			case 100://d
				mYaw-=10.0f;
				glutPostRedisplay();
				break;
			case 119://w
				mPitch+=10.f;
				glutPostRedisplay();
				break;
			case 115://s
				mPitch-=10.0f;
				glutPostRedisplay();
				break;
			case 102://f mDepthFar
				mFar += 10.0f;
				printf("mFar=%7.0f\n",mFar);
				break;
			case 103://g mDepthFar
				mFar -= 10.0f;
				printf("mFar=%7.0f\n",mFar);
				break;
			
		}
	}

	//3Dスクリーンの描画(隣のピクセルとくっついている)
	void draw3DScreen2(void){
		unsigned short min=482,max=2047;
		//calculateMinMaxDepth(&min,&max);
		float fDiv=(float)(max-min);
		//デプスを求める
		float zFact = 600.0f;
		float* fDepth=(float*)malloc(sizeof(float)*KINECT_W*KINECT_H);
		for(int j=0;j<KINECT_H;j++){
			for(int i=0;i<KINECT_W;i++){
				int d=mDepthBuffer[KINECT_W*j+i];
				fDepth[KINECT_W*j+i] 
					= -zFact*(float)(d-min)/fDiv+zFact/2.0f;
			}
		}
		//QUAD描画
		float cx=(float)(KINECT_W)/2.0f;
		float cy=(float)(KINECT_H)/2.0f;
		int dx=-25;//キャリブレーション
		int dy=10;
		for(int j=0;j<KINECT_H-1;j++){
			for(int i=0;i<KINECT_W-1;i++){
				//デプスチェック
				if(fDepth[KINECT_W* j   +i]<mFar){
					continue;
				}
				float depth[4];
				depth[0]=fDepth[KINECT_W* j   +i];
				depth[1]=fDepth[KINECT_W*(j+1)+i];
				depth[2]=fDepth[KINECT_W*(j+1)+i+1];
				depth[3]=fDepth[KINECT_W* j   +i+1];
				float min=60000.0f;
				float max=-60000.0f;
				for(int k=0;k<4;k++){
					min=min(depth[k],min);max=max(depth[k],max);
				}
				if(max-min>50.0f){//前後に伸びているポリゴンは表示しない
					continue;
				}
				//カラー取得
				unsigned char cR,cG,cB;
				if(i+dx<0 || i+dx>=Kinect::KINECT_COLOR_WIDTH ||
					j+dy<0 || j+dy>=Kinect::KINECT_COLOR_HEIGHT){//カラーバッファの外のとき
						cR=cG=cB=0;
				} else {//中のとき
					unsigned char* p = &mColorBuffer[((j+dy)*Kinect::KINECT_COLOR_WIDTH+(i+dx))*3];
					cR=*(p++), cG=*(p++), cB=*(p++);
				}
				float color[3];
				color[0]=cR/255.0f;
				color[1]=cG/255.0f;
				color[2]=cB/255.0f;
				//位置計算
				float x[4],y[4];
				x[0]=(float)i     -cx; y[0]=(float)-j     +cy;
				x[1]=(float)i     -cx; y[1]=(float)-j-1.0f+cy;
				x[2]=(float)i+1.0f-cx; y[2]=(float)-j-1.0f+cy;
				x[3]=(float)i+1.0f-cx; y[3]=(float)-j     +cy;
				//描画
				glBegin(GL_QUADS);
				glMaterialfv(GL_FRONT,GL_DIFFUSE,color);
				glNormal3f(0.0f,0.0f,1.0f);
				glVertex3f(x[0],y[0],depth[0]);
				glVertex3f(x[1],y[1],depth[1]);
				glVertex3f(x[2],y[2],depth[2]);
				glVertex3f(x[3],y[3],depth[3]);
				glEnd();
			}
		}
		free(fDepth);
	}

	//描画イベント
	void OnDraw(){
		glClearColor(0.0, 0.0, 0.0, 1.0);
		glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

		glMatrixMode(GL_MODELVIEW);
		glLoadIdentity();
		double yaw = DEG2RAD(mYaw);
		double pitch = DEG2RAD(mPitch);
		double r = mDistance;
		gluLookAt(r*sin(yaw)*cos(pitch), r*sin(pitch), r*cos(yaw)*cos(pitch),  /* eye is at (0,0,5) */
			0.0, 0.0, 0.0,      /* center is at (0,0,0) */
			0.0, 1.0, 0.);      /* up is in positive Y direction */
		draw3DScreen2();

		glutSwapBuffers();
	}
};

#endif//__KINECT_APP_H__

KinectApp.cpp

// KinectApp.cpp : アプリクラス
//

#include "stdafx.h"
#include "Vector.h"
#include "KinectApp.h"

//グローバル変数
static KinectApp* g_app=NULL;

//インスタンス取得（シングルトン
KinectApp* KinectApp::GetApp(){
	if(g_app==NULL){
		g_app = new KinectApp();
	}
	return g_app;
}

//glut 描画イベント
void KinectApp::glutOnDraw(void)
{
	KinectApp::GetApp()->OnDraw();
}


//glut キーイベント
void KinectApp::glutOnKeyPress(unsigned char key, int x, int y)
{
	KinectApp::GetApp()->OnKeyPress(key);
}


//エントリーポイント
int _tmain(int argc, char **argv)
{
	KinectApp* app = KinectApp::GetApp();
	app->InitDisplay(argc,argv);
	app->InitKinect();
	glutMainLoop();
	return 0;
}

URL:この記事が参考になったと言う方、ibisMailもみてやってくださいm(_ _)m こちら