Kinect v2 Coordinate System Mapping

Coordinate System

The basic stream data that retrieve from Kinect v2 has been each following coordinate-systems.

Coordinate System Stream Data
Color Coordinate System Color
Depth Coordinate System Depth / Infrared / BodyIndex
Camera Coordinate System Body

Coordinate Mapper

ICoordinateMapper class of Kinect SDK v2 provides features for mapping each coordinate-system.
It has the feature that to retrieve mapping in frame and point units.
You can mapping data to other coordinate-system using mapping information that retrieved from ICoordinateMapper apis.

Coordinate Mapping

I will introduce how to mapping Color and Depth coordinate-system. *1 *2 *3
This sample program is published in following.


*1 This source code is used error check macro ERROR_CHECK() for simplify.
*2 This source code is used smart pointer Microsoft::WRL::ComPtr() for simplify.
*3 The descriptions such how to retrieve data has been omitted. In addition, It doesn’t optimized for simplify.

Retrieve Coordinate Mapper

First, You will retrieve ICoordinateMapper from IKinectSensor.

// 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

If you want to retrieve Color data in Depth coordinate-system resolution, You will processing as follows.
You will retrieve mapping information that each color points ( cx, cy ) corresponding to each depth points ( dx, dy ) using ICoordinateMapper::MapDepthFrameToColorSpace().
You can retrieve mapped color data (512*424) by mapping color data to depth coordinate-system using mapping information.

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

This result is here.

map_color_to_depth_resolution
raw_depth

Retrieve Depth Data in Color Resolution

If you want to retrieve Depth data in Color coordinate-system resolution, You will processing as follows.
You will retrieve mapping information that each depth points ( dx, dy ) corresponding to each color points ( cx, cy ) using ICoordinateMapper::MapColorFrameToDepthSpace().
You can retrieve mapped depth data (1920*1080) by mapping depth data to color coordinate-system using mapping information.

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

This result is here.

raw_color
map_depth_to_color_resolution

Retrieve Joint Position in Color Coordinate System

Similarly, You will retrieve Joint::Position in Color coordinate-system ( cy, 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

You will retrieve Joint::Position in Depth coordinate-system ( dy, 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 );

Leave a Reply

Your email address will not be published. Required fields are marked *