Coordinate System
Kinect v2の各センサーは物理的に異なる場所に配置されており、そこから取得できるデータはそれぞれ座標系が異なります。
Kinect v2から取得できる基本ストリームのデータは、それぞれ以下の座標系のデータです。
Coordinate System | Stream Data |
---|---|
Color Coordinate System | Color |
Depth Coordinate System | Depth / Infrared / BodyIndex |
Camera Coordinate System | Body |
Coordinate Mapper
Kinect SDK v2のICoordinateMapperクラスには、各座標系を対応付けるための関数が用意されています。
これはフレーム単位で対応関係を取得する関数とポイント単位で対応関係を取得する関数が用意されており、用途によって使い分けます。
取得した座標系間の対応関係をもとにそれぞれのデータを位置合わせすることができます。
Coordinate Mapping
データの位置合わせ例として、Color座標系とDepth座標系の位置合わせを紹介します。*1 *2 *3
全体のプログラムは以下を参照してください。
*1 エラーチェックの説明を簡略化するためERROR_CHECK()マクロを使用しています。
*2 COMインターフェイスのポインターをメモリ管理するためのスマートポインター「Microsoft::WRL::ComPtr
*3 データの取得などの説明は省略しています。また、最適化も行っておりません。
Retrieve Coordinate Mapper
IKinectSensorからICoordinateMapperを取得します。
// Create Sensor Instance ComPtr<IKinectSensor> kinect; ERROR_CHECK( GetDefaultKinectSensor( &kinect ) ); // Open Sensor ERROR_CHECK( kinect->Open() ); // Retrieved Coordinate Mapper ComPtr<ICoordinateMapper> coordinateMapper; ERROR_CHECK( kinect->get_CoordinateMapper( &coordinateMapper ) );
Retrieve Color Data in Depth Resolution
Color画像をDepth座標系の解像度で取得します。
ICoordinateMapper::MapDepthFrameToColorSpace()を利用して、Depth座標系の座標( dx, dy )それぞれに対応するColor座標系の座標( cx, cy )を得ます。
この対応関係をもとにデータを位置合わせすると、Depth座標系の解像度(512*424)のColor画像が得られます。
// Retrieve Mapped Coordinates std::vector<ColorSpacePoint> colorSpacePoints( depthWidth * depthHeight ); ERROR_CHECK( coordinateMapper->MapDepthFrameToColorSpace( depthBuffer.size(), &depthBuffer[0], colorSpacePoints.size(), &colorSpacePoints[0] ) ); // Mapped Color Buffer std::vector<BYTE> buffer( depthWidth * depthHeight * colorBytesPerPixel ); // Mapping Color Data to Depth Resolution for( int depthY = 0; depthY < depthHeight; depthY++ ){ for( int depthX = 0; depthX < depthWidth; depthX++ ){ const unsigned int depthIndex = depthY * depthWidth + depthX; const int colorX = static_cast<int>( colorSpacePoints[depthIndex].X + 0.5f ); const int colorY = static_cast<int>( colorSpacePoints[depthIndex].Y + 0.5f ); if( ( 0 <= colorX ) && ( colorX < colorWidth ) && ( 0 <= colorY ) && ( colorY < colorHeight ) ){ const unsigned int colorIndex = colorY * colorWidth + colorX; buffer[depthIndex * colorBytesPerPixel + 0] = colorBuffer[colorIndex * colorBytesPerPixel + 0]; buffer[depthIndex * colorBytesPerPixel + 1] = colorBuffer[colorIndex * colorBytesPerPixel + 1]; buffer[depthIndex * colorBytesPerPixel + 2] = colorBuffer[colorIndex * colorBytesPerPixel + 2]; buffer[depthIndex * colorBytesPerPixel + 3] = colorBuffer[colorIndex * colorBytesPerPixel + 3]; } } } /* // e.g. Mapped Color Buffer to cv::Mat cv::Mat colorMat = cv::Mat( depthHeight, depthWidth, CV_8UC4, &buffer[0] ).clone(); */
位置合わせした結果は以下のようになります。
Retrieve Depth Data in Color Resolution
DepthデータをColor座標系の解像度で取得します。
ICoordinateMapper::MapColorFrameToDepthSpace()を利用して、Color座標系の座標( cx, cy )それぞれに対応するDepth座標系の座標( dx, dy )が得られます。
この対応関係をもとにデータを位置合わせすると、Color座標系の解像度(1920*1080)のDepth画像が得られます。
// Retrieve Mapped Coordinates std::vector<DepthSpacePoint> depthSpacePoints( colorWidth * colorHeight ); ERROR_CHECK( coordinateMapper->MapColorFrameToDepthSpace( depthBuffer.size(), &depthBuffer[0], depthSpacePoints.size(), &depthSpacePoints[0] ) ); // Mapped Depth Buffer std::vector<UINT16> buffer( colorWidth * colorHeight ); // Mapping Depth Data to Color Resolution for ( int colorY = 0; colorY < colorHeight; colorY++ ){ for ( int colorX = 0; colorX < colorWidth; colorX++ ){ const unsigned int colorIndex = colorY * colorWidth + colorX; const int depthX = static_cast<int>( depthSpacePoints[colorIndex].X + 0.5f ); const int depthY = static_cast<int>( depthSpacePoints[colorIndex].Y + 0.5f ); if ( ( 0 <= depthX ) && ( depthX < depthWidth ) && ( 0 <= depthY ) && ( depthY < depthHeight ) ){ const unsigned int depthIndex = depthY * depthWidth + depthX; buffer[colorIndex] = depthBuffer[depthIndex]; } } } /* // e.g. Mapped Depth Buffer to cv::Mat cv::Mat depthMat = cv::Mat( colorHeight, colorWidth, CV_16UC1, &buffer[0] ).clone(); */
位置合わせした結果は以下のようになります。
Retrieve Joint Position in Color Coordinate System
同様に、BodyデータのJoint::Positionに対応するColor座標系の座標( cx, cy )で取得することもできます。
ColorSpacePoint colorSpacePoint; ERROR_CHECK( coordinateMapper->MapCameraPointToColorSpace( joint.Position, &colorSpacePoint ) ); const int cx = static_cast<int>( colorSpacePoint.X + 0.5f ); const int cy = static_cast<int>( colorSpacePoint.Y + 0.5f );
Retrieve Joint Position in Depth Coordinate System
BodyデータのJoint::Positionに対応するDepth座標系の座標( dx, dy )で取得することもできます。
DepthSpacePoint depthSpacePoint; ERROR_CHECK( coordinateMapper->MapCameraPointToDepthSpace( joint.Position, &depthSpacePoint ) ); const int dx = static_cast<int>( depthSpacePoint.X + 0.5f ); const int dy = static_cast<int>( depthSpacePoint.Y + 0.5f );
わっかんないや