Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 #define BOOST_PARAMETER_MAX_ARITY 7
00037 #include "jsk_pcl_ros/mask_image_cluster_filter.h"
00038 #include <cv_bridge/cv_bridge.h>
00039 #include <image_geometry/pinhole_camera_model.h>
00040 #include "jsk_pcl_ros/pcl_conversion_util.h"
00041 #include <sensor_msgs/image_encodings.h>
00042 #include <pcl/filters/extract_indices.h>
00043
00044
00045 namespace jsk_pcl_ros
00046 {
00047 void MaskImageClusterFilter::onInit()
00048 {
00049 DiagnosticNodelet::onInit();
00050 pub_ = advertise<PCLIndicesMsg>(
00051 *pnh_, "output", 1);
00052 }
00053
00054 void MaskImageClusterFilter::subscribe()
00055 {
00056 sub_image_ = pnh_->subscribe("input/mask", 1,
00057 &MaskImageClusterFilter::imageCalback, this);
00058 sub_info_ = pnh_->subscribe("input/camera_info", 1,
00059 &MaskImageClusterFilter::infoCalback, this);
00060 sub_input_.subscribe(*pnh_, "input", 1);
00061 sub_target_.subscribe(*pnh_, "target", 1);
00062 sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
00063 sync_->connectInput(sub_input_, sub_target_);
00064 sync_->registerCallback(boost::bind(&MaskImageClusterFilter::concat, this, _1, _2));
00065 }
00066
00067 void MaskImageClusterFilter::unsubscribe()
00068 {
00069 sub_image_.shutdown();
00070 sub_info_.shutdown();
00071 sub_input_.unsubscribe();
00072 sub_target_.unsubscribe();
00073 }
00074
00075 void MaskImageClusterFilter::infoCalback(
00076 const sensor_msgs::CameraInfo::ConstPtr& info_ms)
00077 {
00078 boost::mutex::scoped_lock lock(mutex_);
00079 camera_info_ = info_ms;
00080 }
00081
00082 void MaskImageClusterFilter::imageCalback(
00083 const sensor_msgs::Image::ConstPtr& mask_msg)
00084 {
00085 boost::mutex::scoped_lock lock(mutex_);
00086 cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(
00087 mask_msg, sensor_msgs::image_encodings::MONO8);
00088 mask_image_ = cv_ptr->image;
00089 }
00090
00091 void MaskImageClusterFilter::concat(
00092 const sensor_msgs::PointCloud2::ConstPtr& cloud_msg,
00093 const jsk_recognition_msgs::ClusterPointIndicesConstPtr &indices_input)
00094 {
00095 boost::mutex::scoped_lock lock(mutex_);
00096 if (camera_info_ && !mask_image_.empty()) {
00097 image_geometry::PinholeCameraModel model;
00098 model.fromCameraInfo(camera_info_);
00099 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud
00100 (new pcl::PointCloud<pcl::PointXYZRGB>);
00101 pcl::fromROSMsg(*cloud_msg, *cloud);
00102 PCLIndicesMsg indices;
00103 indices.header = cloud_msg->header;
00104 pcl::ExtractIndices<pcl::PointXYZRGB> extract;
00105 extract.setInputCloud(cloud);
00106 for (size_t i = 0; i < indices_input->cluster_indices.size(); i++) {
00107 pcl::PointCloud<pcl::PointXYZRGB>::Ptr segmented_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
00108 pcl::IndicesPtr vindices;
00109 vindices.reset (new std::vector<int> (indices_input->cluster_indices[i].indices));
00110 extract.setIndices(vindices);
00111 extract.filter(*segmented_cloud);
00112 bool in_mask = false;
00113 for (size_t j = 0; j < segmented_cloud->points.size(); j++) {
00114 pcl::PointXYZRGB p = segmented_cloud->points[j];
00115 cv::Point2d uv = model.project3dToPixel(cv::Point3d(p.x, p.y, p.z));
00116
00117 if (uv.x > 0 && uv.x < mask_image_.cols &&
00118 uv.y > 0 && uv.y < mask_image_.rows) {
00119 if (mask_image_.at<uchar>(uv.y, uv.x) == 255) {
00120 in_mask = true;
00121 break;
00122 }
00123 }
00124 }
00125 if (in_mask) {
00126 vindices->size();
00127 for(size_t j=0; j < vindices->size(); j++)
00128 {
00129 indices.indices.push_back((*vindices)[j]);
00130 }
00131 }
00132 }
00133 pub_.publish(indices);
00134 }
00135 }
00136 }
00137
00138 #include <pluginlib/class_list_macros.h>
00139 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::MaskImageClusterFilter, nodelet::Nodelet);
00140
00141