OpenCVのSIFT,SURF,FASTの実験

OpenCVの特徴量について、最新のものをキャッチアップできてないので実験。OpenCVはv2.3.1なので最新でもないですが。比較のためにキャプチャしたものを貼っておく。理論的なことは、
@sonsonさんのここらへんが詳しいかも。実装についてはopencv.jpを参照のこと。

• SURF

• SIFT

• FAST

Kinect SDKのソースコードを書き下した。

KinectSDKを使ってC++ソースコードを書いたので紹介します。使いたい方はご自由にどうぞ

• 概要

Kinectの深度画像、カメラ画像、スケルトン情報等を取得するKinectSDKのラッパーです。
Windows 7 + MS Visual Studio C++ 2010 expressで開発しました。
OpenCVをよく使うのでC/C++で書きました。
音源方位、音声認識は今後対応予定。
CKinectUtilというクラスをmain()から使うプログラムを以下に載せます。スケルトンを表示する部分は省いてます。
LogCommon.hはオレオレ用ログです。消してご利用ください。

• KinectUtil.h

/**
 * @file KinectUtil.h
 *  Kinect for Windows SDK v1.0
 *  OpenCV v2.3.1
 */

#pragma once

#include <opencv/cv.h>
#include <memory>
#include <windows.h>
#include <NuiApi.h>
#include <vector>

namespace Utility
{
	/**
	 * @brief 深度画像上の座標情報
	 */
	typedef class CDepthPoint
	{
	public:
		float fX;
		float fY;
		USHORT usDepth;
		static const int CAP_WIDTH  = 320;
		static const int CAP_HEIGHT = 240;
	public:
		CDepthPoint();
		CDepthPoint(float x, float y, USHORT depth);
		virtual ~CDepthPoint();
		CDepthPoint(const CDepthPoint& obj);
	} CDptPoint;
	/**
	 * @brief トラッキング情報
	 */
	class CTrackingInfo
	{
	public:
		// ID
		long lTrackingID;
		// 各関節点座標
		std::vector<CDptPoint> skeletalPoints;
	public:
		CTrackingInfo();
		virtual ~CTrackingInfo();
		CTrackingInfo(const CTrackingInfo& obj);
	};
	/**
	 * @class CKinectController
	 * @brief Kinect制御
	 *  OpenCV型依存
	 */
	typedef class CKinectController
	{
	public:
		//---------------------------------------------------------
		// API
		//---------------------------------------------------------
		/**
		 * @brief コンストラクタ
		 */
		CKinectController();
		/**
		 * @brief デストラクタ
		 */
		virtual ~CKinectController();
		/**
		 * @brief 初期化
		 *  キャプチャを開始する
		 * @return 成功:0 / 失敗:0以外
		 */
		long Initialize();
		/**
		 * @brief 終了処理
		 *  キャプチャを終了する
		 * @return 成功:0 / 失敗:0以外
		 */
		long Finalize();
		/**
		 * @brief 次フレームを取得する
		 * @return 成功:0 / 失敗:0以外
		 */
		long GrabFrame();
		/**
		 * @brief Depth画像を取得する
		 *  1チャンネル uint16幅
		 * @return 成功:0 / 失敗:0以外
		 */
		long GetDepthFrame(IplImage*& pImage);
		/**
		 * @brief RGB画像を取得する
		 *  4チャンネル uchar幅
		 * @return 成功:0 / 失敗:0以外
		 */
		long GetRGBFrame(IplImage*& pImage);
		/**
		 * @brief トラッキングID分布画像を取得
		 * @return 成功:0 / 失敗:0以外
		 */
		long GetTrakingIdFrame(IplImage*& pImage);
		/**
		 * @brief トラッキング情報を取得
		 * @return 成功:0 / 失敗:0以外
		 */
		long GetTrakingInfo(std::vector<CTrackingInfo>& trackingInfoList);
		/**
		 * @brief ピッチ駆動
		 * @return 成功:0 / 失敗:0以外
		 */
		long SetElevationAngle(float fAgl);
		/**
		 * @brief ピッチ現在角度を取得
		 * @return 成功:0 / 失敗:0以外
		 */
		long GetElevationAngle(float& fAgl);
	public:
		static long Depth16bitTo8bitImage(const IplImage* srcImage, IplImage*& dstImage);
	protected:
		//---------------------------------------------------------
		// 操作
		//---------------------------------------------------------
		// 次フレーム取得
		long UpdateFrame();
		// 現フレーム破棄
		long ReleaseFrame();
		// Depth画像バッファリング
		long BufferDepthImage(const NUI_IMAGE_FRAME* pSrcFrame, const NUI_LOCKED_RECT& lockRect, IplImage*& pDstImage, IplImage*& pUserIdImage);
		// RGB画像バッファリング
		long BufferRgbImage(const NUI_IMAGE_FRAME* pSrcFrame, const NUI_LOCKED_RECT& lockRect, IplImage*& pDstImage);
		// LockRectから画素値を取り出す
		long PixelVal(USHORT& val, int x, int y, const NUI_IMAGE_FRAME* pSrcFrame, const NUI_LOCKED_RECT& lockRect);
	protected:
		//---------------------------------------------------------
 		// 属性
		//---------------------------------------------------------
		//------ RGB関連 ------
		HANDLE m_hImgStreamEvent;
		HANDLE m_hImgStreamHandle;
		const NUI_IMAGE_FRAME* m_pImgFrame; // フレームワーク側のバッファ.
		NUI_LOCKED_RECT m_ImgLckRect;
		IplImage* m_pRgbImageBuff;
		//------ DEPTH関連 ------
		HANDLE m_hDepStreamEvent;
		HANDLE m_hDepStreamHandle;
		const NUI_IMAGE_FRAME* m_pDepFrame; // フレームワーク側のバッファ.
		NUI_LOCKED_RECT m_DepLckRect;
		IplImage* m_pDepImageBuff;
		//------ SKELTON/USER TRACKING関連 ------
		HANDLE m_hSklStreamEvent;
		NUI_SKELETON_FRAME m_SklFrame;
		IplImage* m_pUserImageBuff;
	protected:
		//---------------------------------------------------------
		// 定数
		//---------------------------------------------------------
		/**
		 * @brief フレーム解放-->次フレーム取得の実施を待つ時間(unit:msec).
		 */
		static const DWORD MSEC_REFRESH_WAIT_TIME = 50;

	} CKinectCtrl;
}

