Beispiel #1
0
    Extractor() {
        tree.reset(new pcl::KdTreeFLANN<Point > ());
        cloud.reset(new pcl::PointCloud<Point>);
        cloud_filtered.reset(new pcl::PointCloud<Point>);
        cloud_normals.reset(new pcl::PointCloud<pcl::Normal>);
        coefficients_plane_.reset(new pcl::ModelCoefficients);
        coefficients_cylinder_.reset(new pcl::ModelCoefficients);
        inliers_plane_.reset(new pcl::PointIndices);
        inliers_cylinder_.reset(new pcl::PointIndices);

        // Filter Pass
        pass.setFilterFieldName("z");
        pass.setFilterLimits(-100, 100);

        // VoxelGrid for Downsampling
        LEAFSIZE = 0.01f;
        vg.setLeafSize(LEAFSIZE, LEAFSIZE, LEAFSIZE);

        // Any object < CUT_THRESHOLD will be abandoned.
        //CUT_THRESHOLD = (int) (LEAFSIZE * LEAFSIZE * 7000000); // 700
        CUT_THRESHOLD =  700; //for nonfiltering

        // Clustering
        cluster_.setClusterTolerance(0.06 * UNIT);
        cluster_.setMinClusterSize(50.0);
        cluster_.setSearchMethod(clusters_tree_);

        // Normals
        ne.setSearchMethod(tree);
        ne.setKSearch(50); // 50 by default

        // plane SAC
        seg_plane.setOptimizeCoefficients(true);
        seg_plane.setModelType(pcl::SACMODEL_NORMAL_PLANE);
        seg_plane.setNormalDistanceWeight(0.1); // 0.1
        seg_plane.setMethodType(pcl::SAC_RANSAC);
        seg_plane.setMaxIterations(1000); // 10000
        seg_plane.setDistanceThreshold(0.05); // 0.03

        // cylinder SAC
        seg_cylinder.setOptimizeCoefficients(true);
        seg_cylinder.setModelType(pcl::SACMODEL_CYLINDER);
        seg_cylinder.setMethodType(pcl::SAC_RANSAC);
        seg_cylinder.setNormalDistanceWeight(0.1);
        seg_cylinder.setMaxIterations(10000);
        seg_cylinder.setDistanceThreshold(0.02); // 0.05
        seg_cylinder.setRadiusLimits(0.02, 0.07); // [0, 0.1]
    }
Beispiel #2
0
    void initialize(bool toFilter) {
        // A passthrough filter to remove spurious NaNs
        pass.setInputCloud(cloud);
        pass.filter(*cloud_filtered);
        std::cout << "PointCloud after pass through has: " <<
                cloud_filtered->points.size() << " data points." << std::endl;


        // Downsample the dataset using a leaf size of 1cm
        // After filtering the point cloud, all indices do not point to the
        // original points. Therefore disable this if called from initializeFromPointCLoud
        if (toFilter) {
            vg.setInputCloud(cloud_filtered);
            vg.filter(*cloud_filtered);
            std::cout << "PointCloud after filtering has: " <<
                    cloud_filtered->points.size() << " data points." << std::endl;
        }
        // Estimate point normals
        ne.setInputCloud(cloud_filtered);
        ne.compute(*cloud_normals);
        ROS_INFO("%lu normals estimated", cloud_normals->points.size());
    }
