void PointCloudToMaskImage::convert(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg) { vital_checker_->poke(); pcl::PointCloud<pcl::PointXYZ>::Ptr pc(new pcl::PointCloud<pcl::PointXYZ>); pcl::fromROSMsg(*cloud_msg, *pc); if (!pc->isOrganized()) { NODELET_FATAL("Input point cloud is not organized."); return; } cv::Mat mask_image = cv::Mat::zeros(cloud_msg->height, cloud_msg->width, CV_8UC1); for (size_t index = 0; index < pc->points.size(); index++) { if (isnan(pc->points[index].x) || isnan(pc->points[index].y) || isnan(pc->points[index].z)) { continue; } int width_index = index % cloud_msg->width; int height_index = index / cloud_msg->width; mask_image.at<uchar>(height_index, width_index) = 255; } cv_bridge::CvImage mask_bridge(cloud_msg->header, sensor_msgs::image_encodings::MONO8, mask_image); pub_.publish(mask_bridge.toImageMsg()); }
void PointIndicesToMaskImage::mask( const PCLIndicesMsg::ConstPtr& indices_msg, const sensor_msgs::Image::ConstPtr& image_msg) { vital_checker_->poke(); int width = image_msg->width; int height = image_msg->height; cv::Mat mask_image = cv::Mat::zeros(height, width, CV_8UC1); for (size_t i = 0; i < indices_msg->indices.size(); i++) { int index = indices_msg->indices[i]; int width_index = index % width; int height_index = index / width; mask_image.at<uchar>(height_index, width_index) = 255; } cv_bridge::CvImage mask_bridge(indices_msg->header, sensor_msgs::image_encodings::MONO8, mask_image); pub_.publish(mask_bridge.toImageMsg()); }