Last active
July 4, 2016 14:42
-
-
Save UnaNancyOwen/7e2c685752e16f8e42cc to your computer and use it in GitHub Desktop.
Kinect v2 Coordinate System Mapping
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#include <iostream> | |
#include <sstream> | |
#include <Windows.h> | |
#include <Kinect.h> | |
#include <opencv2/opencv.hpp> | |
#include <atlbase.h> | |
// Error Check | |
#define ERROR_CHECK( ret ) \ | |
if( (ret) != S_OK ){ \ | |
std::stringstream ss; \ | |
ss << "failed " #ret " " << std::hex << ret << std::endl; \ | |
throw std::runtime_error( ss.str().c_str() ); \ | |
} | |
class Kinect | |
{ | |
private: | |
// Sensor | |
CComPtr<IKinectSensor> kinect; | |
CComPtr<ICoordinateMapper> mapper; | |
// Color | |
CComPtr<IColorFrameReader> colorFrameReader; | |
std::vector<BYTE> colorBuffer; | |
int colorWidth; | |
int colorHeight; | |
unsigned int colorBytesPerPixel; | |
cv::Mat colorMat; | |
// Depth | |
CComPtr<IDepthFrameReader> depthFrameReader; | |
std::vector<UINT16> depthBuffer; | |
int depthWidth; | |
int depthHeight; | |
cv::Mat depthMat; | |
public: | |
// Constructor | |
Kinect() | |
{ | |
// Initialize | |
initialize(); | |
} | |
~Kinect() | |
{ | |
// Finalize | |
finalize(); | |
} | |
// Run | |
void run() | |
{ | |
while ( 1 ) { | |
// Update | |
update(); | |
// Draw | |
draw(); | |
// Show | |
show(); | |
// Key Check | |
const int key = cv::waitKey( 10 ); | |
if( key == VK_ESCAPE ){ | |
break; | |
} | |
} | |
} | |
private: | |
// Initialize | |
void initialize() | |
{ | |
cv::setUseOptimized( true ); | |
// Initialize Sensor | |
initializeSensor(); | |
// Initialize Color | |
initializeColor(); | |
// Initialize Depth | |
initializeDepth(); | |
} | |
// Initialize Sensor | |
void initializeSensor() | |
{ | |
// Open Sensor | |
ERROR_CHECK( GetDefaultKinectSensor( &kinect ) ); | |
ERROR_CHECK( kinect->Open() ); | |
// Check Open | |
BOOLEAN isOpen = FALSE; | |
ERROR_CHECK( kinect->get_IsOpen( &isOpen ) ); | |
if( !isOpen ){ | |
throw std::runtime_error( "failed IKinectSensor::get_IsOpen( &isOpen )" ); | |
} | |
// Retrieved Coordinate Mapper | |
ERROR_CHECK( kinect->get_CoordinateMapper( &mapper ) ); | |
} | |
// Initialize Color | |
void initializeColor() | |
{ | |
// Open Color Reader | |
CComPtr<IColorFrameSource> colorFrameSource; | |
ERROR_CHECK( kinect->get_ColorFrameSource( &colorFrameSource ) ); | |
ERROR_CHECK( colorFrameSource->OpenReader( &colorFrameReader ) ); | |
// Retrieved Color Description | |
CComPtr<IFrameDescription> colorFrameDescription; | |
ERROR_CHECK( colorFrameSource->CreateFrameDescription( ColorImageFormat::ColorImageFormat_Bgra, &colorFrameDescription ) ); | |
ERROR_CHECK( colorFrameDescription->get_Width( &colorWidth ) ); // 1920 | |
ERROR_CHECK( colorFrameDescription->get_Height( &colorHeight ) ); // 1080 | |
ERROR_CHECK( colorFrameDescription->get_BytesPerPixel( &colorBytesPerPixel ) ); // 4 | |
// Allocation Color Buffer | |
colorBuffer.resize( colorWidth * colorHeight * colorBytesPerPixel ); | |
} | |
// Initialize Depth | |
void initializeDepth() | |
{ | |
// Open Depth Reader | |
CComPtr<IDepthFrameSource> depthFrameSource; | |
ERROR_CHECK( kinect->get_DepthFrameSource( &depthFrameSource ) ); | |
ERROR_CHECK( depthFrameSource->OpenReader( &depthFrameReader ) ); | |
// Retrieved Depth Description | |
CComPtr<IFrameDescription> depthFrameDescription; | |
ERROR_CHECK( depthFrameSource->get_FrameDescription( &depthFrameDescription ) ); | |
ERROR_CHECK( depthFrameDescription->get_Width( &depthWidth ) ); // 512 | |
ERROR_CHECK( depthFrameDescription->get_Height( &depthHeight ) ); // 424 | |
// Allocation Depth Buffer | |
depthBuffer.resize( depthWidth * depthHeight ); | |
} | |
// Finalize | |
void finalize() | |
{ | |
// Close Sensor | |
ERROR_CHECK( kinect->Close() ); | |
} | |
// Update | |
void update() | |
{ | |
// Update Color | |
updateColor(); | |
// Update Depth | |
updateDepth(); | |
} | |
// Update Color | |
void updateColor() | |
{ | |
// Retrieved Color Frame | |
CComPtr<IColorFrame> colorFrame; | |
const HRESULT ret = colorFrameReader->AcquireLatestFrame( &colorFrame ); | |
if( FAILED( ret ) ){ | |
return; | |
} | |
// Copy Data and Convert Format ( YUY2 -> BGRA ) | |
ERROR_CHECK( colorFrame->CopyConvertedFrameDataToArray( static_cast<UINT>( colorBuffer.size() ), &colorBuffer[0], ColorImageFormat::ColorImageFormat_Bgra ) ); | |
} | |
// Update Depth | |
void updateDepth() | |
{ | |
// Retrieved Depth Frame | |
CComPtr<IDepthFrame> depthFrame; | |
const HRESULT ret = depthFrameReader->AcquireLatestFrame( &depthFrame ); | |
if( FAILED( ret ) ){ | |
return; | |
} | |
// Copy Data | |
ERROR_CHECK( depthFrame->CopyFrameDataToArray( depthBuffer.size(), &depthBuffer[0] ) ); | |
} | |
// Draw | |
void draw() | |
{ | |
// Draw Color | |
drawColor(); | |
// Draw Depth | |
drawDepth(); | |
} | |
// Draw Color | |
void drawColor() | |
{ | |
// Retrieve Mapped Coordinates | |
std::vector<ColorSpacePoint> colorSpace( depthWidth * depthHeight ); | |
ERROR_CHECK( mapper->MapDepthFrameToColorSpace( depthBuffer.size(), &depthBuffer[0], colorSpace.size(), &colorSpace[0] ) ); | |
// Mapping Depth to Color Resolution | |
std::vector<BYTE> buffer( depthWidth * depthHeight * colorBytesPerPixel ); | |
for( int depthY = 0; depthY < depthHeight; depthY++ ){ | |
for( int depthX = 0; depthX < depthWidth; depthX++ ){ | |
unsigned int depthIndex = depthY * depthWidth + depthX; | |
int colorX = static_cast<int>( colorSpace[depthIndex].X + 0.5f ); | |
int colorY = static_cast<int>( colorSpace[depthIndex].Y + 0.5f ); | |
if( ( 0 <= colorX ) && ( colorX < colorWidth ) && ( 0 <= colorY ) && ( colorY < colorHeight ) ){ | |
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]; | |
} | |
} | |
} | |
// Set Coordinate Buffer to cv::Mat | |
colorMat = cv::Mat( depthHeight, depthWidth, CV_8UC4, &buffer[0] ).clone(); | |
} | |
// Draw Depth | |
void drawDepth() | |
{ | |
// Retrieve Mapped Coordinates | |
std::vector<DepthSpacePoint> depthSpace( colorWidth * colorHeight ); | |
ERROR_CHECK( mapper->MapColorFrameToDepthSpace( depthBuffer.size(), &depthBuffer[0], depthSpace.size(), &depthSpace[0] ) ); | |
// Mapping Depth to Color Resolution | |
std::vector<UINT16> buffer( colorWidth * colorHeight ); | |
for ( int colorY = 0; colorY < colorHeight; colorY++ ){ | |
for ( int colorX = 0; colorX < colorWidth; colorX++ ){ | |
unsigned int colorIndex = colorY * colorWidth + colorX; | |
int depthX = static_cast<int>( depthSpace[colorIndex].X + 0.5f ); | |
int depthY = static_cast<int>( depthSpace[colorIndex].Y + 0.5f ); | |
if ( ( 0 <= depthX ) && ( depthX < depthWidth ) && ( 0 <= depthY ) && ( depthY < depthHeight ) ){ | |
unsigned int depthIndex = depthY * depthWidth + depthX; | |
buffer[colorIndex] = depthBuffer[depthIndex]; | |
} | |
} | |
} | |
// Set Coordinate Buffer to cv::Mat | |
cv::Mat rawMat = cv::Mat( colorHeight, colorWidth, CV_16UC1, &buffer[0] ).clone(); | |
// Convert Raw(16bit) to 8bit | |
rawMat.convertTo( depthMat, CV_8U, -255.0f / 8000.0f, 255.0f ); // 255(white) - 0(black) | |
//rawMat.convertTo( depthMat, CV_8U, 255.0f / 8000.0f, 0.0f ); // 0(black) - 255(white) | |
} | |
// Show | |
void show() | |
{ | |
// Show Color | |
showColor(); | |
// Show Depth | |
showDepth(); | |
} | |
// Show Color | |
void showColor() | |
{ | |
// Show Color Data | |
cv::imshow( "Color", colorMat ); | |
} | |
// Show Depth | |
void showDepth() | |
{ | |
// Resize Image | |
const double scale = 0.5; | |
cv::Mat resizeMat; | |
cv::resize( depthMat, resizeMat, cv::Size(), scale, scale ); | |
// Show Depth Data | |
cv::imshow( "Depth", resizeMat ); | |
} | |
}; | |
void main() | |
{ | |
try { | |
Kinect kinect; | |
kinect.run(); | |
} | |
catch ( std::exception& ex ){ | |
std::cout << ex.what() << std::endl; | |
} | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Optimized Mapping Loop
Optimization Method
How to use OpenMP
Optimized Code
Performance
In the most general terms, optimized code is about 5.5 - 6.0x as fast as non optimized code on 4-core 8-thread CPU.
The rest of the time would be secured sufficient for real-time processing.
More Optimization