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());
}
