Kinect v2 Coordinate System Mapping

Coordinate System

The each sensors that mounted on Kinect v2 is located on a different physical position.
Therefore, The data that is able to retrieved from them sensors have different coordinate systems.

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 *