void pcl::io::OpenNI2Grabber::depthCallback (DepthImage::Ptr depth_image, void*) { if (num_slots<sig_cb_openni_point_cloud_rgb> () > 0 || num_slots<sig_cb_openni_point_cloud_rgba> () > 0 || num_slots<sig_cb_openni_image_depth_image> () > 0) rgb_sync_.add1 (depth_image, depth_image->getTimestamp ()); if (num_slots<sig_cb_openni_point_cloud_i> () > 0 || num_slots<sig_cb_openni_ir_depth_image> () > 0) ir_sync_.add1 (depth_image, depth_image->getTimestamp ()); if (depth_image_signal_->num_slots () > 0) depth_image_signal_->operator ()(depth_image); if (point_cloud_signal_->num_slots () > 0) point_cloud_signal_->operator ()(convertToXYZPointCloud (depth_image)); }
pcl::PointCloud<pcl::PointXYZI>::Ptr pcl::io::OpenNI2Grabber::convertToXYZIPointCloud (const IRImage::Ptr &ir_image, const DepthImage::Ptr &depth_image) { boost::shared_ptr<pcl::PointCloud<pcl::PointXYZI> > cloud (new pcl::PointCloud<pcl::PointXYZI > ()); cloud->header.seq = depth_image->getFrameID (); cloud->header.stamp = depth_image->getTimestamp (); cloud->header.frame_id = rgb_frame_id_; cloud->height = depth_height_; cloud->width = depth_width_; cloud->is_dense = false; cloud->points.resize (cloud->height * cloud->width); float fx = device_->getDepthFocalLength (); // Horizontal focal length float fy = device_->getDepthFocalLength (); // Vertcal focal length float cx = ((float)cloud->width - 1.f) / 2.f; // Center x float cy = ((float)cloud->height - 1.f) / 2.f; // Center y // Load pre-calibrated camera parameters if they exist if (pcl_isfinite (depth_parameters_.focal_length_x)) fx = static_cast<float> (depth_parameters_.focal_length_x); if (pcl_isfinite (depth_parameters_.focal_length_y)) fy = static_cast<float> (depth_parameters_.focal_length_y); if (pcl_isfinite (depth_parameters_.principal_point_x)) cx = static_cast<float> (depth_parameters_.principal_point_x); if (pcl_isfinite (depth_parameters_.principal_point_y)) cy = static_cast<float> (depth_parameters_.principal_point_y); float fx_inv = 1.0f / fx; float fy_inv = 1.0f / fy; const uint16_t* depth_map = (const uint16_t*) depth_image->getData (); if (depth_image->getWidth () != depth_width_ || depth_image->getHeight () != depth_height_) { // Resize the image if nessacery depth_resize_buffer_.resize(depth_width_ * depth_height_); depth_map = depth_resize_buffer_.data(); depth_image->fillDepthImageRaw (depth_width_, depth_height_, (unsigned short*) depth_map ); } const uint16_t* ir_map = (const uint16_t*) ir_image->getData (); if (ir_image->getWidth () != depth_width_ || ir_image->getHeight () != depth_height_) { // Resize the image if nessacery ir_resize_buffer_.resize(depth_width_ * depth_height_); ir_map = ir_resize_buffer_.data(); ir_image->fillRaw (depth_width_, depth_height_, (unsigned short*) ir_map); } int depth_idx = 0; float bad_point = std::numeric_limits<float>::quiet_NaN (); for (int v = 0; v < depth_height_; ++v) { for (int u = 0; u < depth_width_; ++u, ++depth_idx) { pcl::PointXYZI& pt = cloud->points[depth_idx]; /// @todo Different values for these cases // Check for invalid measurements if (depth_map[depth_idx] == 0 || depth_map[depth_idx] == depth_image->getNoSampleValue () || depth_map[depth_idx] == depth_image->getShadowValue ()) { pt.x = pt.y = pt.z = bad_point; } else { pt.z = depth_map[depth_idx] * 0.001f; // millimeters to meters pt.x = (static_cast<float> (u) - cx) * pt.z * fx_inv; pt.y = (static_cast<float> (v) - cy) * pt.z * fy_inv; } pt.data_c[0] = pt.data_c[1] = pt.data_c[2] = pt.data_c[3] = 0; pt.intensity = static_cast<float> (ir_map[depth_idx]); } } cloud->sensor_origin_.setZero (); cloud->sensor_orientation_.setIdentity (); return (cloud); }
template <typename PointT> typename pcl::PointCloud<PointT>::Ptr pcl::io::OpenNI2Grabber::convertToXYZRGBPointCloud (const Image::Ptr &image, const DepthImage::Ptr &depth_image) { boost::shared_ptr<pcl::PointCloud<PointT> > cloud (new pcl::PointCloud<PointT>); cloud->header.seq = depth_image->getFrameID (); cloud->header.stamp = depth_image->getTimestamp (); cloud->header.frame_id = rgb_frame_id_; cloud->height = std::max (image_height_, depth_height_); cloud->width = std::max (image_width_, depth_width_); cloud->is_dense = false; cloud->points.resize (cloud->height * cloud->width); // Generate default camera parameters float fx = device_->getDepthFocalLength (); // Horizontal focal length float fy = device_->getDepthFocalLength (); // Vertcal focal length float cx = ((float)depth_width_ - 1.f) / 2.f; // Center x float cy = ((float)depth_height_- 1.f) / 2.f; // Center y // Load pre-calibrated camera parameters if they exist if (pcl_isfinite (depth_parameters_.focal_length_x)) fx = static_cast<float> (depth_parameters_.focal_length_x); if (pcl_isfinite (depth_parameters_.focal_length_y)) fy = static_cast<float> (depth_parameters_.focal_length_y); if (pcl_isfinite (depth_parameters_.principal_point_x)) cx = static_cast<float> (depth_parameters_.principal_point_x); if (pcl_isfinite (depth_parameters_.principal_point_y)) cy = static_cast<float> (depth_parameters_.principal_point_y); // Get inverse focal length for calculations below float fx_inv = 1.0f / fx; float fy_inv = 1.0f / fy; const uint16_t* depth_map = (const uint16_t*) depth_image->getData (); if (depth_image->getWidth () != depth_width_ || depth_image->getHeight () != depth_height_) { // Resize the image if nessacery depth_resize_buffer_.resize(depth_width_ * depth_height_); depth_map = depth_resize_buffer_.data(); depth_image->fillDepthImageRaw (depth_width_, depth_height_, (unsigned short*) depth_map ); } const uint8_t* rgb_buffer = (const uint8_t*) image->getData (); if (image->getWidth () != image_width_ || image->getHeight () != image_height_) { // Resize the image if nessacery color_resize_buffer_.resize(image_width_ * image_height_ * 3); rgb_buffer = color_resize_buffer_.data(); image->fillRGB (image_width_, image_height_, (unsigned char*) rgb_buffer, image_width_ * 3); } float bad_point = std::numeric_limits<float>::quiet_NaN (); // set xyz to Nan and rgb to 0 (black) if (image_width_ != depth_width_) { PointT pt; pt.x = pt.y = pt.z = bad_point; pt.b = pt.g = pt.r = 0; pt.a = 255; // point has no color info -> alpha = max => transparent cloud->points.assign (cloud->points.size (), pt); } // fill in XYZ values unsigned step = cloud->width / depth_width_; unsigned skip = cloud->width - (depth_width_ * step); int value_idx = 0; int point_idx = 0; for (int v = 0; v < depth_height_; ++v, point_idx += skip) { for (int u = 0; u < depth_width_; ++u, ++value_idx, point_idx += step) { PointT& pt = cloud->points[point_idx]; /// @todo Different values for these cases // Check for invalid measurements OniDepthPixel pixel = depth_map[value_idx]; if (pixel != 0 && pixel != depth_image->getNoSampleValue () && pixel != depth_image->getShadowValue () ) { pt.z = depth_map[value_idx] * 0.001f; // millimeters to meters pt.x = (static_cast<float> (u) - cx) * pt.z * fx_inv; pt.y = (static_cast<float> (v) - cy) * pt.z * fy_inv; } else { pt.x = pt.y = pt.z = bad_point; } } } // fill in the RGB values step = cloud->width / image_width_; skip = cloud->width - (image_width_ * step); value_idx = 0; point_idx = 0; RGBValue color; color.Alpha = 0xff; for (unsigned yIdx = 0; yIdx < image_height_; ++yIdx, point_idx += skip) { for (unsigned xIdx = 0; xIdx < image_width_; ++xIdx, point_idx += step, value_idx += 3) { PointT& pt = cloud->points[point_idx]; color.Red = rgb_buffer[value_idx]; color.Green = rgb_buffer[value_idx + 1]; color.Blue = rgb_buffer[value_idx + 2]; pt.rgba = color.long_value; } } cloud->sensor_origin_.setZero (); cloud->sensor_orientation_.setIdentity (); return (cloud); }
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl::io::OpenNI2Grabber::convertToXYZPointCloud (const DepthImage::Ptr& depth_image) { pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud <pcl::PointXYZ>); cloud->header.seq = depth_image->getFrameID (); cloud->header.stamp = depth_image->getTimestamp (); cloud->height = depth_height_; cloud->width = depth_width_; cloud->is_dense = false; cloud->points.resize (cloud->height * cloud->width); float constant_x = 1.0f / device_->getDepthFocalLength (); float constant_y = 1.0f / device_->getDepthFocalLength (); float centerX = ((float)cloud->width - 1.f) / 2.f; float centerY = ((float)cloud->height - 1.f) / 2.f; if (pcl_isfinite (depth_parameters_.focal_length_x)) constant_x = 1.0f / static_cast<float> (depth_parameters_.focal_length_x); if (pcl_isfinite (depth_parameters_.focal_length_y)) constant_y = 1.0f / static_cast<float> (depth_parameters_.focal_length_y); if (pcl_isfinite (depth_parameters_.principal_point_x)) centerX = static_cast<float> (depth_parameters_.principal_point_x); if (pcl_isfinite (depth_parameters_.principal_point_y)) centerY = static_cast<float> (depth_parameters_.principal_point_y); if ( device_->isDepthRegistered() ) cloud->header.frame_id = rgb_frame_id_; else cloud->header.frame_id = depth_frame_id_; float bad_point = std::numeric_limits<float>::quiet_NaN (); const uint16_t* depth_map = (const uint16_t*) depth_image->getData (); if (depth_image->getWidth () != depth_width_ || depth_image->getHeight () != depth_height_) { // Resize the image if nessacery depth_resize_buffer_.resize(depth_width_ * depth_height_); depth_image->fillDepthImageRaw (depth_width_, depth_height_, (uint16_t*) depth_resize_buffer_.data() ); depth_map = depth_resize_buffer_.data(); } int depth_idx = 0; for (int v = 0; v < depth_height_; ++v) { for (int u = 0; u < depth_width_; ++u, ++depth_idx) { pcl::PointXYZ& pt = cloud->points[depth_idx]; // Check for invalid measurements if (depth_map[depth_idx] == 0 || depth_map[depth_idx] == depth_image->getNoSampleValue () || depth_map[depth_idx] == depth_image->getShadowValue ()) { // not valid pt.x = pt.y = pt.z = bad_point; continue; } pt.z = depth_map[depth_idx] * 0.001f; pt.x = (static_cast<float> (u) - centerX) * pt.z * constant_x; pt.y = (static_cast<float> (v) - centerY) * pt.z * constant_y; } } cloud->sensor_origin_.setZero (); cloud->sensor_orientation_.w () = 1.0f; cloud->sensor_orientation_.x () = 0.0f; cloud->sensor_orientation_.y () = 0.0f; cloud->sensor_orientation_.z () = 0.0f; return (cloud); }