Kinect v2 Coordinate System Mapping

Coordinate System

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画像が得られます。

map_depth_frame_to_color_space

// 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();
*/

位置合わせした結果は以下のようになります。

map_color_to_depth_resolution
raw_depth

Retrieve Depth Data in Color Resolution

DepthデータをColor座標系の解像度で取得します。
ICoordinateMapper::MapColorFrameToDepthSpace()を利用して、Color座標系の座標( cx, cy )それぞれに対応するDepth座標系の座標( dx, dy )が得られます。
この対応関係をもとにデータを位置合わせすると、Color座標系の解像度(1920*1080)のDepth画像が得られます。

map_color_frame_to_depth_space

// 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();
*/

位置合わせした結果は以下のようになります。

raw_color
map_depth_to_color_resolution

Retrieve Joint Position in Color Coordinate System

同様に、BodyデータのJoint::Positionに対応するColor座標系の座標( cx, cy )で取得することもできます。

map_camera_point_to_color_space

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 )で取得することもできます。

map_camera_point_to_depth_space

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 );

 


コメントを残す

メールアドレスが公開されることはありません。 * が付いている欄は必須項目です