• KinectUtil.cpp

/**
 * @file KinectUtil.cpp
 */

#include "KinectUtil.h"
#include "LogCommon.h"
#include <vector>
#define _USE_MATH_DEFINES
#include <math.h>

#define LOG INF_LOG

namespace Utility
{
	long CKinectController::GrabFrame()
	{
		// フレーム破棄.
		long lRes = ReleaseFrame();
		if(lRes != 0){
			LOG(" ERR: 前フレームリリース失敗. res=%ld.", lRes);
		}
		Sleep(MSEC_REFRESH_WAIT_TIME); // 待つ.
		// 次フレーム取得.
		lRes = UpdateFrame();
		if(lRes != 0){
			LOG(" ERR: 次フレーム取得失敗. res=%ld.", lRes);
			return lRes;
		}
		// RGB画像バッファリング.
		if(m_pRgbImageBuff != NULL){
			cvReleaseImage(&m_pRgbImageBuff);
			m_pRgbImageBuff = NULL;
		}
		lRes = BufferRgbImage(m_pImgFrame, m_ImgLckRect, m_pRgbImageBuff);
		if(lRes != 0){
			LOG(" ERR: RGB画像バッファリング失敗. res=%ld.", lRes);
			return lRes;
		}
		// DEPTH画像バッファリング.
		if(m_pDepImageBuff != NULL){
			cvReleaseImage(&m_pDepImageBuff);
			m_pDepImageBuff = NULL;
		}
		lRes = BufferDepthImage(m_pDepFrame, m_DepLckRect, m_pDepImageBuff, m_pUserImageBuff);
		if(lRes != 0){
			LOG(" ERR: DEPTH画像バッファリング失敗. res=%ld.", lRes);
			return lRes;
		}
		return 0;
	}
	long CKinectController::GetRGBFrame(IplImage*& pImage)
	{
		if(m_pRgbImageBuff == NULL){
			LOG(" ERR: キャプチャがバッファされてない. Capture is Not Buffering!");
			return 1;
		}
		pImage = cvCloneImage( m_pRgbImageBuff );
		if(pImage == NULL){
			LOG(" ERR: cvCloneImage() returns NULL.");
			return 2;
		}
		return 0;
	}
	long CKinectController::GetDepthFrame(IplImage*& pImage)
	{
		if(m_pDepImageBuff == NULL){
			LOG(" ERR: キャプチャがバッファされてない. Capture is Not Buffering!");
			return 1;
		}
		pImage = cvCloneImage( m_pDepImageBuff );
		if(pImage == NULL){
			LOG(" ERR: cvCloneImage() returns NULL.");
			return 2;
		}
		return 0;
	}
	long CKinectController::GetTrakingIdFrame(IplImage*& pImage)
	{
		if(m_pUserImageBuff == NULL){
			LOG(" ERR: キャプチャがバッファされてない. Capture is Not Buffering!");
			return 1;
		}
		pImage = cvCloneImage( m_pUserImageBuff );
		if(pImage == NULL){
			LOG(" ERR: cvCloneImage() returns NULL.");
			return 2;
		}
		return 0;
	}
	long CKinectController::Initialize()
	{
		// 初期化.
		static const DWORD dwFlag =	NUI_INITIALIZE_FLAG_USES_DEPTH_AND_PLAYER_INDEX | 
																NUI_INITIALIZE_FLAG_USES_SKELETON | 
																NUI_INITIALIZE_FLAG_USES_COLOR |
																NUI_INITIALIZE_FLAG_USES_AUDIO ;
		HRESULT hRes = NuiInitialize(dwFlag);
		if(hRes != S_OK){
			LOG(" ERR: Kinect初期化失敗. NuiInitialize() returns %d.", hRes);
			return 1;
		}
		// RGBストリームオープン.
		hRes = NuiImageStreamOpen(NUI_IMAGE_TYPE_COLOR, NUI_IMAGE_RESOLUTION_640x480, 0, 2, m_hImgStreamEvent, &m_hImgStreamHandle);
		if(hRes != S_OK){
			LOG(" ERR: RGBストリームオープン失敗. NuiImageStreamOpen() returns %d.", hRes);
			return 2;
		}
		// Depthストリームオープン.
		hRes = NuiImageStreamOpen(NUI_IMAGE_TYPE_DEPTH_AND_PLAYER_INDEX, NUI_IMAGE_RESOLUTION_320x240 , 0, 2, m_hDepStreamEvent, &m_hDepStreamHandle);
		if(hRes != S_OK){
			LOG(" ERR: Depthストリームオープン失敗. NuiImageStreamOpen() returns %d.", hRes);
			return 3;
		}
		// スケルトントラッキングオン設定
		hRes = NuiSkeletonTrackingEnable(m_hSklStreamEvent, 0);
		if(hRes != S_OK){
			LOG(" スケルトントラッキングオン設定失敗. NuiSkeletonTrackingEnable() returns %ld.", hRes);
			return 4;
		}

		return 0;
	}
	long CKinectController::Finalize()
	{
		NuiSkeletonTrackingDisable();

		// 終了処理.
		NuiShutdown();

		return 0;
	}
	long CKinectController::UpdateFrame()
	{
		// RGBフレーム取得.
		HRESULT hRes = NuiImageStreamGetNextFrame(m_hImgStreamHandle, 0, &m_pImgFrame);
		if(hRes != S_OK){
			LOG(" ERR: RGB次フレーム取得失敗. NuiImageStreamGetNextFrame() returns %d.", hRes);
			return 1;
		}
		// RGBバッファをロック.
		hRes = m_pImgFrame->pFrameTexture->LockRect(0, &m_ImgLckRect, 0, 0);
		if(hRes != S_OK){
			LOG(" ERR: RGBフレームバッファロック失敗. res=%d.", hRes);
			return 2;
		}
		// Depthフレーム取得.
		hRes = NuiImageStreamGetNextFrame(m_hDepStreamHandle, 0, &m_pDepFrame);
		if(hRes != S_OK){
			LOG(" ERR: Depth次フレーム取得失敗. NuiImageStreamGetNextFrame() returns %d.", hRes);
			return 11;
		}
		// Depthバッファをロック.
		m_pDepFrame->pFrameTexture->LockRect(0, &m_DepLckRect, 0, 0);
		if(hRes != S_OK){
			LOG(" ERR: Depthフレームバッファロック失敗. res=%d.", hRes);
			return 12;
		}
		hRes = NuiSkeletonGetNextFrame(0, &m_SklFrame);
		if(hRes != S_OK){
			LOG(" ERR: スケルトン次フレーム取得失敗. res=%d.", hRes);
			//return 3;
		}
		
		return 0;
	}
	long CKinectController::ReleaseFrame()
	{
		if(m_pImgFrame != NULL){
			HRESULT hRes = NuiImageStreamReleaseFrame(m_hImgStreamHandle, m_pImgFrame);
			if(hRes != S_OK){
				LOG(" ERR: RGBフレーム解放失敗. res=%d.", hRes);
				return 1;
			}
		}
		if(m_pImgFrame != NULL){
			HRESULT hRes = NuiImageStreamReleaseFrame(m_hDepStreamHandle, m_pDepFrame);
			if(hRes != S_OK){
				LOG(" ERR: Depthフレーム解放失敗. res=%d.", hRes);
				return 2;
			}
		}

		return 0;
	}
	long CKinectController::BufferDepthImage(const NUI_IMAGE_FRAME* pSrcFrame, 
		const NUI_LOCKED_RECT& lockRect, IplImage*& pDstImage, IplImage*& pUserIdImage)
	{
		if(pSrcFrame == NULL){
			LOG(" ERR: pSrcFrame is NULL.");
			return -1;
		}
		// 画像サイズ取得.
		NUI_SURFACE_DESC surfaceSize;
		HRESULT hRes = pSrcFrame->pFrameTexture->GetLevelDesc(0, &surfaceSize);
		if(hRes != S_OK){
			LOG(" ERR: バッファ画像サイズ取得失敗. GetLevelDesc() returns %d.", hRes);
			return 1;
		}
		// 画像バッファ作成.
		CvSize imgSize = cvSize(surfaceSize.Width, surfaceSize.Height);
		IplImage* dstImage = cvCreateImage(imgSize, IPL_DEPTH_16U, 1); // unsigned short x 1ch
		if(dstImage == NULL){
			LOG(" ERR: cvCreateImage() returns NULL. img(W=%d,H=%d).", imgSize.width, imgSize.height);
			return 2;
		}
		cvSetZero(dstImage);
		IplImage* usrImage = cvCreateImage(imgSize, IPL_DEPTH_8U, 1); // byte x 1ch.
		if(usrImage == NULL){
			LOG(" ERR: cvCreateImage() returns NULL. img(W=%d,H=%d).", imgSize.width, imgSize.height);
			return 3;
		}
		cvSetZero(usrImage);
		// Depthバッファコピー.
		unsigned short* dstPtr = (unsigned short*)dstImage->imageData;
		unsigned char*  usrPtr = (unsigned char*) usrImage->imageData;
		UINT* srcPtr = (UINT*)lockRect.pBits;
		for(int y=0; y<imgSize.height; y++){
			for(int x=0; x<imgSize.width; x++){
				USHORT usValue = 0; // 画素値取得.
				long lRes = PixelVal(usValue, x, y, pSrcFrame, lockRect);
				if(lRes != 0){
					LOG("  [%d] ERR: バッファ画素値取得失敗. res=%ld. pt(X=%,Y=%d). --> 終了.", lRes, (x/2), (y/2));
					return lRes;
				}
				// 画素読み取り. 
				*usrPtr  = (usValue & 0x7) * 10; // UserID.
				*dstPtr  = (usValue >> 3); // 3bitズラす.
				// 次の画素.
				dstPtr++;
				usrPtr++;
			}
		}
		// 引数に格納.
		pDstImage = dstImage;
		pUserIdImage = usrImage;

		return 0;
	}
	long CKinectController::BufferRgbImage(const NUI_IMAGE_FRAME* pSrcFrame, 
		const NUI_LOCKED_RECT& lockRect, IplImage*& pDstImage)
	{
		if(pSrcFrame == NULL){
			LOG(" ERR: pSrcFrame is NULL.");
			return -1;
		}
		// 画像サイズ取得.
		NUI_SURFACE_DESC surfaceSize;
		HRESULT hRes = pSrcFrame->pFrameTexture->GetLevelDesc(0, &surfaceSize);
		if(hRes != S_OK){
			LOG(" ERR: バッファ画像サイズ取得失敗. GetLevelDesc() returns %d.", hRes);
			return 1;
		}
		// 画像バッファ作成.
		CvSize imgSize = cvSize(surfaceSize.Width, surfaceSize.Height);
		IplImage* dstImage = cvCreateImage(imgSize, IPL_DEPTH_8U, 4);
		if(dstImage == NULL){
			LOG(" ERR: cvCreateImage() returns NULL. img(W=%d,H=%d).", imgSize.width, imgSize.height);
			return 2;
		}
		unsigned int cpySize = imgSize.width * imgSize.height * dstImage->nChannels * sizeof(BYTE);
		memcpy( dstImage->imageData, (BYTE*)lockRect.pBits, cpySize );
		// 引数に格納.
		pDstImage = dstImage;
		return 0;
	}
	long CKinectController::PixelVal(USHORT& val, int x, int y, const NUI_IMAGE_FRAME* pSrcFrame, const NUI_LOCKED_RECT& lockRect)
	{
		// 引数チェック.
		if(pSrcFrame == NULL){
			return -1;
		}
		NUI_SURFACE_DESC surfaceSize;
		HRESULT hRes = pSrcFrame->pFrameTexture->GetLevelDesc(0, &surfaceSize);
		if(hRes != S_OK){
			LOG(" ERR: バッファ画像サイズ取得失敗. GetLevelDesc() returns %d.", hRes);
			return 1;
		}
		if( (x < 0) || (x > surfaceSize.Width) || (y < 0) || (y > surfaceSize.Height) ){
			LOG(" ERR: Invalid Coord Input. pt(X=%d,Y=%d) <--> img(W=%d,H=%d).", x, y, surfaceSize.Width, surfaceSize.Height);
			return -1;
		}
		// 計算.
		const USHORT* usPtr = (const USHORT*)lockRect.pBits;
		const UCHAR* ucPtr = (UCHAR*) &usPtr[ (surfaceSize.Width * y) + x ];
		val = (USHORT)(ucPtr[0] | ucPtr[1] << 8);
		return 0;
	}
	long CKinectController::GetTrakingInfo(std::vector<CTrackingInfo>& trackingInfoList)
	{
		// スケルトン情報の参照.
		//LOG(" スケルトントラッキング情報取得.");
		for(int ns=0; ns<NUI_SKELETON_COUNT; ns++){
			if(m_SklFrame.SkeletonData[ns].eTrackingState == NUI_SKELETON_TRACKED){
				// 関節情報の取り出し
				CTrackingInfo cInfo;
				cInfo.lTrackingID = m_SklFrame.SkeletonData[ns].dwTrackingID;
				//LOG("  [%d/%d] TRACKING!! id=%d. ", ns, NUI_SKELETON_COUNT, cInfo.lTrackingID);
				for(int np=0; np<NUI_SKELETON_POSITION_COUNT; np++){
					long lX = 0, lY = 0;
					USHORT usDp = 0;
					// 座標変換.
					NuiTransformSkeletonToDepthImage(
						m_SklFrame.SkeletonData[ns].SkeletonPositions[np], 
						&lX, &lY, &usDp);
					//LOG("   [%d/%d][skl:%d] (X=%d,Y=%d,D=%d).", ns, NUI_SKELETON_COUNT, np, lX, lY, usDp);
					CDptPoint cDptPt((float)lX, (float)lY, usDp);
					// リストに追加.
					cInfo.skeletalPoints.push_back( cDptPt );
				}
				trackingInfoList.push_back( cInfo );
			}else{
				//LOG("  [%d/%d] NOT TRACKING..... ", ns, NUI_SKELETON_COUNT);
			}
		}
		
		return 0;
	}
	long CKinectController::SetElevationAngle(float fAgl)
	{
		long lAngle = (long)fAgl;
		HRESULT hRes = NuiCameraElevationSetAngle(lAngle);
		LOG(" ピッチ角制御: res=%d. angle=%ld.", hRes, lAngle);
		if(hRes != S_OK){
			return 1;
		}
		return 0;
	}
	long CKinectController::GetElevationAngle(float& fAgl)
	{
		long lAngle = -1;
		HRESULT hRes = NuiCameraElevationGetAngle(&lAngle);
		LOG(" ピッチ角取得: res=%d. angle=%ld.", hRes, lAngle);
		if(hRes != S_OK){
			return 1;
		}
		fAgl = (float)lAngle;
		return 0;
	}
	long CKinectController::Depth16bitTo8bitImage(const IplImage* srcImage, IplImage*& dstImage)
	{
		if(srcImage == NULL){
			return -1;
		}
		if(srcImage->nChannels != 1){
			return -1;
		}
		if(srcImage->depth != IPL_DEPTH_16U){
			return -1;
		}
		IplImage* tmpImage = cvCreateImage( cvGetSize(srcImage), IPL_DEPTH_16U, 1 );
		if(tmpImage == NULL){
			LOG(" ERR: cvCreateImage() returns NULL.");
			return 1;
		}
		static const int NORM_COEFF = 6;
		cvSetZero(tmpImage);
		unsigned short* pSrc = (unsigned short*)srcImage->imageData;
		unsigned short* pDst = (unsigned short*)tmpImage->imageData;
		for(int y=0; y<tmpImage->height; y++){
			for(int x=0; x<tmpImage->width; x++, pSrc++, pDst++){
				*pDst = *pSrc * NORM_COEFF;
			}
		}
		dstImage = cvCreateImage( cvGetSize(tmpImage), IPL_DEPTH_8U, 1 );
		if(dstImage == NULL){
			LOG(" ERR: cvCreateImage() returns NULL.");
			return 2;
		}
		double dScale = 1.0 / UCHAR_MAX;
		cvCvtScale(tmpImage, dstImage, dScale);

		cvReleaseImage(&tmpImage);
		return 0;
	}
	CKinectController::CKinectController()
	{
		// RGB関連
		m_hImgStreamEvent = NULL;
		m_hImgStreamHandle = NULL;
		m_pImgFrame = NULL;
		m_pRgbImageBuff = NULL;
		// Depth関連
		m_hDepStreamEvent = NULL;
		m_hDepStreamHandle = NULL;
		m_pDepFrame = NULL;
		m_pDepImageBuff = NULL;
		//------ SKELTON/USER TRACKING関連 ------
		m_hSklStreamEvent = NULL;
		m_pUserImageBuff = NULL;
	}
	CKinectController::~CKinectController()
	{
		Finalize();
	}

