XnPoint3D xncv::projectiveToWorld(const cv::Point& point, XnFloat z, const xn::DepthGenerator& depth)
{
XnPoint3D p;
p.X = static_cast<XnFloat>(point.x);
p.Y = static_cast<XnFloat>(point.y);
p.Z = static_cast<XnFloat>(z);
XnPoint3D p2;
depth.ConvertProjectiveToRealWorld(1, &p, &p2);
return p2;
}
IplImage* CvCapture_OpenNI::retrievePointCloudMap()
{
if( !depthMetaData.Data() )
return 0;
cv::Mat depth;
getDepthMapFromMetaData( depthMetaData, depth, noSampleValue, shadowValue );
const int badPoint = INVALID_PIXEL_VAL;
const float badCoord = INVALID_COORDINATE_VAL;
int cols = depthMetaData.XRes(), rows = depthMetaData.YRes();
cv::Mat pointCloud_XYZ( rows, cols, CV_32FC3, cv::Scalar::all(badPoint) );
cv::Ptr<XnPoint3D> proj = new XnPoint3D[cols*rows];
cv::Ptr<XnPoint3D> real = new XnPoint3D[cols*rows];
for( int y = 0; y < rows; y++ )
{
for( int x = 0; x < cols; x++ )
{
int ind = y*cols+x;
proj[ind].X = (float)x;
proj[ind].Y = (float)y;
proj[ind].Z = depth.at<unsigned short>(y, x);
}
}
depthGenerator.ConvertProjectiveToRealWorld(cols*rows, proj, real);
for( int y = 0; y < rows; y++ )
{
for( int x = 0; x < cols; x++ )
{
// Check for invalid measurements
if( depth.at<unsigned short>(y, x) == badPoint ) // not valid
pointCloud_XYZ.at<cv::Point3f>(y,x) = cv::Point3f( badCoord, badCoord, badCoord );
else
{
int ind = y*cols+x;
pointCloud_XYZ.at<cv::Point3f>(y,x) = cv::Point3f( real[ind].X*0.001f, real[ind].Y*0.001f, real[ind].Z*0.001f); // from mm to meters
}
}
}
outputMaps[CV_CAP_OPENNI_POINT_CLOUD_MAP].mat = pointCloud_XYZ;
return outputMaps[CV_CAP_OPENNI_POINT_CLOUD_MAP].getIplImagePtr();
}
int _tmain(int argc, _TCHAR* argv[])
{
InitialKinect();
std::stringstream out_yml_filename;
cv::Mat Read_Depth;
cv::Mat c16BitDepth;
for (int k = 0; k < image_number; k++)
{
out_yml_filename.str( *(depthImgName+k) );
cv::FileStorage fsd( out_yml_filename.str(), cv::FileStorage::READ);
fsd["frameCount"] >> c16BitDepth;
c16BitDepth.convertTo( Read_Depth, CV_32FC1);
int idxShift, idx;
XnPoint3D* pDepthPointSet = new XnPoint3D[ 640 * 480 ];
pRealWorldCamaraPointSet = new XnPoint3D[ 640 * 480 ];
for ( int j = 0; j < 480; ++j )
{
idxShift = j * 640;
for ( int i = 0; i < 640; ++i)
{
idx = idxShift + i;
pDepthPointSet[idx].X = (int)i;
pDepthPointSet[idx].Y = (int)j;
pDepthPointSet[idx].Z = (int)Read_Depth.at<float>(idx);
}
}
mDepthGenerator.ConvertProjectiveToRealWorld( 640 * 480, pDepthPointSet, pRealWorldCamaraPointSet );
WriteToFile(pRealWorldCamaraPointSet, k);
cout << "Image " << RealNumber[k] << " Done!!\n";
fsd.release();
delete [] pDepthPointSet;
delete [] pRealWorldCamaraPointSet;
}
return 0;
}
void DepthMapLogger::DumpDepthMap(const xn::DepthMetaData& dmd, const xn::SceneMetaData& smd)
{
static char name_str[20], comment_str[255];
// Don't do anything if the h5 file is not open
if(!p_h5_file_ || !p_frames_group_) { return; }
// References to various bits of the HDF5 output
H5File &file(*p_h5_file_);
Group &frames_group(*p_frames_group_);
// This frame's index is the number of frames we've previously saved
hsize_t this_frame_idx = frames_group.getNumObjs();
// Create this frame's group
snprintf(name_str, 20, "frame_%06lld", this_frame_idx);
snprintf(comment_str, 255, "Data for frame %lld", this_frame_idx);
Group this_frame_group(frames_group.createGroup(name_str));
this_frame_group.setComment(".", comment_str);
// Create attributes for this group
Attribute idx_attr = this_frame_group.createAttribute("idx", PredType::NATIVE_HSIZE, DataSpace());
idx_attr.write(PredType::NATIVE_HSIZE, &this_frame_idx);
// Create this frame's datasets
DSetCreatPropList creat_props;
uint16_t fill_value(0);
creat_props.setFillValue(PredType::NATIVE_UINT16, &fill_value);
hsize_t rows(static_cast<hsize_t>(dmd.YRes())), cols(static_cast<hsize_t>(dmd.XRes()));
hsize_t creation_dims[2] = { rows, cols };
hsize_t max_dims[2] = { rows, cols };
DataSpace mem_space(2, creation_dims, max_dims);
DataSet depth_ds(this_frame_group.createDataSet(
"depth", PredType::NATIVE_UINT16, mem_space, creat_props));
DataSet label_ds(this_frame_group.createDataSet(
"label", PredType::NATIVE_UINT16, mem_space, creat_props));
// Get depth and label buffers
const uint16_t *p_depths = dmd.Data();
const uint16_t *p_labels = smd.Data();
// Write depth data
depth_ds.write(p_depths, PredType::NATIVE_UINT16);
// Write label data
label_ds.write(p_labels, PredType::NATIVE_UINT16);
// Convert non-zero depth values into 3D point positions
XnPoint3D *pts = new XnPoint3D[rows*cols];
uint16_t *pt_labels = new uint16_t[rows*cols];
size_t n_pts(0);
for(size_t depth_idx(0); depth_idx < rows*cols; ++depth_idx) {
// Skip zero depth values
if(p_depths[depth_idx] == 0) {
continue;
}
// Store projective-values
pts[n_pts].X = depth_idx % cols;
pts[n_pts].Y = depth_idx / cols;
pts[n_pts].Z = p_depths[depth_idx];
pt_labels[n_pts] = p_labels[depth_idx];
++n_pts;
}
g_DepthGenerator.ConvertProjectiveToRealWorld(n_pts, pts, pts);
if (n_pts > 0)
{
// Create points dataset
hsize_t pts_creation_dims[2] = { n_pts, 3 };
hsize_t pts_max_dims[2] = { n_pts, 3 };
DataSpace pts_mem_space(2, pts_creation_dims, pts_max_dims);
DataSet pts_ds(this_frame_group.createDataSet(
"points", PredType::NATIVE_FLOAT, pts_mem_space, creat_props));
hsize_t pt_labels_creation_dims[1] = { n_pts };
hsize_t pt_labels_max_dims[1] = { n_pts };
DataSpace pt_labels_mem_space(1, pt_labels_creation_dims, pt_labels_max_dims);
DataSet pt_labels_ds(this_frame_group.createDataSet(
"point_labels", PredType::NATIVE_UINT16, pt_labels_mem_space, creat_props));
// Write points data
pts_ds.write(pts, PredType::NATIVE_FLOAT);
pt_labels_ds.write(pt_labels, PredType::NATIVE_UINT16);
}
// Create groups to store detected users
Group users_group(this_frame_group.createGroup("users"));
// Dump each user in turn
char strLabel[50] = "";
XnUserID aUsers[15];
XnUInt16 nUsers = 15;
g_UserGenerator.GetUsers(aUsers, nUsers);
for (int i = 0; i < nUsers; ++i)
{
// Create a group for this user
snprintf(name_str, 20, "user_%02d", aUsers[i]);
Group this_user_group(users_group.createGroup(name_str));
// Create attributes for this group
Attribute user_idx_attr = this_user_group.createAttribute(
"idx", PredType::NATIVE_UINT16, DataSpace());
uint16_t this_user_idx(aUsers[i]);
user_idx_attr.write(PredType::NATIVE_UINT16, &this_user_idx);
// Write state (if any)
H5std_string strwritebuf;
if (g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i]))
{
strwritebuf = "tracking";
}
else if (g_UserGenerator.GetSkeletonCap().IsCalibrating(aUsers[i]))
{
// Calibrating
strwritebuf = "calibrating";
}
else
{
// Nothing
strwritebuf = "looking";
}
StrType strdatatype(PredType::C_S1, strwritebuf.size()); // of length 256 characters
Attribute user_state_attr = this_user_group.createAttribute(
"state", strdatatype, DataSpace());
user_state_attr.write(strdatatype, strwritebuf);
static Joint joints[g_NumJointTypes];
int n_joints_found = 0;
// Try to dump all joints
for(int jt_idx=0; jt_idx < g_NumJointTypes; ++jt_idx)
{
if(DumpJoint(aUsers[i], g_JointTypes[jt_idx], joints[n_joints_found]))
{
++n_joints_found;
}
}
if (n_joints_found > 0)
{
// Create joints dataset
hsize_t joints_dim[] = { n_joints_found };
DataSpace joints_space(1, joints_dim);
DataSet joints_ds(this_user_group.createDataSet("joints", joint_dt_, joints_space));
joints_ds.write(joints, joint_dt_);
}
}
}
/* The matlab mex function */
void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )
{
double *Iout;
XnUInt64 *MXadress;
if(nrhs==0)
{
printf("Open failed: Give Pointer to Kinect as input\n");
mexErrMsgTxt("Kinect Error");
}
MXadress = (XnUInt64*)mxGetData(prhs[0]);
if(MXadress[0]>0){ g_Context = ((xn::Context*) MXadress[0])[0]; }
if(MXadress[2]>0)
{
g_DepthGenerator = ((xn::DepthGenerator*) MXadress[2])[0];
}
else
{
mexErrMsgTxt("No Depth Node in Kinect Context");
}
XnStatus nRetVal = XN_STATUS_OK;
xn::DepthMetaData depthMD;
// Process the data
g_DepthGenerator.GetMetaData(depthMD);
XnUInt16 g_nXRes = depthMD.FullXRes();
XnUInt16 g_nYRes = depthMD.FullYRes();
const XnDepthPixel* pDepth = depthMD.Data();
int Jdimsc[3];
Jdimsc[0]=g_nXRes;
Jdimsc[1]=g_nYRes;
Jdimsc[2]=3;
int npixels=Jdimsc[0]*Jdimsc[1];
int index;
XnPoint3D *pt_proj =new XnPoint3D[npixels];
XnPoint3D *pt_world=new XnPoint3D[npixels];
for(int y=0; y<Jdimsc[1]; y++)
{
for(int x=0; x<Jdimsc[0]; x++)
{
index=x+y*Jdimsc[0];
pt_proj[index].X=(XnFloat)x;
pt_proj[index].Y=(XnFloat)y;
pt_proj[index].Z=pDepth[x+y*Jdimsc[0]];
}
}
g_DepthGenerator.ConvertProjectiveToRealWorld ( (XnUInt32)npixels, pt_proj, pt_world);
plhs[0] = mxCreateNumericArray(3, Jdimsc, mxDOUBLE_CLASS, mxREAL);
Iout = mxGetPr(plhs[0]);
for(int y=0; y<Jdimsc[1]; y++)
{
for(int x=0; x<Jdimsc[0]; x++)
{
index=x+y*Jdimsc[0];
Iout[index]=pt_world[index].X;
Iout[index+npixels]=pt_world[index].Y;
Iout[index+npixels*2]=pt_world[index].Z;
}
}
delete[] pt_proj;
delete[] pt_world;
}
