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;
}
const vector<ivec2>& CinderDSAPI::mapDepthToColorFrame()
{
mDepthToColor.clear();
Channel16u::Iter cIter = mDepthFrame.getIter();
while (cIter.line())
{
while (cIter.pixel())
{
float cZ[3]{static_cast<float>(cIter.x()),
static_cast<float>(cIter.y()),
static_cast<float>(cIter.v())
};
float cRgb[2]{0};
DSTransformFromZImageToRectOtherImage(mZIntrinsics, mZToRgb, mRgbIntrinsics, cZ, cRgb);
mDepthToColor.push_back(ivec2((int)cRgb[0], (int)cRgb[1]));
}
}
return mDepthToColor;
}