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 #include "jsk_pcl_ros/organized_edge_detector.h"
00037 #include "jsk_pcl_ros/pcl_conversion_util.h"
00038
00039 #include <pcl/features/organized_edge_detection.h>
00040 #include <pcl/features/integral_image_normal.h>
00041 #include "jsk_pcl_ros/pcl_util.h"
00042 #include <cv_bridge/cv_bridge.h>
00043 #include <sensor_msgs/image_encodings.h>
00044
00045 #include <opencv2/opencv.hpp>
00046 #include <jsk_recognition_msgs/ClusterPointIndices.h>
00047
00048 namespace jsk_pcl_ros
00049 {
00050 void OrganizedEdgeDetector::onInit()
00051 {
00053
00055 pub_nan_boundary_edges_indices_
00056 = advertise<PCLIndicesMsg>(*pnh_, "output_nan_boundary_edge_indices", 1);
00057 pub_occluding_edges_indices_
00058 = advertise<PCLIndicesMsg>(*pnh_, "output_occluding_edge_indices", 1);
00059 pub_occluded_edges_indices_
00060 = advertise<PCLIndicesMsg>(*pnh_, "output_occluded_edge_indices", 1);
00061 pub_curvature_edges_indices_
00062 = advertise<PCLIndicesMsg>(*pnh_, "output_curvature_edge_indices", 1);
00063 pub_rgb_edges_indices_
00064 = advertise<PCLIndicesMsg>(*pnh_, "output_rgb_edge_indices", 1);
00065 pub_all_edges_indices_
00066 = advertise<PCLIndicesMsg>(*pnh_, "output_indices", 1);
00067 pub_straight_edges_indices_
00068 = advertise<jsk_recognition_msgs::ClusterPointIndices>(*pnh_,
00069 "output_straight_edges_indices", 1);
00071
00073 pub_normal_ = advertise<sensor_msgs::PointCloud2>(*pnh_, "output_normal", 1);
00074 pub_nan_boundary_edges_
00075 = advertise<sensor_msgs::PointCloud2>(*pnh_, "output_nan_boundary_edge", 1);
00076 pub_occluding_edges_
00077 = advertise<sensor_msgs::PointCloud2>(*pnh_, "output_occluding_edge", 1);
00078 pub_occluded_edges_
00079 = advertise<sensor_msgs::PointCloud2>(*pnh_, "output_occluded_edge", 1);
00080 pub_curvature_edges_
00081 = advertise<sensor_msgs::PointCloud2>(*pnh_, "output_curvature_edge", 1);
00082 pub_rgb_edges_
00083 = advertise<sensor_msgs::PointCloud2>(*pnh_, "output_rgb_edge", 1);
00084 pub_all_edges_
00085 = advertise<sensor_msgs::PointCloud2>(*pnh_, "output", 1);
00087
00089 image_transport::ImageTransport it(*pnh_);
00090 pub_edge_image_ = it.advertise("edge_image", 1);
00091 pub_hough_image_ = it.advertise("hough_image", 1);
00093
00095 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
00096 dynamic_reconfigure::Server<Config>::CallbackType f =
00097 boost::bind (&OrganizedEdgeDetector::configCallback, this, _1, _2);
00098 srv_->setCallback (f);
00099 }
00100
00101 void OrganizedEdgeDetector::subscribe()
00102 {
00103 sub_ = pnh_->subscribe("input", 1, &OrganizedEdgeDetector::estimate, this);
00104 }
00105
00106 void OrganizedEdgeDetector::unsubscribe()
00107 {
00108 sub_.shutdown();
00109 }
00110
00111 void OrganizedEdgeDetector::estimateNormal(
00112 const pcl::PointCloud<PointT>::Ptr& input,
00113 pcl::PointCloud<pcl::Normal>::Ptr output,
00114 const std_msgs::Header& header)
00115 {
00116 pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne;
00117 if (estimation_method_ == 0) {
00118 ne.setNormalEstimationMethod (ne.AVERAGE_3D_GRADIENT);
00119 }
00120 else if (estimation_method_ == 1) {
00121 ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
00122 }
00123 else if (estimation_method_ == 2) {
00124 ne.setNormalEstimationMethod (ne.AVERAGE_DEPTH_CHANGE);
00125 }
00126 else {
00127 JSK_NODELET_FATAL("unknown estimation method: %d", estimation_method_);
00128 return;
00129 }
00130
00131 if (border_policy_ignore_) {
00132 ne.setBorderPolicy(pcl::IntegralImageNormalEstimation<PointT, pcl::Normal>::BORDER_POLICY_IGNORE);
00133 }
00134 else {
00135 ne.setBorderPolicy(pcl::IntegralImageNormalEstimation<PointT, pcl::Normal>::BORDER_POLICY_MIRROR);
00136 }
00137
00138 ne.setMaxDepthChangeFactor(max_depth_change_factor_);
00139 ne.setNormalSmoothingSize(normal_smoothing_size_);
00140 ne.setDepthDependentSmoothing(depth_dependent_smoothing_);
00141 ne.setInputCloud(input);
00142 ne.compute(*output);
00143 if (publish_normal_) {
00144 sensor_msgs::PointCloud2 ros_output;
00145 pcl::toROSMsg(*output, ros_output);
00146 ros_output.header = header;
00147 pub_normal_.publish(ros_output);
00148 }
00149 }
00150
00151 void OrganizedEdgeDetector::configCallback (Config &config, uint32_t level)
00152 {
00153 boost::mutex::scoped_lock lock(mutex_);
00154 max_depth_change_factor_ = config.max_depth_change_factor;
00155 normal_smoothing_size_ = config.normal_smoothing_size;
00156 depth_dependent_smoothing_ = config.depth_dependent_smoothing;
00157 estimation_method_ = config.estimation_method;
00158 border_policy_ignore_ = config.border_policy_ignore;
00159 max_search_neighbors_ = config.max_search_neighbors;
00160 depth_discontinuation_threshold_ = config.depth_discontinuation_threshold;
00161 publish_normal_ = config.publish_normal;
00162 use_nan_boundary_ = config.use_nan_boundary;
00163 use_occluding_ = config.use_occluding;
00164 use_occluded_ = config.use_occluded;
00165 use_curvature_ = config.use_curvature;
00166 use_rgb_ = config.use_rgb;
00167 use_straightline_detection_ = config.use_straightline_detection;
00168 rho_ = config.rho;
00169 theta_ = config.theta;
00170 straightline_threshold_ = config.straightline_threshold;
00171 min_line_length_ = config.min_line_length;
00172 max_line_gap_ = config.max_line_gap;
00173 publish_debug_image_ = config.publish_debug_image;
00174 }
00175
00176 void OrganizedEdgeDetector::estimateEdge(
00177 const pcl::PointCloud<PointT>::Ptr& input,
00178 const pcl::PointCloud<pcl::Normal>::Ptr& normal,
00179 pcl::PointCloud<pcl::Label>::Ptr& output,
00180 std::vector<pcl::PointIndices>& label_indices)
00181 {
00182 pcl::OrganizedEdgeFromRGBNormals<PointT, pcl::Normal, pcl::Label> oed;
00183 oed.setDepthDisconThreshold (depth_discontinuation_threshold_);
00184 oed.setMaxSearchNeighbors (max_search_neighbors_);
00185 int flags = 0;
00186 if (use_nan_boundary_) {
00187 flags |= oed.EDGELABEL_NAN_BOUNDARY;
00188 }
00189 if (use_occluding_) {
00190 flags |= oed.EDGELABEL_OCCLUDING;
00191 }
00192 if (use_occluded_) {
00193 flags |= oed.EDGELABEL_OCCLUDED;
00194 }
00195 if (use_curvature_) {
00196 flags |= oed.EDGELABEL_HIGH_CURVATURE;
00197 }
00198 if (use_rgb_) {
00199 flags |= oed.EDGELABEL_RGB_CANNY;
00200 }
00201 oed.setEdgeType (flags);
00202 oed.setInputNormals(normal);
00203 oed.setInputCloud(input);
00204 oed.compute(*output, label_indices);
00205 }
00206
00207 void OrganizedEdgeDetector::publishIndices(
00208 ros::Publisher& pub,
00209 ros::Publisher& pub_indices,
00210 const pcl::PointCloud<PointT>::Ptr& cloud,
00211 const std::vector<int>& indices,
00212 const std_msgs::Header& header)
00213 {
00215
00217 PCLIndicesMsg msg;
00218 msg.header = header;
00219 msg.indices = indices;
00220 pub_indices.publish(msg);
00221
00223
00225 pcl::PointCloud<PointT>::Ptr output(new pcl::PointCloud<PointT>);
00226 pcl::copyPointCloud(*cloud, indices, *output);
00227 sensor_msgs::PointCloud2 ros_output;
00228 pcl::toROSMsg(*output, ros_output);
00229 ros_output.header = header;
00230 pub.publish(ros_output);
00231 }
00232
00233 void OrganizedEdgeDetector::publishStraightEdges(
00234 const pcl::PointCloud<PointT>::Ptr& cloud,
00235 const std_msgs::Header& header,
00236 const std::vector<std::vector<int> > indices)
00237 {
00238
00239 jsk_recognition_msgs::ClusterPointIndices ros_msg;
00240 ros_msg.header = header;
00241 ros_msg.cluster_indices.resize(indices.size());
00242 for (size_t i = 0; i < indices.size(); i++) {
00243 PCLIndicesMsg ros_indices;
00244 ros_indices.header = header;
00245 ros_indices.indices = indices[i];
00246 ros_msg.cluster_indices[i] = ros_indices;
00247 }
00248 pub_straight_edges_indices_.publish(ros_msg);
00249 }
00250
00251 void OrganizedEdgeDetector::estimateStraightEdges(
00252 const pcl::PointCloud<PointT>::Ptr& cloud,
00253 const std::vector<int>& indices,
00254 const std_msgs::Header& header,
00255 std::vector<std::vector<int> >& output_indices)
00256 {
00257
00258 cv::Mat mat = cv::Mat(cloud->height, cloud->width, CV_8UC1) * 0;
00259 for (size_t i = 0; i < indices.size(); i++) {
00260 int index = indices[i];
00261 int index_height = index / cloud->width;
00262 int index_width = index % cloud->width;
00263 mat.data[index_height * mat.step + index_width * mat.elemSize()] = 255;
00264 }
00265
00266 std::vector<cv::Vec4i> lines;
00267 cv::HoughLinesP(mat, lines, rho_, theta_ * CV_PI/180,
00268 straightline_threshold_, min_line_length_, max_line_gap_);
00269 output_indices.resize(lines.size());
00270 std::set<int> all_indices_set(indices.begin(), indices.end());
00271 for (size_t i_line = 0; i_line < lines.size(); i_line++) {
00272 std::vector<int> pixels;
00273 cv::LineIterator it(mat,
00274 cv::Point(lines[i_line][0], lines[i_line][1]),
00275 cv::Point(lines[i_line][2], lines[i_line][3]), 4);
00276 for(int i_pixel = 0; i_pixel < it.count; i_pixel++, ++it) {
00277 cv::Point point = it.pos();
00278 int flatten_index = point.x + point.y * cloud->width;
00279
00280 if (all_indices_set.find(flatten_index) != all_indices_set.end()) {
00281 pixels.push_back(flatten_index);
00282 }
00283 }
00284 output_indices[i_line] = pixels;
00285 }
00286 if (publish_debug_image_) {
00287 cv::Mat color_dst;
00288 cv::cvtColor(mat, color_dst, CV_GRAY2BGR );
00289 for( size_t i = 0; i < lines.size(); i++ )
00290 {
00291 cv::line( color_dst,
00292 cv::Point(lines[i][0], lines[i][1]),
00293 cv::Point(lines[i][2], lines[i][3]), cv::Scalar(0,0,255), 3, 8);
00294 }
00295 sensor_msgs::Image::Ptr ros_edge_image
00296 = cv_bridge::CvImage(header,
00297 sensor_msgs::image_encodings::MONO8,
00298 mat).toImageMsg();
00299 pub_edge_image_.publish(ros_edge_image);
00300 sensor_msgs::Image::Ptr ros_hough_image
00301 = cv_bridge::CvImage(header,
00302 sensor_msgs::image_encodings::BGR8,
00303 color_dst).toImageMsg();
00304 pub_hough_image_.publish(ros_hough_image);
00305 }
00306 }
00307
00308 void OrganizedEdgeDetector::estimate(
00309 const sensor_msgs::PointCloud2::ConstPtr& msg)
00310 {
00311 boost::mutex::scoped_lock lock(mutex_);
00312 if (msg->height == 1) {
00313 JSK_NODELET_ERROR("[OrganizedEdgeDetector] organized pointcloud is required");
00314 return;
00315 }
00316 pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
00317 pcl::PointCloud<pcl::Normal>::Ptr normal(new pcl::PointCloud<pcl::Normal>);
00318 pcl::PointCloud<pcl::Label>::Ptr label(new pcl::PointCloud<pcl::Label>);
00319 std::vector<pcl::PointIndices> label_indices;
00320 pcl::fromROSMsg(*msg, *cloud);
00321
00322 estimateNormal(cloud, normal, msg->header);
00323 estimateEdge(cloud, normal, label, label_indices);
00325
00327 std::vector<int> tmp1 = addIndices(label_indices[0].indices,
00328 label_indices[1].indices);
00329 std::vector<int> tmp2 = addIndices(tmp1,
00330 label_indices[2].indices);
00331 std::vector<int> tmp3 = addIndices(tmp2,
00332 label_indices[3].indices);
00333 std::vector<int> all = addIndices(tmp3,
00334 label_indices[4].indices);
00335 if (use_straightline_detection_) {
00336 std::vector<std::vector<int> > straightline_indices;
00337 estimateStraightEdges(cloud, all, msg->header, straightline_indices);
00338
00339 publishStraightEdges(cloud, msg->header, straightline_indices);
00340 }
00342
00344 publishIndices(pub_nan_boundary_edges_, pub_nan_boundary_edges_indices_,
00345 cloud,
00346 label_indices[0].indices, msg->header);
00347 publishIndices(pub_occluding_edges_, pub_occluding_edges_indices_,
00348 cloud,
00349 label_indices[1].indices, msg->header);
00350 publishIndices(pub_occluded_edges_, pub_occluded_edges_indices_,
00351 cloud,
00352 label_indices[2].indices, msg->header);
00353 publishIndices(pub_curvature_edges_, pub_curvature_edges_indices_,
00354 cloud,
00355 label_indices[3].indices, msg->header);
00356 publishIndices(pub_rgb_edges_, pub_rgb_edges_indices_,
00357 cloud,
00358 label_indices[4].indices, msg->header);
00359 publishIndices(pub_all_edges_, pub_all_edges_indices_,
00360 cloud,
00361 all, msg->header);
00362 }
00363 }
00364
00365 #include <pluginlib/class_list_macros.h>
00366 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::OrganizedEdgeDetector, nodelet::Nodelet);