void ParticleFilterTracking::initTargetModel(const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &cloud,
                                              pcl::PointCloud<pcl::PointXYZRGBA>::Ptr &segmented_cloud)
  {
      std::vector<pcl::PointIndices> cluster_indices;
      euclideanSegment (cloud, cluster_indices);

      // select the cluster to track
      pcl::PointCloud<pcl::PointXYZRGBA>::Ptr temp_cloud (new pcl::PointCloud<pcl::PointXYZRGBA>);
      extractSegmentCluster (cloud, cluster_indices, 0, *temp_cloud);
      Eigen::Vector4f c;
      pcl::compute3DCentroid<pcl::PointXYZRGBA> (*temp_cloud, c);
      int segment_index = 0;
      double segment_distance = c[0] * c[0] + c[1] * c[1];

      //choose most near cloud to z axis.
      for (size_t i = 1; i < cluster_indices.size (); i++)
        {
          temp_cloud.reset (new pcl::PointCloud<pcl::PointXYZRGBA>);
          extractSegmentCluster (cloud, cluster_indices, int (i), *temp_cloud);
          pcl::compute3DCentroid<pcl::PointXYZRGBA> (*temp_cloud, c);
          double distance = c[0] * c[0] + c[1] * c[1];
          if (distance < segment_distance)
            {
              segment_index = int (i);
              segment_distance = distance;
            }
        }
      extractSegmentCluster (cloud, cluster_indices, segment_index, *segmented_cloud);
  }
Exemplo n.º 2
0
    CloudPtr
    get ()
    {
      //lock while we swap our cloud and reset it.
      boost::mutex::scoped_lock lock (mtx_);
      CloudPtr temp_cloud (new Cloud);
      CloudPtr temp_cloud2 (new Cloud);

      grid_.setInputCloud (cloud_);
      grid_.filter (*temp_cloud);

      pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
      pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());

      seg_.setInputCloud (temp_cloud);
      seg_.segment (*inliers, *coefficients);

      // Project the model inliers 
      proj_.setInputCloud (temp_cloud);
      proj_.setModelCoefficients (coefficients);
      proj_.filter (*temp_cloud2);

      // Create a Convex Hull representation of the projected inliers
      chull_.setInputCloud (temp_cloud2);
      chull_.reconstruct (*temp_cloud);

      return (temp_cloud);
    }
Exemplo n.º 3
0
    CloudPtr
    get ()
    {
      //lock while we swap our cloud and reset it.
      boost::mutex::scoped_lock lock (mtx_);
      CloudPtr temp_cloud (new Cloud);
     
      grid_.setInputCloud (cloud_);
      grid_.filter (*temp_cloud);

      return (temp_cloud);
    }
Exemplo n.º 4
0
 void pcdCloud::updateReservedIndices()
 {
     if(!_removedIndices->empty()){
         CloudPtr temp_cloud(new Cloud);
         extract(temp_cloud, _removedIndices, _reservedIndices);
         
         if(VERBOSE) std::cout << "update: # _reservedIndices = " << _reservedIndices->size() << std::endl;
     }else{
         std::cerr << "cannot update _reservedIndices since _removedIndices is empty\n";
         exit(1);
     } 
 }