	CDepthPoint::CDepthPoint() :fX(0.0), fY(0.0), usDepth(0) {
	}
	CDepthPoint::CDepthPoint(float x, float y, USHORT depth) {
		fX = x; fY = y; usDepth = depth;
	}
	CDepthPoint::~CDepthPoint(){
	}
	CDepthPoint::CDepthPoint(const CDepthPoint& obj){
		this->fX = obj.fX; this->fY = obj.fY; this->usDepth = obj.usDepth;
	}

	CTrackingInfo::CTrackingInfo() : lTrackingID(0) {
	}
	CTrackingInfo::~CTrackingInfo(){
	}
	CTrackingInfo::CTrackingInfo(const CTrackingInfo& obj){
		this->lTrackingID = obj.lTrackingID; this->skeletalPoints = obj.skeletalPoints;
	}

}

• main.cpp

#include "stdafx.h"

#include "KinectUtil.h"
#include <opencv/highgui.h>
#include <vector>

int _tmain(int argc, _TCHAR* argv[])
{
	Utility::CKinectCtrl cKinectCtrl;

	// 初期化.
	long lRes = cKinectCtrl.Initialize();
	if(lRes != 0){
		return lRes;
	}
	cKinectCtrl.SetElevationAngle(0); // サーボ角制御.

	while(1){
		// キャプチャ処理.
		lRes = cKinectCtrl.GrabFrame();
		if(lRes != 0){
			break;
		}
		// RGB画像.
		IplImage* rgbImage = NULL;
		lRes = cKinectCtrl.GetRGBFrame(rgbImage);
		if(lRes != 0){
			break;
		}
		cvShowImage("RGB", rgbImage);
		// DEPTH画像.
		IplImage* depthSrc = NULL;
		lRes = cKinectCtrl.GetDepthFrame(depthSrc);
		if(lRes != 0){
			break;
		}
		cvShowImage("DEPTH", depthSrc);
		int key = cvWaitKey(10);
		if((key == 27)||(key == 'q')){
			break;
		}
	}

	return 0;
}

