Kinect v2 Coordinate System Mapping

• English (United States)
• 日本語

Coordinate System

Kinect v2の各センサーは物理的に異なる場所に配置されており、そこから取得できるデータはそれぞれ座標系が異なります。

Kinect v2から取得できる基本ストリームのデータは、それぞれ以下の座標系のデータです。

Coordinate Mapper

Kinect SDK v2のICoordinateMapperクラスには、各座標系を対応付けるための関数が用意されています。
これはフレーム単位で対応関係を取得する関数とポイント単位で対応関係を取得する関数が用意されており、用途によって使い分けます。
取得した座標系間の対応関係をもとにそれぞれのデータを位置合わせすることができます。

Coordinate Mapping

データの位置合わせ例として、Color座標系とDepth座標系の位置合わせを紹介します。^{*1 *2 *3}
全体のプログラムは以下を参照してください。

• Kinect2Sample – CoordinateMapper | GitHub

^{*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 );