Exemplo n.º 5
0
void DenseReconstruction::normalsEstimation(const pcl::PointCloud<pcl::PointXYZLRegionF>::Ptr &cloud,pcl::PointCloud<pcl::Normal>::Ptr &normals){


	pcl::PointCloud<pcl::PointXYZRGBA>::Ptr temp_cloud(new pcl::PointCloud<pcl::PointXYZRGBA>);
	pcl::copyPointCloud(*cloud,*temp_cloud);


	 // Create a KD-Tree
	tree_.reset(new pcl::search::KdTree<pcl::PointXYZRGBA>);

	pcl::NormalEstimation<pcl::PointXYZRGBA, pcl::Normal> ne;
	ne.setInputCloud(temp_cloud);
	ne.setSearchMethod(tree_);
	ne.setRadiusSearch(0.03);
//	ne.compute(*cloud_normals_);
	ne.compute(*normals);
}
Exemplo n.º 6
0
extern "C" void parse(const rosbag::MessageInstance* m,
  madara::knowledge::KnowledgeBase * kb, 
  std::string container_name)
{
  // THIS METHOD IS HARDCODED TO RUN WITH PCL BASED SCHEMAS

  boost::shared_ptr<sensor_msgs::PointCloud2> pointcloud =
    m->instantiate<sensor_msgs::PointCloud2>();

  // Load PCL schema
  std::string pointcloud_schema_name = "PointCloudXYZ";
  capnp::MallocMessageBuilder buffer;
  gams::types::PointCloudXYZ::Builder pointcloud_builder = 
    buffer.initRoot<gams::types::PointCloudXYZ>();

  // Convert from ROS PointCloud2 to a PCL PointCloud
  pcl::PCLPointCloud2 pcl_pc2;
  pcl_conversions::toPCL(*pointcloud, pcl_pc2);
  pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::fromPCLPointCloud2(pcl_pc2, *temp_cloud);

  //Fill the list of points with data
  auto point_list = pointcloud_builder.initPoints((*temp_cloud).size());
  int point_counter = 0;
  for (pcl::PointXYZ point : *temp_cloud)
  {
    point_list[point_counter].setX(point.x);
    point_list[point_counter].setY(point.y);
    point_list[point_counter].setZ(point.z);
    point_counter++;
  }

  // Now fill the other fields
  pointcloud_builder.setTov(
   ((unsigned long)pointcloud->header.stamp.sec * 1e9) +
   (unsigned long)pointcloud->header.stamp.nsec);
  pointcloud_builder.setFrameId(pointcloud->header.frame_id.c_str());
  pointcloud_builder.setWidth(pointcloud->width);
  pointcloud_builder.setHeight(pointcloud->height);
  pointcloud_builder.setIsDense((bool)pointcloud->is_dense);

  // Store in the knowledgebase
  madara::knowledge::GenericCapnObject any(pointcloud_schema_name.c_str(),
    buffer);
  kb->set_any(container_name, any);
}
Exemplo n.º 7
0
int sac_ia_alignment(pcl::PointCloud<pcl::PointXYZ>::Ptr prev_cloud,
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in) {

	pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud(new pcl::PointCloud<pcl::PointXYZ>(*prev_cloud));

	std::cout << "Performing Sample Consensus Initial Alignment.. "
			<< std::endl;
	FeatureCloud targetCloud;
	FeatureCloud templateCloud;
	targetCloud.setInputCloud(temp_cloud);
	templateCloud.setInputCloud(cloud_in);


	TemplateAlignment templateAlign;
	templateAlign.addTemplateCloud(templateCloud);
	templateAlign.setTargetCloud(targetCloud);

	Result bestAlignment;
	std::cout << "entering alignment" << std::endl;
	templateAlign.findBestAlignment(bestAlignment);
	std::cout << "exiting alignment" << std::endl;

	printf("Fitness Score: %f \n", bestAlignment.fitness_score);

	Eigen::Matrix3f rotation = bestAlignment.final_transformation.block<3,3>(0, 0);
	Eigen::Vector3f translation =
			bestAlignment.final_transformation.block<3,1>(0, 3);

	printf("\n");
	printf("    | %6.3f %6.3f %6.3f | \n", rotation(0, 0), rotation(0, 1),
			rotation(0, 2));
	printf("R = | %6.3f %6.3f %6.3f | \n", rotation(1, 0), rotation(1, 1),
			rotation(1, 2));
	printf("    | %6.3f %6.3f %6.3f | \n", rotation(2, 0), rotation(2, 1),
			rotation(2, 2));
	printf("\n");
	printf("t = < %0.3f, %0.3f, %0.3f >\n", translation(0), translation(1),
			translation(2));

	pcl::transformPointCloud(*templateCloud.getPointCloud(), *cloud_in,
			bestAlignment.final_transformation);

	std::cout << "***Initial alignment complete***" << std::endl;

}
Exemplo n.º 8
0
void display::mapPublisher(const boost::shared_ptr<const sensor_msgs::PointCloud2>& input)
{

    pcl::PCLPointCloud2 pcl_pc2;
    pcl_conversions::toPCL(*input,pcl_pc2);
    pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::fromPCLPointCloud2(pcl_pc2,*temp_cloud);

    sensor_msgs::PointCloud2 pcl_msg;
    pcl::toROSMsg(*temp_cloud,pcl_msg);
    const char* MAP_FRAME_ID = "Quad";
    pcl_msg.header.frame_id = MAP_FRAME_ID;
    pcl_msg.header.stamp  = ros::Time::now();
    pcl_pub2.publish(input);



}
Exemplo n.º 9
0
    CloudPtr
    get ()
    {
        //lock while we swap our cloud and reset it.
        boost::mutex::scoped_lock lock (mtx_);
        CloudPtr temp_cloud (new Cloud);
        CloudPtr temp_cloud2 (new Cloud);

        grid_.setInputCloud (cloud_);
        grid_.filter (*temp_cloud);

        pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
        pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());

        seg_.setInputCloud (temp_cloud);
        seg_.segment (*inliers, *coefficients);

        extract_.setInputCloud (temp_cloud);
        extract_.setIndices (inliers);
        extract_.filter (*temp_cloud2);

        return (temp_cloud2);
    }