• 追伸

2012/06/02
Kinect for Windows SDK v1.5でもコンパイルが通ることを確認しました。

mrptでDepth画像を取得する

訳あって懲りずにmrptいじくってます。
KinectのDepthデータをOpenCVで扱いたいと思って、いろいろ試してみましたが、うまくいかなくて、結局mrptで書いたのが動いたのでソースコードを載せておきます。
首動かせるよ、みたいなソースコード書いてありますが、動かないのであしからず。

環境

• Lenovo Thinkpad X201
• Ubuntu 11.10 x64
• Kinect Sensor for XBOX360
• mrpt(Mobile Robot Programming Toolkit) 0.9
• OpenCV 2.3.1

ソースコード

/**
 * @file main.cpp
 */
 
#include "LogCommon.h"

#include <opencv2/opencv.hpp>
#include <mrpt/hwdrivers.h>

#define LOG INF_LOG // Log Utiliry Macro.

//---------------------------------------------------------------
// constant
//---------------------------------------------------------------
static const int DEPTH_WIDTH  = 640;
static const int DEPTH_HEIGHT = 480;

static const double MAX_TILT = 30.0;
static const double MIN_TILT = -30.0;

static const char* wndNameDepth = "DEPTH";
//---------------------------------------------------------------
// definition
//---------------------------------------------------------------
struct TThreadParam
{
	TThreadParam() : bQuit(false), dTiltDeg(0.0), iKeyPushed(0) {}
	volatile bool bQuit;
	volatile double dTiltDeg;
	volatile int iKeyPushed;
	mrpt::synch::CThreadSafeVariable<mrpt::slam::CObservation3DRangeScanPtr> pObsSafeData;
};