Beispiel #3
0
    void initialize(const std::string & pcd_name) {
        // Read in the cloud data
        reader.read(pcd_name, *cloud);
        std::cerr << "PointCloud has: " << cloud->points.size() << " data points." << std::endl;

        // pcl::io::loadPCDFile(pcd_name, *cloud);

        // A passthrough filter to remove spurious NaNs
        pass.setInputCloud(cloud);
        pass.filter(*cloud_filtered);
        std::cout << "PointCloud after filtering has: " <<
                cloud_filtered->points.size() << " data points." << std::endl;

        // Downsample the dataset using a leaf size of 1cm
        vg.setInputCloud(cloud_filtered);
        vg.filter(*cloud_filtered);
        std::cout << "PointCloud after filtering has: " <<
                cloud_filtered->points.size() << " data points." << std::endl;

        // Estimate point normals
        ne.setInputCloud(cloud_filtered);
        ne.compute(*cloud_normals);
        ROS_INFO("%lu normals estimated", cloud_normals->points.size());
    }
  void vad_cb(const sensor_msgs::PointCloud2ConstPtr& cloud) {
      if ((cloud->width * cloud->height) == 0)
        return;
      //ROS_INFO ("Received %d data points in frame %s with the following fields: %s", (int)cloud->width * cloud->height, cloud->header.frame_id.c_str (), pcl::getFieldsList (*cloud).c_str ());
      //std::cout << "fromROSMsg?" << std::endl;
      pcl::fromROSMsg (*cloud, cloud_xyz_);
      //std::cout << "  fromROSMsg done." << std::endl;
      t0 = my_clock();

      if( limitPoint( cloud_xyz_, cloud_xyz, distance_th ) > 10 ){
	//std::cout << "  limit done." << std::endl;
	std::cout << "compute normals and voxelize...." << std::endl;

	//****************************************
	//* compute normals
	n3d.setInputCloud (cloud_xyz.makeShared());
	n3d.setRadiusSearch (normals_radius_search);
	normals_tree = boost::make_shared<pcl::KdTreeFLANN<pcl::PointXYZ> > ();
	n3d.setSearchMethod (normals_tree);
	n3d.compute (cloud_normal);
	pcl::concatenateFields (cloud_xyz, cloud_normal, cloud_xyz_normal);

	t0_2 = my_clock();
	
	//* voxelize
	getVoxelGrid( grid, cloud_xyz_normal, cloud_downsampled, voxel_size );
	std::cout << "     ...done.." << std::endl;
	
	const int pnum = cloud_downsampled.points.size();
	float x_min = 10000000, y_min = 10000000, z_min = 10000000;
	float x_max = -10000000, y_max = -10000000, z_max = -10000000;
	for( int p=0; p<pnum; p++ ){
	  if( cloud_downsampled.points[ p ].x < x_min ) x_min = cloud_downsampled.points[ p ].x;
	  if( cloud_downsampled.points[ p ].y < y_min ) y_min = cloud_downsampled.points[ p ].y;
	  if( cloud_downsampled.points[ p ].z < z_min ) z_min = cloud_downsampled.points[ p ].z;
	  if( cloud_downsampled.points[ p ].x > x_max ) x_max = cloud_downsampled.points[ p ].x;
	  if( cloud_downsampled.points[ p ].y > y_max ) y_max = cloud_downsampled.points[ p ].y;
	  if( cloud_downsampled.points[ p ].z > z_max ) z_max = cloud_downsampled.points[ p ].z;
	}
	//std::cout << x_min << " " << y_min << " " << z_min << std::endl;
	//std::cout << x_max << " " << y_max << " " << z_max << std::endl;
	//std::cout << grid.getMinBoxCoordinates() << std::endl;

	std::cout << "search start..." << std::endl;
	//****************************************
	//* object detection start
	t1 = my_clock();
	search_obj.cleanData();
	search_obj.setGRSD( dim, grid, cloud_xyz_normal, cloud_downsampled, voxel_size, box_size );
	t1_2 = my_clock();
	if( ( search_obj.XYnum() != 0 ) && ( search_obj.Znum() != 0 ) )
	  search_obj.searchWithoutRotation();
	t2 = my_clock();
	//* object detection end
	//****************************************
	std::cout << "  ...search done." << std::endl;
	
	tAll += t2 - t0;
	process_count++;
	std::cout << "normal estimation  :"<< t0_2 - t0 << " sec" << std::endl;
	std::cout << "voxelize           :"<< t1 - t0_2 << " sec" << std::endl;
	std::cout << "feature extraction : "<< t1_2 - t1 << " sec" <<std::endl;
	std::cout << "search             : "<< t2 - t1_2 << " sec" <<std::endl;
	std::cout << "all processes      : "<< t2 - t0 << " sec" << std::endl;
	std::cout << "AVERAGE            : "<< tAll / process_count << " sec" << std::endl;
	marker_pub_ = nh_.advertise<visualization_msgs::Marker>("visualization_marker_range", 1); 
	marker_array_pub_ = nh_.advertise<visualization_msgs::MarkerArray>("visualization_marker_array", 100);
	visualization_msgs::MarkerArray marker_array_msg_;
	
	//* show the limited space
	visualization_msgs::Marker marker_;
	marker_.header.frame_id = "openni_rgb_optical_frame";
	marker_.header.stamp = ros::Time::now();
	marker_.ns = "BoxEstimation";
	marker_.id = -1;
	marker_.type = visualization_msgs::Marker::CUBE;
	marker_.action = visualization_msgs::Marker::ADD;
	marker_.pose.position.x = (x_max+x_min)/2;
	marker_.pose.position.y = (y_max+y_min)/2;
	marker_.pose.position.z = (z_max+z_min)/2;
	marker_.pose.orientation.x = 0;
	marker_.pose.orientation.y = 0;
	marker_.pose.orientation.z = 0;
	marker_.pose.orientation.w = 1;
	marker_.scale.x = x_max-x_min;
	marker_.scale.y = x_max-x_min;
	marker_.scale.z = x_max-x_min;
	marker_.color.a = 0.1;
	marker_.color.r = 1.0;
	marker_.color.g = 0.0;
	marker_.color.b = 0.0;
	marker_.lifetime = ros::Duration();
	marker_pub_.publish(marker_);
	
	for( int q=0; q<rank_num; q++ ){
	  if( search_obj.maxDot( q ) < detect_th ) break;
	  std::cout << search_obj.maxX( q ) << " " << search_obj.maxY( q ) << " " << search_obj.maxZ( q ) << std::endl;
	  std::cout << "dot " << search_obj.maxDot( q ) << std::endl;
	  //if( (search_obj.maxX( q )!=0)||(search_obj.maxY( q )!=0)||(search_obj.maxZ( q )!=0) ){
	  //* publish marker
	  visualization_msgs::Marker marker_;
	  //marker_.header.frame_id = "base_link";
	  marker_.header.frame_id = "openni_rgb_optical_frame";
	  marker_.header.stamp = ros::Time::now();
	  marker_.ns = "BoxEstimation";
	  marker_.id = q;
	  marker_.type = visualization_msgs::Marker::CUBE;
	  marker_.action = visualization_msgs::Marker::ADD;
	  marker_.pose.position.x = search_obj.maxX( q ) * region_size + sliding_box_size_x/2 + x_min;
	  marker_.pose.position.y = search_obj.maxY( q ) * region_size + sliding_box_size_y/2 + y_min;
	  marker_.pose.position.z = search_obj.maxZ( q ) * region_size + sliding_box_size_z/2 + z_min;
	  marker_.pose.orientation.x = 0;
	  marker_.pose.orientation.y = 0;
	  marker_.pose.orientation.z = 0;
	  marker_.pose.orientation.w = 1;
	  marker_.scale.x = sliding_box_size_x;
	  marker_.scale.y = sliding_box_size_y;
	  marker_.scale.z = sliding_box_size_z;
	  marker_.color.a = 0.5;
	  marker_.color.r = 0.0;
	  marker_.color.g = 1.0;
	  marker_.color.b = 0.0;
	  marker_.lifetime = ros::Duration();
	  // std::cerr << "BOX MARKER COMPUTED, WITH FRAME " << marker_.header.frame_id << " POSITION: " 
	  // 	    << marker_.pose.position.x << " " << marker_.pose.position.y << " " 
	  // 	    << marker_.pose.position.z << std::endl;
	  //   marker_pub_.publish (marker_);    
	  // }
	  marker_array_msg_.markers.push_back(marker_);
	}
	//std::cerr << "MARKER ARRAY published with size: " << marker_array_msg_.markers.size() << std::endl; 
	marker_array_pub_.publish(marker_array_msg_);
      }
      std::cout << "Waiting msg..." << std::endl;
  }