Exemplo n.º 10
0
int pcDenoise::pcDecompose()
{
	if( cloud->empty())
	{
			std::cout<< "The input point cloud seems to be empty, try invoke setCloud or read it from pcd file.";
			return -1;
	}

	pcType temp_cloud(new pcl::PointCloud<PT>);

	  int K = 100;
	  pcl::KdTreeFLANN<PT> kdtree;
	  kdtree.setInputCloud(cloud);

	  std::vector<int> Idx(K);
	  std::vector<float> Dist(K);
	  double mx=0;
	  double my=0;
	  double mz=0;

	  for( int i=0; i<cloud->width;i++)
	  {
		  mx += (cloud->points.at(i).x);
		  my += (cloud->points.at(i).y);
		  mz += (cloud->points.at(i).z);
	  }
	  mx/=cloud->width;
	  my/=cloud->width;
	  mz/=cloud->width;

	  for( int i=0; i<cloud->width;i++)
	  {
		  kdtree.nearestKSearch (i, K, Idx, Dist);
		  int neighbourIdx = 0;
		  double xx=0;
		  double yy=0;
		  double zz=0;
		  for( int j=0; j<K; j++)
		  {

			  neighbourIdx = Idx[j];
			  xx += (cloud->points.at(neighbourIdx).x);
			  yy += (cloud->points.at(neighbourIdx).y);
			  zz += (cloud->points.at(neighbourIdx).z);
		  }
		  xx/=K;
		  yy/=K;
		  zz/=K;
		  //xx-=cloud->points.at(i).x;
		  //yy-=cloud->points.at(i).y;
		  //zz-=cloud->points.at(i).z;
		  xx-=mx;
		  yy-=my;
		  zz-=mz;
		  PT* temp = new PT();
		  temp->x = xx;
		  temp->y = yy;
		  temp->z = zz;
		  temp->r = (cloud->points.at(i).r);
		  temp->g = (cloud->points.at(i).g);
		  temp->b = (cloud->points.at(i).b);
		  temp_cloud->push_back( *temp );
	  }
	  cloud->clear();

	  pcl::copyPointCloud(*temp_cloud,*cloud);



	return 0;
}
Exemplo n.º 11
0
int 
main (int argc, char ** argv)
{
  if (argc < 2) 
  {
    pcl::console::print_info ("Syntax is: %s input.pcd <options>\n", argv[0]);
    pcl::console::print_info ("  where options are:\n");
    pcl::console::print_info ("    -p dist_threshold,max_iters  ..... Subtract the dominant plane\n");
    pcl::console::print_info ("    -c tolerance,min_size,max_size ... Cluster points\n");
    pcl::console::print_info ("    -s output.pcd .................... Save the largest cluster\n");
    return (1);
  }

  // Load the input file
  PointCloudPtr cloud (new PointCloud);
  pcl::io::loadPCDFile (argv[1], *cloud);
  pcl::console::print_info ("Loaded %s (%zu points)\n", argv[1], cloud->size ());

  // Subtract the dominant plane
  double dist_threshold, max_iters;
  bool subtract_plane = pcl::console::parse_2x_arguments (argc, argv, "-p", dist_threshold, max_iters) > 0;
  if (subtract_plane)
  {
    size_t n = cloud->size ();
    cloud = findAndSubtractPlane (cloud, dist_threshold, (int)max_iters);
    pcl::console::print_info ("Subtracted %zu points along the detected plane\n", n - cloud->size ());
  }

  // Cluster points
  double tolerance, min_size, max_size;
  std::vector<pcl::PointIndices> cluster_indices;
  bool cluster_points = pcl::console::parse_3x_arguments (argc, argv, "-c", tolerance, min_size, max_size) > 0;
  if (cluster_points)
  {
    clusterObjects (cloud, tolerance, (int)min_size, (int)max_size, cluster_indices);
    pcl::console::print_info ("Found %zu clusters\n", cluster_indices.size ());
  }

  // Save output
  std::string output_filename;
  bool save_cloud = pcl::console::parse_argument (argc, argv, "-s", output_filename) > 0;
  if (save_cloud)
  {
    // If clustering was performed, save only the first (i.e., largest) cluster
    if (cluster_points)
    {
      PointCloudPtr temp_cloud (new PointCloud);
      pcl::copyPointCloud (*cloud, cluster_indices[0], *temp_cloud);
      cloud = temp_cloud;
    }
    pcl::console::print_info ("Saving result as %s...\n", output_filename.c_str ());
    pcl::io::savePCDFile (output_filename, *cloud);
  }
  // Or visualize the result
  else
  {
    pcl::console::print_info ("Starting visualizer... Close window to exit.\n");
    pcl::visualization::PCLVisualizer vis;

    // If clustering was performed, display each cluster with a random color
    if (cluster_points)
    {
      for (size_t i = 0; i < cluster_indices.size (); ++i)
      {
        // Extract the i_th cluster into a new cloud
        pcl::PointCloud<pcl::PointXYZ>::Ptr cluster_i (new pcl::PointCloud<pcl::PointXYZ>);
        pcl::copyPointCloud (*cloud, cluster_indices[i], *cluster_i);

        // Create a random color
        pcl::visualization::PointCloudColorHandlerRandom<pcl::PointXYZ> random_color (cluster_i);

        // Create a unique identifier
        std::stringstream cluster_id ("cluster");
        cluster_id << i;

        // Add the i_th cluster to the visualizer with a random color and a unique identifier
        vis.addPointCloud<pcl::PointXYZ> (cluster_i, random_color, cluster_id.str ());
      }
    }
    else
    {
      // If clustering wasn't performed, just display the cloud
      vis.addPointCloud (cloud);
    }
    vis.resetCamera ();
    vis.spin ();
  }

  return (0);
}
Exemplo n.º 12
0
void cloud_cb(const sensor_msgs::PointCloud2ConstPtr& input)
{
    sensor_msgs::PointCloud2 result_cloud_msg;

    pcl::PointCloud<PointInT>::Ptr cloud (new pcl::PointCloud<PointInT>);

    pcl::fromROSMsg(*input, *cloud);

    // Perform downsamplign via VoxelGrid
    pcl::VoxelGrid<PointInT> voxel_grid;
    voxel_grid.setInputCloud(cloud);
    voxel_grid.setLeafSize(grid_size, grid_size, grid_size);

    PointInTPtr temp_cloud (new pcl::PointCloud<PointInT> ());
    voxel_grid.filter(*temp_cloud);

    cloud = temp_cloud;

    // Filtering out noise
    pcl::StatisticalOutlierRemoval<PointInT> sor;
    sor.setInputCloud (cloud);
    sor.setMeanK (sor_mean_k); // 50 - default
    sor.setStddevMulThresh (sor_stddev_mul_th);
    sor.filter (*cloud);

    octree->setInputCloud (cloud);
    octree->addPointsFromInputCloud ();

    std::vector<int> newPointIdxVector;

    octree->getPointIndicesFromNewVoxels (newPointIdxVector, noise_filter);

    if(!newPointIdxVector.size())
    {
        ROS_INFO("Person wasn't detected\n");
        return;
    }

    pcl::PointCloud<PointInT>::Ptr person_cloud (new pcl::PointCloud<PointInT>);

    pcl::copyPointCloud(*cloud, newPointIdxVector, *person_cloud);

    ROS_INFO("Person cloud has %d points", (int)person_cloud->points.size());

    pcl::RadiusOutlierRemoval<PointInT> outrem;
    outrem.setInputCloud(person_cloud);
    outrem.setRadiusSearch(ror_rad);
    outrem.setMinNeighborsInRadius (ror_min_heighbors);
    outrem.filter (*person_cloud);

    // Reject too small cloud
    if(person_cloud->points.size() < min_size_thresh)
    {
        ROS_INFO("Person wasn't detected\n");
        return;
    }

    pcl::toROSMsg(*person_cloud, result_cloud_msg);

    result_cloud_msg.header.frame_id = input->header.frame_id;
    result_pub.publish(result_cloud_msg);

    // Get pose of person and publish results
    Eigen::Vector4f centroid;
    pcl::compute3DCentroid(*person_cloud, centroid);

    ROS_INFO("Person pose: %.3f, %.3f, %.3f", centroid[0], centroid[1], centroid[2]);

    geometry_msgs::Pose pose;
    pose.position.x = centroid[0];
    pose.position.y = centroid[1];
    pose.position.z = centroid[2];

    pose.orientation.w = 0;
    pose.orientation.x = 0;
    pose.orientation.y = 0;
    pose.orientation.z = 0;

    geometry_msgs::PoseStamped pose_msg;
    pose_msg.header.frame_id = input->header.frame_id;
    pose_msg.pose = pose;

    pose_pub.publish(pose_msg);


    // Calculate bounding box
    float x_len, y_len, z_len; // lenghts of the bounding box

    Eigen::Vector4f min, max;

    pcl::getMinMax3D(*person_cloud, min, max);

    x_len = max[0] - min[0];
    y_len = max[1] - min[1];
    z_len = max[2] - min[2];

    shape_msgs::SolidPrimitive bbox;
    bbox.type = bbox.BOX;
    bbox.dimensions.resize(3);
    bbox.dimensions[0] = x_len;
    bbox.dimensions[1] = y_len;
    bbox.dimensions[2] = z_len;

    velodyne_track_person::PersonInstance person_instance;
    person_instance.pose = pose;
    person_instance.bbox = bbox;
    person_instance.header.frame_id = input->header.frame_id;
    person_instance.header.stamp = ros::Time::now();

    person_pub.publish(person_instance);

    ROS_INFO("Person pose was published\n");

    // Switch octree buffers: This resets octree but keeps previous tree structure in memory.
    octree->switchBuffers ();
}
Exemplo n.º 13
0
  CloudPtr
      get ()
      {
        //lock while we swap our cloud and reset it.
        boost::mutex::scoped_lock lock (mtx_);
        CloudPtr temp_cloud (new Cloud);
        CloudPtr temp_cloud2 (new Cloud);
        CloudPtr temp_cloud3 (new Cloud);
        CloudPtr temp_cloud4 (new Cloud);
        CloudPtr temp_cloud5 (new Cloud);
        CloudConstPtr empty_cloud;


        cout << "===============================\n"
                "======Start of frame===========\n"
                "===============================\n";
        //cerr << "cloud size orig: " << cloud_->size() << endl;
        voxel_grid.setInputCloud (cloud_);
        voxel_grid.filter (*temp_cloud);  // filter cloud for z depth

        //cerr << "cloud size postzfilter: " << temp_cloud->size() << endl;

        pcl::ModelCoefficients::Ptr planecoefficients (new pcl::ModelCoefficients ());
        pcl::PointIndices::Ptr plane_inliers (new pcl::PointIndices ());
        std::vector<pcl::ModelCoefficients> linecoefficients1;
        pcl::ModelCoefficients model;
        model.values.resize (6);
        pcl::PointIndices::Ptr line_inliers (new pcl::PointIndices ());
        std::vector<Eigen::Vector3f> corners;

        if(temp_cloud->size() > MIN_CLOUD_POINTS) {
          plane_seg.setInputCloud (temp_cloud);
          plane_seg.segment (*plane_inliers, *planecoefficients); // find plane
        }

        //cerr << "plane inliers size: " << plane_inliers->indices.size() << endl;

        cout << "planecoeffs: " 
            << planecoefficients->values[0]  << " "
            << planecoefficients->values[1]  << " "
            << planecoefficients->values[2]  << " "
            << planecoefficients->values[3]  << " "
            << endl;

        Eigen::Vector3f pn = Eigen::Vector3f(
            planecoefficients->values[0],
            planecoefficients->values[1],
            planecoefficients->values[2]);

        float planedeg = pcl::rad2deg(acos(pn.dot(Eigen::Vector3f::UnitZ())));
        cout << "angle of camera to floor normal: " << planedeg << " degrees" << endl;
        cout << "distance of camera to floor: " << planecoefficients->values[3]
            << " meters" <<  endl;

        plane_extract.setNegative (true); 
        plane_extract.setInputCloud (temp_cloud);
        plane_extract.setIndices (plane_inliers);
        plane_extract.filter (*temp_cloud2);   // remove plane
        plane_extract.setNegative (false); 
        plane_extract.filter (*temp_cloud5);   // only plane

        for(size_t i = 0; i < temp_cloud5->size (); ++i)
        {
          temp_cloud5->points[i].r = 0; 
          temp_cloud5->points[i].g = 255; 
          temp_cloud5->points[i].b = 0; 
          // tint found plane green for ground
        }

        for(size_t j = 0 ; j < MAX_LINES && temp_cloud2->size() > MIN_CLOUD_POINTS; j++) 
          // look for x lines until cloud gets too small
        {
//          cerr << "cloud size: " << temp_cloud2->size() << endl;

          line_seg.setInputCloud (temp_cloud2);
          line_seg.segment (*line_inliers, model); // find line

 //         cerr << "line inliears size: " << line_inliers->indices.size() << endl;

          if(line_inliers->indices.size() < MIN_CLOUD_POINTS)
            break;

          linecoefficients1.push_back (model);  // store line coeffs

          line_extract.setNegative (true); 
          line_extract.setInputCloud (temp_cloud2);
          line_extract.setIndices (line_inliers);
          line_extract.filter (*temp_cloud3);  // remove plane
          line_extract.setNegative (false); 
          line_extract.filter (*temp_cloud4);  // only plane
          for(size_t i = 0; i < temp_cloud4->size (); ++i) {
            temp_cloud4->points[i].g = 0; 
            if(j%2) {
              temp_cloud4->points[i].r = 255-j*int(255/MAX_LINES); 
              temp_cloud4->points[i].b = 0+j*int(255/MAX_LINES); 
            } else {
              temp_cloud4->points[i].b = 255-j*int(255/MAX_LINES); 
              temp_cloud4->points[i].r = 0+j*int(255/MAX_LINES); 
            }
          }
          *temp_cloud5 += *temp_cloud4;	// add line to ground

          temp_cloud2.swap ( temp_cloud3); // remove line

        }

        cout << "found " << linecoefficients1.size() << " lines." << endl;

        for(size_t i = 0; i < linecoefficients1.size(); i++)
        {
          //cerr << "linecoeffs: " << i  << " "
          //    << linecoefficients1[i].values[0]  << " "
          //    << linecoefficients1[i].values[1]  << " "
          //    << linecoefficients1[i].values[2]  << " "
          //    << linecoefficients1[i].values[3]  << " "
          //    << linecoefficients1[i].values[4]  << " "
          //    << linecoefficients1[i].values[5]  << " "
          //    << endl;

          Eigen::Vector3f lv = Eigen::Vector3f(
              linecoefficients1[i].values[3],
              linecoefficients1[i].values[4],
              linecoefficients1[i].values[5]); 

          Eigen::Vector3f lp = Eigen::Vector3f(
              linecoefficients1[i].values[0],
              linecoefficients1[i].values[1],
              linecoefficients1[i].values[2]); 

          float r = pn.dot(lv);
          float deg = pcl::rad2deg(acos(r));

          cout << "angle of line to floor normal: " << deg << " degrees" << endl;

          if(abs(deg-30) < 5 || abs(deg-150) < 5) 
          {
            cout << "found corner line" << endl;

            float t = -(lp.dot(pn) + planecoefficients->values[3])/ r;
            Eigen::Vector3f intersect = lp + lv*t;
            cout << "corner intersects floor at: " << endl << intersect << endl;
            cout << "straight line distance from camera to corner: " <<
                intersect.norm() << " meters" << endl;

            corners.push_back(intersect);

            Eigen::Vector3f floor_distance = intersect + pn;  // should be - ???

            cout << "distance along floor to corner: " <<
                floor_distance.norm() << " meters" << endl;
          }
          else if(abs(deg-90) < 5) 
          {
            cout << "found horizontal line" << endl;
          }

        }

        switch(corners.size())
        {
          case 2:
            cout << "distance between corners " << (corners[0] - corners[1]).norm() << endl;
            cout << "angle of pyramid to camera " <<
                pcl::rad2deg(acos(((corners[0] - corners[1]).normalized()).dot(Eigen::Vector3f::UnitX())))
                << endl;

            break;

          case 3:
            cout << "distance between corners " << (corners[0] - corners[1]).norm() << endl;
            cout << "distance between corners " << (corners[0] - corners[2]).norm() << endl;
            cout << "distance between corners " << (corners[1] - corners[2]).norm() << endl;

            cout << "angle of corner on floor (should be 90) " <<
                pcl::rad2deg(acos((corners[0] - corners[1]).dot(corners[0] - corners [2])))
                << endl;
            
            cout << "angle of pyramid to camera " <<
                pcl::rad2deg(acos(((corners[0] - corners[1]).normalized()).dot(Eigen::Vector3f::UnitX())))
                << endl;

            break;
        }

        if (saveCloud)
        {
          std::stringstream stream, stream1;
          std::string filename;

          stream << "inputCloud" << filesSaved << ".pcd";
          filename = stream.str();
          if (pcl::io::savePCDFile(filename, *cloud_, true) == 0)
          {
            filesSaved++;
            cout << "Saved " << filename << "." << endl;
          }
          else PCL_ERROR("Problem saving %s.\n", filename.c_str());

          
          stream1 << "inputCloud" << filesSaved << ".pcd";
          filename = stream1.str();
          if (pcl::io::savePCDFile(filename, *temp_cloud5, true) == 0)
          {
            filesSaved++;
            cout << "Saved " << filename << "." << endl;
          }
          else PCL_ERROR("Problem saving %s.\n", filename.c_str());

          saveCloud = false;
        }

        empty_cloud.swap(cloud_);  // set cloud_ to null

        if(toggleView == 1) 
          return (temp_cloud);  // return orig cloud
        else
          return (temp_cloud5); // return colored cloud
      }