//---------------------------------------------------------------
// prototype
//---------------------------------------------------------------
// Initialize
int init();
// Capture Depth
void thread_grab(TThreadParam& pThParam);
// Point Cloud To Image
long ToImage(const mrpt::slam::CObservation3DRangeScanPtr p3dData, cv::Mat& depthImage);
// Depth To Pixel
long DepthToPixel(float src, unsigned char& dst);

// Main Procedure
int main(void)
{	
	init();
	// Create Thread of Capture
	TThreadParam thParam;
	mrpt::system::TThreadHandle threadHandle = mrpt::system::createThreadRef( thread_grab, thParam );
	// Init Window
	cv::Mat depthImage( DEPTH_HEIGHT, DEPTH_WIDTH, CV_8U );
	// Main Loop.
	int mainCout = 0;
	while(!thParam.bQuit){
		// Retrieve Data.
		mrpt::slam::CObservation3DRangeScanPtr obs3dScanData = thParam.pObsSafeData.get();
		if( (obs3dScanData == NULL) || (obs3dScanData->hasPoints3D == false) ){
			continue;
		}
		// 3D Range to Depth Image.
		ToImage(obs3dScanData, depthImage);
		
		cv::imshow( wndNameDepth, depthImage );
		int key = cv::waitKey(30);
		if(key == 'q'){
			// QUIT.
			thParam.bQuit = true;
			break;
		}else if(key == 38){ // Up Key.
			LOG(" Input Up Key.");
			if(thParam.dTiltDeg <= MAX_TILT){
				thParam.dTiltDeg += 2.0;
				LOG(" >> Up");
			}
		}else if(key == 40){ // Down Key.
			LOG(" Input Down Key.");
			if(thParam.dTiltDeg <= MIN_TILT){
				thParam.dTiltDeg -= 2.0;
				LOG(" >> Down");
			}
		}
		thParam.iKeyPushed = key;
		mainCout++;
	}
	mrpt::system::joinThread(threadHandle);
	
	return 0;
}

