Channel16u mapDepthFrameToColor( const Channel16u& depth, ICoordinateMapper* mapper )
{
size_t numPoints = depth.getWidth() * depth.getHeight();
Channel16u channel( depth.getWidth(), depth.getHeight() );
vector<ColorSpacePoint> colorSpacePoints( numPoints );
long hr = mapper->MapDepthFrameToColorSpace( (UINT)numPoints, depth.getData(), numPoints, &colorSpacePoints[ 0 ] );
if ( SUCCEEDED( hr ) ) {
Channel16u::Iter iter = channel.getIter();
size_t i = 0;
while ( iter.line() ) {
while ( iter.pixel() ) {
Vec2i pos = Vec2i( toVec2f( colorSpacePoints[ i ] ) );
uint16_t v = 0x0000;
if ( pos.x >= 0 && pos.x < depth.getWidth() && pos.y >= 0 && pos.y < depth.getHeight() ) {
v = depth.getValue( pos );
}
iter.v() = v;
}
}
}
return channel;
}
cv::Mat Kinectv2::getColorDepthImage()
{
std::vector<ColorSpacePoint> colorSpacePoints(depthBuffer.size());
hResult = pCoordinateMapper->MapDepthFrameToColorSpace(depthBuffer.size(), &depthBuffer[0], colorSpacePoints.size(), &colorSpacePoints[0]);
cv::Mat colorDepthImage(depthHeight, depthWidth, CV_8UC4);
if(SUCCEEDED(hResult))
{
for(int i = 0;i < depthWidth * depthHeight;i++)
{
int x = (int)colorSpacePoints[i].X;
int y = (int)colorSpacePoints[i].Y;
int ColorBytesPerPixel = 4;
int srcIndex = y * colorWidth + x;
// int srcIndex = ((y * colorWidth / 2 + x)) * ColorBytesPerPixel;
int destIndex = i * ColorBytesPerPixel;
if(((0 <= x) && (x < colorWidth) && (0 <= y) && (y < colorHeight)))
{
colorDepthImage.data[destIndex + 0] = colorBuffer[srcIndex].rgbBlue;
colorDepthImage.data[destIndex + 1] = colorBuffer[srcIndex].rgbGreen;
colorDepthImage.data[destIndex + 2] = colorBuffer[srcIndex].rgbRed;
}else {
colorDepthImage.data[destIndex + 0] = 255;
colorDepthImage.data[destIndex + 1] = 255;
colorDepthImage.data[destIndex + 2] = 255;
}
}
cv::flip(colorDepthImage, colorDepthImage, 1);
}
return colorDepthImage;
}