osaOpenNI::Errno osaOpenNI::GetRGBImage(vctDynamicMatrix<unsigned char>& RGBimage) {
xn::ImageMetaData rgbMD;
Data->rgbgenerator.GetMetaData(rgbMD);
// create image
RGBimage.SetSize(rgbMD.YRes(), rgbMD.XRes()*3);
memcpy(RGBimage.Pointer(), rgbMD.Data(), rgbMD.YRes()*rgbMD.XRes()*3*sizeof(unsigned char));
return osaOpenNI::ESUCCESS;
}
void osaOpenNI::GetDepthImage(vctDynamicMatrix<double>& placeHolder) {
// Get depth data
xn::DepthMetaData depthMD;
Data->depthgenerator.GetMetaData(depthMD);
const XnDepthPixel* pDepth = depthMD.Data();
placeHolder.SetSize(depthMD.YRes(), depthMD.XRes());
double* ptr = placeHolder.Pointer();
const size_t end = depthMD.YRes()*depthMD.XRes();
for (size_t i = 0; i < end; i++)
{
(*ptr) = 255.0 * (*pDepth) / 2048.0;
ptr++; pDepth++;
}
}
osaOpenNI::Errno osaOpenNI::GetDepthImageRaw(vctDynamicMatrix<double> & depthimage) {
// Get data
xn::DepthMetaData depthMD;
Data->depthgenerator.GetMetaData(depthMD);
const XnDepthPixel* src = depthMD.Data();
depthimage.SetSize(depthMD.YRes(), depthMD.XRes());
double* dest = depthimage.Pointer();
const size_t N = depthMD.YRes()*depthMD.XRes();
for (size_t i = 0; i<N; i++) {
(*dest) = (*src);
src++;
dest++;
}
return osaOpenNI::ESUCCESS;
}
osaOpenNI::Errno
osaOpenNI::Convert3DToProjectiveMask(const vctDynamicMatrix<double>& rangedata,
vctDynamicMatrix<bool>& mask) {
// allocate the arrays
XnUInt32 cnt = rangedata.cols();
XnPoint3D* wrld = new XnPoint3D[ cnt ];
XnPoint3D* proj = new XnPoint3D[ cnt ];
// copy the 3d points to the 3d array
for (XnUInt32 i=0; i<cnt; i++) {
wrld[i].X = -rangedata[0][i]*1000.0;
wrld[i].Y = rangedata[1][i]*1000.0;
wrld[i].Z = rangedata[2][i]*1000.0;
}
// convert to projective
XnStatus status;
status = Data->depthgenerator.ConvertRealWorldToProjective(cnt, wrld, proj);
if (status != XN_STATUS_OK) {
CMN_LOG_RUN_ERROR << "Failed to convert world to projective: "
<< xnGetStatusString(status) << std::endl;
return osaOpenNI::EFAILURE;
}
// use this to find the size of the mask
xn::DepthMetaData depthMD;
Data->depthgenerator.GetMetaData(depthMD);
mask.SetSize(depthMD.YRes(), depthMD.XRes());
mask.SetAll(false);
for (XnUInt32 i=0; i<cnt; i++) {
mask[ proj[i].Y ][ proj[i].X ] = true;
}
delete[] wrld;
delete[] proj;
return osaOpenNI::ESUCCESS;
}
osaOpenNI::Errno
osaOpenNI::GetRangeData(vctDynamicMatrix<double>& rangedata,
const std::vector< vctFixedSizeVector<unsigned short,2> >& pixels) {
// Get data
xn::DepthMetaData depthMD;
Data->depthgenerator.GetMetaData(depthMD);
// create arrays
XnUInt32 cnt;
XnPoint3D* proj = NULL;
XnPoint3D* wrld = NULL;
if (pixels.empty()) {
// create arrays
cnt = depthMD.XRes()*depthMD.YRes();
proj = new XnPoint3D[ cnt ];
wrld = new XnPoint3D[ cnt ];
// Create projective coordinates
for (size_t i=0, x=0; x<depthMD.XRes(); x++) {
for (size_t y=0; y<depthMD.YRes(); i++, y++) {
proj[i].X = (XnFloat)x;
proj[i].Y = (XnFloat)y;
proj[i].Z = depthMD(x,y);
}
}
}
else{
// create arrays
cnt = pixels.size();
proj = new XnPoint3D[ cnt ];
wrld = new XnPoint3D[ cnt ];
for (size_t i=0; i<pixels.size(); i++) {
unsigned int x = pixels[i][0];
unsigned int y = pixels[i][1];
proj[i].X = (XnFloat)x;
proj[i].Y = (XnFloat)y;
proj[i].Z = depthMD(x,y);
}
}
// Convert projective to 3D
XnStatus status = Data->depthgenerator.ConvertProjectiveToRealWorld(cnt, proj, wrld);
if (status != XN_STATUS_OK) {
CMN_LOG_RUN_ERROR << "Failed to convert projective to world: "
<< xnGetStatusString(status) << std::endl;
}
// create matrix
rangedata.SetSize(3, cnt);
for (size_t i=0; i<cnt; i++) {
rangedata[0][i] = -wrld[i].X/1000.0;
rangedata[1][i] = wrld[i].Y/1000.0;
rangedata[2][i] = wrld[i].Z/1000.0;
}
delete[] proj;
delete[] wrld;
return osaOpenNI::ESUCCESS;
}