void thread_grab(TThreadParam& pThParam)
{
	// Kinect Controller Class
	mrpt::hwdrivers::CKinect cKinectSensor;
	cKinectSensor.initialize();
	LOG(" Kinect Sensor initialize OK");
	// Capture Loop.
	int cnt = 0;
	while(!pThParam.bQuit){
		mrpt::slam::CObservation3DRangeScanPtr  p3dRangeScan = mrpt::slam::CObservation3DRangeScan::Create();
		bool bObsResult = false;
		bool bHardError = false;
		cKinectSensor.getNextObservation( *p3dRangeScan, bObsResult, bHardError); // Get Point Cloud.
		if(p3dRangeScan->hasPoints3D == true){
			pThParam.pObsSafeData.set( p3dRangeScan );
		}else{
			LOG(" ERR: p3dRangeScan do NOT has Point 3D data.");
		}
		// Move Direction.
		if(pThParam.iKeyPushed != 0){
			if( (pThParam.dTiltDeg <= MAX_TILT) && (pThParam.dTiltDeg >= MIN_TILT) ){
				cKinectSensor.setTiltAngleDegrees( pThParam.dTiltDeg );
			}
			pThParam.iKeyPushed = 0;
		}
		cnt++;
	}
}

long ToImage(const mrpt::slam::CObservation3DRangeScanPtr p3dData, cv::Mat& depthImage)
{
	// Check Size.
	if(	(p3dData->points3D_x.size() != p3dData->points3D_y.size() ) ||
		(p3dData->points3D_y.size() != p3dData->points3D_z.size() ) )
	{
		LOG("  ERR: ScanData has Invalid Data.");
		return -1;
	}
	if(p3dData->points3D_x.size() != (depthImage.cols * depthImage.rows)){
		LOG("  ERR: ScanData size is Invalid. size=%d.", p3dData->points3D_x.size());
		return -1;
	}
	// Loop for Each Pixels.
	for(int y=0; y<depthImage.rows; y++){
		for(int x=0; x<depthImage.cols; x++){
			const int idx = y * depthImage.cols + x;
			float srcVal = p3dData->points3D_x[ idx ]; 
			DepthToPixel( srcVal, *(depthImage.ptr( y, x)) );
		}
	}		
	return 0;
}

long DepthToPixel(float src, unsigned char& dst)
{
	static const double dCoeff = 100.0; // T.B.D.
	const int iVal = (int) (src * dCoeff);
	dst = (unsigned char)iVal;
	
	return 0;
}

int init()
{
	setLogPath("test.log", true); // Initialize Log Utility.
	cv::namedWindow( wndNameDepth );
	return 0;
}

Makefile

LogCommon.cppはオレオレ用ログユーティリティです。無視してください。

CC  = g++ -O
SRC = main.cpp \
		  Util/LogCommon.cpp
TGT = KinectCapTest

USR_DIR = /usr
INCPATH = $(USR_DIR)/include
LIBPATH = $(USR_DIR)/lib

OPT = 	-lstdc++ \
			-lopencv_core -lopencv_highgui \
			-lopencv_imgproc -lopencv_legacy \
			-lmrpt-base \
			-lmrpt-obs -lmrpt-slam \
			-lmrpt-hwdrivers \
			-lmrpt-gui -lmrpt-vision

