//******************************************************************
// Get depth image out of the disparity image
//******************************************************************
Matrix<double> AncillaryMethods::GetDepthLibelas(int frame_nr, const Camera& camera, double baseline)
{
//================================================
// Read in the disparity image
//================================================
char dispImagePath[128];
sprintf(dispImagePath, Globals::tempDepthL.c_str(), frame_nr);
string path_string(dispImagePath);
Matrix<float> image(Globals::dImWidth, Globals::dImHeight, 0.0);
if(path_string.find(".txt") != string::npos)
{
image.ReadFromTXT(dispImagePath);
}
else
{
image.ReadFromBinaryFile(dispImagePath);
}
//================================================
// Compute the depth map
//================================================
Matrix<double> depthMap(Globals::dImWidth, Globals::dImHeight, 0.0);
Matrix<double> intrinsics = camera.get_K();
double focalLength = intrinsics(0,0);
double par = (baseline*focalLength);
for(int i = 0; i < Globals::dImWidth; i++)
{
for(int j = 0; j < Globals::dImHeight; j++)
{
depthMap(i,j) = max(0.0, min(30.0,(par/(image(i,j)))/1000.0));
}
}
return depthMap;
}
void runOnce (void)
{
xn::SceneMetaData sceneMD;
xn::DepthMetaData depthMD_;
// Read next available data
// by Heresy, wait original depth generator
g_Context.WaitOneUpdateAll( g_DepthGenerator );
// Process the data
g_DepthGenerator.GetMetaData (depthMD_);
g_DepthGenerator.GetMetaData (depthMD);
depthMD_.MakeDataWritable();
xn::DepthMap& depthMap_ = depthMD_.WritableDepthMap();
xn::DepthMap& depthMap = depthMD.WritableDepthMap();
static float *buffer = 0;
if (buffer == 0)
{
std::cout << "(re)allocating depth buffer" << std::endl;
buffer = (float*) malloc (depthMap.XRes() * depthMap.YRes() * sizeof(float));
}
for (XnUInt y = 0; y < depthMap.YRes(); y++)
{
for (XnUInt x = 0; x < depthMap.XRes(); x++)
{
buffer [x + y * depthMap.XRes()] = depthMap (depthMap.XRes() - x - 1, y) * 0.001;
}
}
float P[12];
P[0] = 585.260; P[1] = 0.0; P[2] = 317.387; P[3] = 0.0;
P[4] = 0.0; P[5] = 585.028; P[6] = 239.264; P[7] = 0.0;
P[8] = 0.0; P[9] = 0.0; P[10] = 1.0; P[11] = 0.0;
double fx = P[0];
double fy = P[5];
double cx = P[2];
double cy = P[6];
double far_plane_ = 8;
double near_plane_ = 0.1;
double glTf[16];
for (unsigned int i = 0; i < 16; ++i)
glTf[i] = 0.0;
// calculate the projection matrix
// NOTE: this minus is there to flip the x-axis of the image.
glTf[0]= -2.0 * fx / depthMap.XRes();
glTf[5]= 2.0 * fy / depthMap.YRes();
glTf[8]= 2.0 * (0.5 - cx / depthMap.XRes());
glTf[9]= 2.0 * (cy / depthMap.YRes() - 0.5);
glTf[10]= - (far_plane_ + near_plane_) / (far_plane_ - near_plane_);
glTf[14]= -2.0 * far_plane_ * near_plane_ / (far_plane_ - near_plane_);
glTf[11]= -1;
filter->filter ((unsigned char*)buffer, glTf, depthMap.XRes(), depthMap.YRes());
GLfloat* masked_depth = filter->getMaskedDepth();
for (XnUInt y = 0; y < depthMap.YRes(); y++)
{
for (XnUInt x = 0; x < depthMap.XRes(); x++)
{
depthMap_(x, y) = XnDepthPixel (masked_depth[x + y * depthMap.XRes()] * 1000);
}
}
mockDepth.SetData(depthMD_);
publishTransforms (std::string("openni_depth_frame"));
}
double GenericObjectDetector::ComputeDepthVariance(Rect win)
{
cv::Scalar mean, stddev;
meanStdDev(depthMap(win), mean, stddev);
return 1.f / (stddev.val[0]+0.000005f);
}