Exemplo n.º 14
0
void 
cloud_cb_white (const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
{
  // Container for original & filtered data
  pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2; 
  pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
  pcl::PCLPointCloud2 cloud_filtered;
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_segment (new pcl::PointCloud<pcl::PointXYZ>);

   // Convert to PCL data type
  pcl_conversions::toPCL(*cloud_msg, *cloud);

  // Perform downsampling
  pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
  sor.setInputCloud (cloudPtr);
  sor.setLeafSize (0.01, 0.01, 0.01);
  sor.filter (cloud_filtered);

  // convert PointCloud2 to pcl::PointCloud<pcl::PointXYZ>
  pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::fromPCLPointCloud2(cloud_filtered,*temp_cloud);

  // segmenting area
  pcl::PassThrough<pcl::PointXYZ> pass;
  pass.setInputCloud (temp_cloud);
  // left - right
  pass.setFilterFieldName ("x");
  pass.setFilterLimits (-3, 3);
  //pass.setFilterLimitsNegative (true);
  pass.filter (*cloud_segment);

  pass.setInputCloud (cloud_segment);
  // up - down
  pass.setFilterFieldName ("y");
  pass.setFilterLimits (-3, 3);
  //pass.setFilterLimitsNegative (true);
  pass.filter (*cloud_segment);

  pass.setInputCloud (cloud_segment);
  // backward - forward
  pass.setFilterFieldName ("z");
  pass.setFilterLimits (0.5, 4);
  //pass.setFilterLimitsNegative (true);
  pass.filter (*cloud_segment);

  // // Find centroid
  float sum_x = 0;
  float sum_y = 0;
  float sum_z = 0;
  if(cloud_segment->points.size() > 50) {
    for(size_t i = 0; i < cloud_segment->points.size(); ++i) {
     sum_x += cloud_segment->points[i].x;
     sum_y += cloud_segment->points[i].y;
     sum_z += cloud_segment->points[i].z;
    }

    sum_x = sum_x / cloud_segment->points.size();
    sum_y = sum_y / cloud_segment->points.size();
    sum_z = sum_z / cloud_segment->points.size();

    geometry_msgs::PoseStamped sPose;
    sPose.header.stamp = ros::Time::now();
    
    sPose.pose.position.x = sum_z; // equals to z in pcl // backward - forward
    sPose.pose.position.y = -(sum_x); // equals to -x in pcl // right - left
    sPose.pose.position.z = -(sum_y); // equals to -y in pcl // down - up

    sPose.pose.orientation.x = 0.0;
    sPose.pose.orientation.y = 0.0;
    sPose.pose.orientation.z = 0.0;
    sPose.pose.orientation.w = 1.0;

    cock_pose_white_pub.publish(sPose);
  }

  // Convert to ROS data type
  sensor_msgs::PointCloud2 output;
  //pcl_conversions::fromPCL(cloud_filtered, output);
  pcl::toROSMsg(*cloud_segment, output);

  // Publish the data
  cloud_pub_white.publish (output);
}