CFLAGS  = -I$(INCPATH) \
	-I$(INCPATH)/mrpt/base/include \
	-I$(INCPATH)/mrpt/slam/include \
	-I$(INCPATH)/mrpt/maps/include \
	-I$(INCPATH)/mrpt/detectors/include \
	-I$(INCPATH)/mrpt/opengl/include \
	-I$(INCPATH)/mrpt/mrpt-config \
	-I$(INCPATH)/mrpt/hmtslam/include \
	-I$(INCPATH)/mrpt/gui/include \
	-I$(INCPATH)/mrpt/obs/include \
	-I$(INCPATH)/mrpt/topography/include \
	-I$(INCPATH)/mrpt/hwdrivers/include \
	-I$(INCPATH)/mrpt/bayes/include \
	-I$(INCPATH)/mrpt/vision/include \
	-I$(INCPATH)/mrpt/reactivenav/include \
	-I$(INCPATH)/mrpt/scanmatching/include \
	-I./Util

LDFLAGS = -L. -L$(LIBPATH) 

all:
	$(CC) $(SRC) -o $(TGT) $(CFLAGS) $(LDFLAGS) $(OPT)

clean:
	rm $(TGT)

Ubuntu 11.10を導入したら入れるべきソフトのメモ

自分用のメモです

マスト

• Synaptic Package Manager

ソフトウェアセンターだけでは不十分

• Simple Lightdm Manager

ログイン画面をカスタマイズしたくなる

• System Load Indicator

gnomeじゃなくなったけど、システムモニタは必要

• vim

ないと困る

• mozc

やっぱりこれ

• google Chrome

当然

ライブラリ

• mrpt

オフィシャルサイトの手順に従う

• PCL

オフィシャルサイトの手順に従う

• opencv

これは自分でビルド

開発ツール

• geany

私がメインで使ってるエディタです

• meld

差分・マージツール

Kinectを広角化するZoom for Xbox360

Kinectを広角カメラ化するZoom for Xbox 360を購入しました。

ZOOM for Xbox 360 (英語版/日本語説明書同梱)

裏は密着するようになっています。意外とちゃんとしている。

ちゃんとLEDの点灯も見えるようになってます。

KinectSDKのKinect Explorerをキャプチャしたので比較してみましょう。

• 装着前

• 装着後

おそろしく違うことにビックリ。

• メリット

正確に計測していませんが、視野角は90°以上にまで広がっています。
より近くの、より多くの人を検出しようと思ったら使える。

• デメリット

Depthが粗いので、手指や表情のセンシングをしようという人にはお勧めできません。
歪みも大きく、正像変換しないと透視投影系の演算はできない。
隅っこの方はDepthが撮れていません。これはスペックルパターンが投影されていない部分をキャプチャしているからと思われます。
これも正確に計測していませんが、RGB-Depthのズレも大きくなっているはずなので、キャリブレーションも必要になってくるかも..

自宅をSLAMったソースコード

前々回くらいで自宅をSLAMったソースコードを載せます。
githubに載せたかったのですが、使い方が分からなかったのでこちらに直接載せます。

SLAMツール

mrpt-sample-appsのkinect-3d-slamを修正しました。
三次元座標のみを出力するだけだったのを、RGBも付けて出力するようにしました。
kinect-3d-slam_main.cppの518行目を以下のように修正するだけです。
※ mrpt-sample-appsのソースコードはサイトから直接落とせます。

	//globalPtsMap.save3D_to_text_file(s);
	globalPtsMap.save3D_and_colour_to_text_file(s);

点群ビューア

PCLをつかって上記のkinect-3d-slamが吐いた点群ファイルを読み込むソースコードを載せておきます。
@otlさんのソースコードを流用させた頂きしました。
大部分はSSVファイルの文字列操作の処理ですので公開する意味が乏しいですが.. そのまま動きますよ。

/**
 * @file View.cpp
 */

#include 
#include 
#include 
#include 
#include 
#include 

#define LOG printf

using namespace pcl;

// 文字列リスト.
typedef std::vector StringList;
// ファイル名.
static const std::string SAMPLE_FILE = "point_cloud.txt";

typedef PointCloud PointCloudXYZRGB;

// テキストからPointCloudXYZRGBを読み込む.
int LoadCloudText(PointCloudXYZRGB& ptCloud, std::string sFileName);
// 行から座標・RGBを読み込む.
int LintToXYZRGB(std::string sLine, 
	float& fX, float& fY, float& fZ, unsigned char& ucR, unsigned char& ucG, unsigned char& ucB);
// テキストファイルから行ごとに文字列を読み込む.
long ReadFileLine(StringList &strList, std::string sFileName);

int main(int argc, char *argv[])
{
	// ファイル名を引数から取得.
	std::string sFileName;
	if(argc > 1){
		LOG(" ARG: %s...", argv[1]);
		sFileName.append( argv[1] );
	}
	if(sFileName.length() < 2){
		LOG(" TARGET FILE: %s...", SAMPLE_FILE.c_str());
		sFileName.append( SAMPLE_FILE );
	}
	// テキストからロード.
	PointCloudXYZRGB ptCloud;
	int res = LoadCloudText(ptCloud, sFileName);
	if(res != 0){
		return res;
	}
	// 表示.
	visualization::CloudViewer viewer("Simple Viewer");
	viewer.showCloud(ptCloud.makeShared());
	while(1){
		if(viewer.wasStopped() == true){
			break;
		}
	}
	
	return 0;
}

