void Inspector::updateDepth(const openni_wrapper::Image& image,
const openni_wrapper::DepthImage& depth)
{
frame_.depth_->setZero();
ushort data[depth.getHeight() * depth.getWidth()];
depth.fillDepthImageRaw(depth.getWidth(), depth.getHeight(), data);
int i = 0;
for(size_t y = 0; y < depth.getHeight(); ++y) {
for(size_t x = 0; x < depth.getWidth(); ++x, ++i) {
if(data[i] == depth.getNoSampleValue() || data[i] == depth.getShadowValue())
continue;
frame_.depth_->coeffRef(y, x) = data[i];
}
}
if(dddm_ && use_intrinsics_)
dddm_->undistort(frame_.depth_.get());
frame_.img_ = oniToCV(image);
Cloud::Ptr cloud(new Cloud);
FrameProjector proj;
proj.width_ = 640;
proj.height_ = 480;
proj.cx_ = proj.width_ / 2;
proj.cy_ = proj.height_ / 2;
proj.fx_ = 525;
proj.fy_ = 525;
proj.frameToCloud(frame_, cloud.get());
pcd_vis_.updatePointCloud(cloud, "cloud");
pcd_vis_.spinOnce(5);
}
void
DataCapture::saveDepthmap (const std::string &filename, const openni_wrapper::DepthImage &depthmap, unsigned char depth_res) const
{
// get meta data associated with the depht image
xn::DepthMetaData& md = const_cast<xn::DepthMetaData&> (depthmap.getDepthMetaData());
/* cout << "depth image data:" << endl;
cout << " - focal length = " << depthmap.getFocalLength() << endl;
cout << " - base line = " << depthmap.getBaseline() << endl;
cout << " - depth res. = " << md.ZRes() << endl;
*/
// copy depth values into a buffer
int width = depthmap.getWidth(), height = depthmap.getHeight();
unsigned short* buffer = new unsigned short[width*height];
depthmap.fillDepthImageRaw (width, height, buffer);
cv::Mat_<unsigned short> mat16bit (height, width, buffer);
// scale according to the depth resolution provided by the sensor
double scale = (std::pow(2.0, depth_res) - 1.0) / md.ZRes();
std::cout << "Max resolution of depth sensor is: " << md.ZRes() << std::endl;
switch (depth_res)
{
case 8: {
// convert from 16 to 8 bit per depth value
cv::Mat mat8bit (height, width, CV_8U);
mat16bit.convertTo ( mat8bit, CV_8U, scale);
cv::imwrite (filename + "_depth.png", mat8bit);
break;
}
case 16: {
//mat16bit *= scale;
cv::imwrite (filename + "_depth.png", mat16bit);
break;
}
default: {
std::cerr << "[saveDepthmap] Depth size of " << depth_res << " not supported (use 8 or 16)" << std::endl;
break;
}
}
// std::cout << "Depth image saved as PNG (" << (int)depth_res << ") to " << filename << "_depth.png" << std::endl;
}
void OpenNINode::publishDepthImage(const openni_wrapper::DepthImage& depth, image_transport::CameraPublisher depth_pub) const {
sensor_msgs::ImagePtr depth_msg = boost::make_shared<sensor_msgs::Image>();
ros::Time time = ros::Time::now();
depth_msg->header.stamp = time;
depth_msg->encoding = sensor_msgs::image_encodings::TYPE_32FC1;
depth_msg->width = depth_width_;
depth_msg->height = depth_height_;
depth_msg->step = depth_msg->width * sizeof(float);
depth_msg->data.resize(depth_msg->height * depth_msg->step);
depth.fillDepthImage(depth_msg->width, depth_msg->height, reinterpret_cast<float*>(&depth_msg->data[0]), depth_msg->step);
depth_pub.publish(depth_msg, getDepthCameraInfo(depth_msg->width, depth_msg->height, time, 1));
//pub.publish(depth_msg);
}