int LoadCloudText(PointCloudXYZRGB& ptCloud, std::string sFileName)
{
	StringList sLineList;
	long res = ReadFileLine(sLineList, sFileName);
	LOG(" テキスト読み込み. res=%ld. file=%s.\n", res, sFileName.c_str());
	if(res != 0){
		LOG(" ERR: テキスト読み込み失敗.\n");
		return res;
	}
	const unsigned int numLine = sLineList.size();
	ptCloud.reserve(numLine);
	LOG(" 座標値読み込み. NUM=%d.\n", numLine);
	for(unsigned int nl=0; nl*1;
	fX = atof( sX.c_str() );
	//--------------------------
	// Yの取り出し.
	std::string sY = sLine;
	sY.erase(sY.begin(), sY.begin()+idxSP0+1);
	size_t idxSP1 = sY.find_first_of(sSP);
	if(idxSP1 == std::string::npos){
		return 2;
	}
	sY.erase(sY.begin()+idxSP1, sY.end());
	fY = atof( sY.c_str() );
	//--------------------------
	// Zの取り出し.
	std::string sZ = sLine;
	sZ.erase(sZ.begin(), sZ.begin()+idxSP0+1+idxSP1+1);
	size_t idxSP2 = sZ.find_first_of(sSP);
	if(idxSP2 == std::string::npos){
		return 3;
	}
	sZ.erase(sZ.begin()+idxSP2, sZ.end());
	fZ = atof( sZ.c_str() );
	//--------------------------
	// Rの取り出し.
	std::string sR = sLine;
	sR.erase(sR.begin(), sR.begin()+idxSP0+1+idxSP1+1+idxSP2+1);
	size_t idxSP3 = sR.find_first_of(sSP);
	if(idxSP3 == std::string::npos){
		return 4;
	}
	sR.erase(sR.begin()+idxSP3, sR.end());
	int iR = atoi(sR.c_str());
	ucR = (unsigned char)iR;
	//--------------------------
	// Gの取り出し.
	std::string sG = sLine;
	sG.erase(sG.begin(), sG.begin()+idxSP0+1+idxSP1+1+idxSP2+1+idxSP3+1);
	size_t idxSP4 = sG.find_first_of(sSP);
	if(idxSP4 == std::string::npos){
		return 5;
	}
	sG.erase(sG.begin()+idxSP4, sG.end());
	int iG = atoi(sG.c_str());
	ucG = (unsigned char)iG;
	//--------------------------
	// Bの取り出し.
	std::string sB =sLine;
	sB.erase(sB.begin(), sB.begin()+idxSP0+1+idxSP1+1+idxSP2+1+idxSP3+1+idxSP4+1);
	int iB = atoi(sB.c_str());
	ucB = (unsigned char)iB;
		
	return 0;
}

long ReadFileLine(StringList &strList, std::string sFileName)
{
	if(sFileName.length() < 3){
		return -1;
	}
	std::ifstream fs;
	fs.open(sFileName.c_str());
	if(fs == false){
		return -1;
	}
	const int STRLEN = 128;
	char buf[STRLEN];
	for(int n=0; ; n++){
		memset(buf, 0, sizeof(char)*STRLEN);
		fs.getline(buf, sizeof(char)*STRLEN);
		if(fs.eof() == true){
			break; // 終端.
		}
		std::string sLine( buf );
		if(sLine.length() < 3){
			break; // 無効な文字列.
		}
		strList.push_back( sLine );
	}
	fs.close();

	return 0;
}

## Makefile ##
SRC = View.cpp
DST = ViewTest

prefix  = /usr
INCPATH = $(prefix)/include
LIBPATH = $(prefix)/lib 

OPT = \
	-lcv -lcvaux -lcxcore -lhighgui \
	-lpcl_common -lpcl_io -lpcl_visualization \
	-lstdc++
CC  = g++ -O

CFLAGS  = \
	-I$(INCPATH)/pcl-1.1 \
	-I$(INCPATH)/eigen3 \
	-I$(INCPATH)/vtk-5.4 
LDFLAGS = -L. -L$(LIBPATH) 

all:
	$(CC) $(SRC)  -o $(DST) $(CFLAGS)  $(LDFLAGS) $(OPT)

clean:
	rm -fr $(DST)

まとめ

点群をポリゴンにするコードはテクスチャが貼れたら公開しようと思っています。

*1:uint32_t)iR << 16 | (uint32_t)iG << 8 | (uint32_t)iB); pt.rgb = *reinterpret_cast(&uiRGB); //pt.rgb = (iR << 16 | iG << 8 | iB); ptCloud.push_back(pt); } return 0; } int LintToXYZRGB(std::string sLine, float& fX, float& fY, float& fZ, unsigned char& ucR, unsigned char& ucG, unsigned char& ucB) { static const std::string sSP = " "; // スペース. //-------------------------- // Xの取り出し. std::string sX = sLine; size_t idxSP0 = sX.find_first_of(sSP); if(idxSP0 == std::string::npos){ return 1; } sX.erase(sX.begin()+idxSP0, sX.end(

自宅をSLAMったー(4)

前回、mrpt+Kinectを用いて取得した点群を、PCLを用いて描画することができましたが、点群だけだとどんな形かイマイチよく分からないので、点群からポリゴンを生成して描画してみました。

PCLのソースに含まれる以下のサンプルプログラムを参考に作成しました。

• surface_example.cpp
• pcl_visualizer_demo.cpp

描画したはいいのですが、またもポリゴンに色がついていないので何が写っているのかよく分かりません..
最終的には部屋を形状復元したいので、改善の余地がありそうですね。