int
main(int, char** argv)
{
  std::string filename = argv[1];
  std::cout << "Reading " << filename << std::endl;
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz (new pcl::PointCloud<pcl::PointXYZ>);
  if(pcl::io::loadPCDFile<pcl::PointXYZ> (filename, *cloud_xyz) == -1) // load the file
  {
    PCL_ERROR ("Couldn't read file");
    return -1;
  }
  std::cout << "points: " << cloud_xyz->points.size () <<std::endl;
  
  // Parameters for sift computation
  const float min_scale = 0.01f;
  const int n_octaves = 3;
  const int n_scales_per_octave = 4;
  const float min_contrast = 0.001f;
  
  // Estimate the normals of the cloud_xyz
  pcl::NormalEstimation<pcl::PointXYZ, pcl::PointNormal> ne;
  pcl::PointCloud<pcl::PointNormal>::Ptr cloud_normals (new pcl::PointCloud<pcl::PointNormal>);
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_n(new pcl::search::KdTree<pcl::PointXYZ>());

  ne.setInputCloud(cloud_xyz);
  ne.setSearchMethod(tree_n);
  ne.setRadiusSearch(0.2);
  ne.compute(*cloud_normals);

  // Copy the xyz info from cloud_xyz and add it to cloud_normals as the xyz field in PointNormals estimation is zero
  for(size_t i = 0; i<cloud_normals->points.size(); ++i)
  {
    cloud_normals->points[i].x = cloud_xyz->points[i].x;
    cloud_normals->points[i].y = cloud_xyz->points[i].y;
    cloud_normals->points[i].z = cloud_xyz->points[i].z;
  }

  // Estimate the sift interest points using normals values from xyz as the Intensity variants
  pcl::SIFTKeypoint<pcl::PointNormal, pcl::PointWithScale> sift;
  pcl::PointCloud<pcl::PointWithScale> result;
  pcl::search::KdTree<pcl::PointNormal>::Ptr tree(new pcl::search::KdTree<pcl::PointNormal> ());
  sift.setSearchMethod(tree);
  sift.setScales(min_scale, n_octaves, n_scales_per_octave);
  sift.setMinimumContrast(min_contrast);
  sift.setInputCloud(cloud_normals);
  sift.compute(result);

  std::cout << "No of SIFT points in the result are " << result.points.size () << std::endl;

/*
  // Copying the pointwithscale to pointxyz so as visualize the cloud
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_temp (new pcl::PointCloud<pcl::PointXYZ>);
  copyPointCloud(result, *cloud_temp);
  std::cout << "SIFT points in the cloud_temp are " << cloud_temp->points.size () << std::endl;
  
  
  // Visualization of keypoints along with the original cloud
  pcl::visualization::PCLVisualizer viewer("PCL Viewer");
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> keypoints_color_handler (cloud_temp, 0, 255, 0);
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_color_handler (cloud_xyz, 255, 0, 0);
  viewer.setBackgroundColor( 0.0, 0.0, 0.0 );
  viewer.addPointCloud(cloud_xyz, cloud_color_handler, "cloud");
  viewer.addPointCloud(cloud_temp, keypoints_color_handler, "keypoints");
  viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "keypoints");
  
  while(!viewer.wasStopped ())
  {
  viewer.spinOnce ();
  }
  
*/

  return 0;
  
}
Esempio n. 2
0
template <typename PointInT, typename PointOutT, typename PointRFT> void
pcl::UniqueShapeContext<PointInT, PointOutT, PointRFT>::computePointDescriptor (size_t index, /*float rf[9],*/ std::vector<float> &desc)
{
  pcl::Vector3fMapConst origin = input_->points[(*indices_)[index]].getVector3fMap ();

  const Eigen::Vector3f x_axis (frames_->points[index].x_axis[0],
                                frames_->points[index].x_axis[1],
                                frames_->points[index].x_axis[2]);
  //const Eigen::Vector3f& y_axis = frames_->points[index].y_axis.getNormalVector3fMap ();
  const Eigen::Vector3f normal (frames_->points[index].z_axis[0],
                                frames_->points[index].z_axis[1],
                                frames_->points[index].z_axis[2]);

  // Find every point within specified search_radius_
  std::vector<int> nn_indices;
  std::vector<float> nn_dists;
  const size_t neighb_cnt = searchForNeighbors ((*indices_)[index], search_radius_, nn_indices, nn_dists);
  // For each point within radius
  for (size_t ne = 0; ne < neighb_cnt; ne++)
  {
    if (pcl::utils::equal(nn_dists[ne], 0.0f))
      continue;
    // Get neighbours coordinates
    Eigen::Vector3f neighbour = surface_->points[nn_indices[ne]].getVector3fMap ();

    // ----- Compute current neighbour polar coordinates -----

    // Get distance between the neighbour and the origin
    float r = std::sqrt (nn_dists[ne]);

    // Project point into the tangent plane
    Eigen::Vector3f proj;
    pcl::geometry::project (neighbour, origin, normal, proj);
    proj -= origin;

    // Normalize to compute the dot product
    proj.normalize ();

    // Compute the angle between the projection and the x axis in the interval [0,360]
    Eigen::Vector3f cross = x_axis.cross (proj);
    float phi = rad2deg (std::atan2 (cross.norm (), x_axis.dot (proj)));
    phi = cross.dot (normal) < 0.f ? (360.0f - phi) : phi;
    /// Compute the angle between the neighbour and the z axis (normal) in the interval [0, 180]
    Eigen::Vector3f no = neighbour - origin;
    no.normalize ();
    float theta = normal.dot (no);
    theta = pcl::rad2deg (acosf (std::min (1.0f, std::max (-1.0f, theta))));

    /// Bin (j, k, l)
    size_t j = 0;
    size_t k = 0;
    size_t l = 0;

    /// Compute the Bin(j, k, l) coordinates of current neighbour
    for (size_t rad = 1; rad < radius_bins_ + 1; rad++)
    {
      if (r <= radii_interval_[rad])
      {
        j = rad - 1;
        break;
      }
    }

    for (size_t ang = 1; ang < elevation_bins_ + 1; ang++)
    {
      if (theta <= theta_divisions_[ang])
      {
        k = ang - 1;
        break;
      }
    }

    for (size_t ang = 1; ang < azimuth_bins_ + 1; ang++)
    {
      if (phi <= phi_divisions_[ang])
      {
        l = ang - 1;
        break;
      }
    }

    /// Local point density = number of points in a sphere of radius "point_density_radius_" around the current neighbour
    std::vector<int> neighbour_indices;
    std::vector<float> neighbour_didtances;
    float point_density = static_cast<float> (searchForNeighbors (*surface_, nn_indices[ne], point_density_radius_, neighbour_indices, neighbour_didtances));
    /// point_density is always bigger than 0 because FindPointsWithinRadius returns at least the point itself
    float w = (1.0f / point_density) * volume_lut_[(l*elevation_bins_*radius_bins_) +
                                                   (k*radius_bins_) +
                                                   j];

    assert (w >= 0.0);
    if (w == std::numeric_limits<float>::infinity ())
      PCL_ERROR ("Shape Context Error INF!\n");
    if (std::isnan(w))
      PCL_ERROR ("Shape Context Error IND!\n");
    /// Accumulate w into correspondent Bin(j,k,l)
    desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] += w;

    assert (desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] >= 0);
  } // end for each neighbour
}
Esempio n. 3
0
template <typename PointT, typename PointNT> void
pcl::SampleConsensusModelCylinder<PointT, PointNT>::projectPoints (
      const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields)
{
  // Needs a valid set of model coefficients
  if (model_coefficients.size () != 7)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelCylinder::projectPoints] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ());
    return;
  }

  projected_points.header = input_->header;
  projected_points.is_dense = input_->is_dense;

  Eigen::Vector4f line_pt  (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
  Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
  float ptdotdir = line_pt.dot (line_dir);
  float dirdotdir = 1.0f / line_dir.dot (line_dir);

  // Copy all the data fields from the input cloud to the projected one?
  if (copy_data_fields)
  {
    // Allocate enough space and copy the basics
    projected_points.points.resize (input_->points.size ());
    projected_points.width    = input_->width;
    projected_points.height   = input_->height;

    typedef typename pcl::traits::fieldList<PointT>::type FieldList;
    // Iterate over each point
    for (size_t i = 0; i < projected_points.points.size (); ++i)
      // Iterate over each dimension
      pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[i], projected_points.points[i]));

    // Iterate through the 3d points and calculate the distances from them to the cylinder
    for (size_t i = 0; i < inliers.size (); ++i)
    {
      Eigen::Vector4f p (input_->points[inliers[i]].x,
                         input_->points[inliers[i]].y,
                         input_->points[inliers[i]].z,
                         1);

      float k = (p.dot (line_dir) - ptdotdir) * dirdotdir;

      pcl::Vector4fMap pp = projected_points.points[inliers[i]].getVector4fMap ();
      pp.matrix () = line_pt + k * line_dir;

      Eigen::Vector4f dir = p - pp;
      dir.normalize ();

      // Calculate the projection of the point onto the cylinder
      pp += dir * model_coefficients[6];
    }
  }
  else
  {
    // Allocate enough space and copy the basics
    projected_points.points.resize (inliers.size ());
    projected_points.width    = static_cast<uint32_t> (inliers.size ());
    projected_points.height   = 1;

    typedef typename pcl::traits::fieldList<PointT>::type FieldList;
    // Iterate over each point
    for (size_t i = 0; i < inliers.size (); ++i)
      // Iterate over each dimension
      pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[inliers[i]], projected_points.points[i]));

    // Iterate through the 3d points and calculate the distances from them to the plane
    for (size_t i = 0; i < inliers.size (); ++i)
    {
      pcl::Vector4fMap pp = projected_points.points[i].getVector4fMap ();
      pcl::Vector4fMapConst p = input_->points[inliers[i]].getVector4fMap ();

      float k = (p.dot (line_dir) - ptdotdir) * dirdotdir;
      // Calculate the projection of the point on the line
      pp.matrix () = line_pt + k * line_dir;

      Eigen::Vector4f dir = p - pp;
      dir.normalize ();

      // Calculate the projection of the point onto the cylinder
      pp += dir * model_coefficients[6];
    }
  }
}
Esempio n. 4
0
int main(int argc, char** argv)
{

  if (argc < 5)
  {
    PCL_INFO ("Usage %s -input_dir /dir/with/models -output_dir /output/dir [options]\n", argv[0]);
    PCL_INFO (" * where options are:\n"
        "         -height <X>            : simulate scans with sensor mounted on height X\n"
        "         -noise_std <X>         : std of gaussian noise added to pointcloud. Default value 0.0001.\n"
        "         -distance <X>          : simulate scans with object located on a distance X. Can be used multiple times. Default value 4.\n"
        "         -tilt <X>              : tilt sensor for X degrees. X == 0 - sensor looks strait. X < 0 Sensor looks down. X > 0 Sensor looks up . Can be used multiple times. Default value -15.\n"
        "         -shift <X>             : shift object from the straight line. Can be used multiple times. Default value 0.\n"
        "         -num_views <X>         : how many times rotate the object in for every distance, tilt and shift. Default value 6.\n"

        "");
    return -1;
  }

  std::string input_dir, output_dir;
  double height = 1.5;
  double num_views = 6;
  double noise_std = 0.0001;
  std::vector<double> distances;
  std::vector<double> tilt;
  std::vector<double> shift;
  int full_model_n_points = 30000;

  pcl::console::parse_argument(argc, argv, "-input_dir", input_dir);
  pcl::console::parse_argument(argc, argv, "-output_dir", output_dir);
  pcl::console::parse_argument(argc, argv, "-num_views", num_views);
  pcl::console::parse_argument(argc, argv, "-height", height);
  pcl::console::parse_argument(argc, argv, "-noise_std", noise_std);
  pcl::console::parse_multiple_arguments(argc, argv, "-distance", distances);
  pcl::console::parse_multiple_arguments(argc, argv, "-tilt", tilt);
  pcl::console::parse_multiple_arguments(argc, argv, "-shift", shift);

  PCL_INFO("distances size: %d\n", distances.size());
  for (size_t i = 0; i < distances.size(); i++)
  {
    PCL_INFO("distance: %f\n", distances[i]);
  }

  // Set default values if user didn't provide any
  if (distances.size() == 0)
    distances.push_back(4);
  if (tilt.size() == 0)
    tilt.push_back(-15);
  if (shift.size() == 0)
    shift.push_back(0);

  // Check if input path exists
  boost::filesystem::path input_path(input_dir);
  if (!boost::filesystem::exists(input_path))
  {
    PCL_ERROR("Input directory doesnt exists.");
    return -1;
  }

  // Check if input path is a directory
  if (!boost::filesystem::is_directory(input_path))
  {
    PCL_ERROR("%s is not directory.", input_path.c_str());
    return -1;
  }

  // Check if output directory exists
  boost::filesystem::path output_path(output_dir);
  if (!boost::filesystem::exists(output_path) || !boost::filesystem::is_directory(output_path))
  {
    if (!boost::filesystem::create_directories(output_path))
    {
      PCL_ERROR ("Error creating directory %s.\n", output_path.c_str ());
      return -1;
    }
  }

  // Find all .vtk files in the input directory
  std::vector<std::string> files_to_process;
  PCL_INFO("Processing following files:\n");
  boost::filesystem::directory_iterator end_iter;
  for (boost::filesystem::directory_iterator iter(input_path); iter != end_iter; iter++)
  {
    boost::filesystem::path class_dir_path(*iter);
    for (boost::filesystem::directory_iterator iter2(class_dir_path); iter2 != end_iter; iter2++)
    {
      boost::filesystem::path file(*iter2);
      if (file.extension() == ".vtk")
      {
        files_to_process.push_back(file.c_str());
        PCL_INFO("\t%s\n", file.c_str());
      }
    }

  }

  // Check if there are any .vtk files to process
  if (files_to_process.size() == 0)
  {
    PCL_ERROR("Directory %s has no .vtk files.", input_path.c_str());
    return -1;
  }

  // Iterate over all files
  for (size_t i = 0; i < files_to_process.size(); i++)
  {
    vtkSmartPointer<vtkPolyData> model;
    vtkSmartPointer<vtkPolyDataReader> reader = vtkPolyDataReader::New();
    vtkSmartPointer<vtkTransform> transform = vtkTransform::New();
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());

    // Compute output directory for this model
    std::vector<std::string> st;
    boost::split(st, files_to_process.at(i), boost::is_any_of("/"), boost::token_compress_on);
    std::string class_dirname = st.at(st.size() - 2);
    std::string dirname = st.at(st.size() - 1);
    dirname = dirname.substr(0, dirname.size() - 4);
    dirname = output_dir + class_dirname + "/" + dirname;

    // Check if output directory for this model exists. If not create
    boost::filesystem::path dirpath(dirname);
    if (!boost::filesystem::exists(dirpath))
    {
      if (!boost::filesystem::create_directories(dirpath))
      {
        PCL_ERROR ("Error creating directory %s.\n", dirpath.c_str ());
        return -1;
      }
    }

    // Load model from file
    reader->SetFileName(files_to_process.at(i).c_str());
    reader->Update();
    model = reader->GetOutput();
    PCL_INFO("Number of points %d\n",model->GetNumberOfPoints());

    // Coumpute bounds and center of the model
    double bounds[6];
    model->GetBounds(bounds);
    double min_z_value = bounds[4];

    double center[3];
    model->GetCenter(center);

    createFullModelPointcloud(model, full_model_n_points, *cloud);
    pcl::io::savePCDFile(dirname + "/full.pcd", *cloud);

    // Initialize PCLVisualizer. Add model to scene.
    pcl::visualization::PCLVisualizer viz;
    viz.initCameraParameters();
    viz.updateCamera();
    viz.setCameraPosition(0, 0, 0, 1, 0, 0, 0, 0, 1);
    viz.addModelFromPolyData(model, transform);
    viz.setRepresentationToSurfaceForAllActors();

    // Iterate over all shifts
    for (size_t shift_index = 0; shift_index < shift.size(); shift_index++)
    {

      // Iterate over all tilts
      for (size_t tilt_index = 0; tilt_index < tilt.size(); tilt_index++)
      {

        // Iterate over all distances
        for (size_t distance_index = 0; distance_index < distances.size(); distance_index++)
        {

          // Iterate over all angles
          double angle = 0;
          double angle_step = 360.0 / num_views;
          for (int angle_index = 0; angle_index < num_views; angle_index++)
          {

            // Set transformation with distance, shift, tilt and angle parameters.
            transform->Identity();
            transform->RotateY(tilt[tilt_index]);
            transform->Translate(distances[distance_index], shift[shift_index], -(height + min_z_value));
            transform->RotateZ(angle);

		/*		

            // Render pointcloud
            viz.renderView(640, 480, cloud);

            //Add noise
            addNoise(cloud, noise_std);

            // Compute new coordinates of the model center
            double new_center[3];
            transform->TransformPoint(center, new_center);

            // Shift origin of the poincloud to the model center and align with initial coordinate system.
            pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_transformed(new pcl::PointCloud<pcl::PointXYZ>());
            moveToNewCenterAndAlign(cloud, cloud_transformed, new_center, tilt[tilt_index]);

            // Compute file name for this pointcloud and save it
            std::stringstream ss;
            ss << dirname << "/rotation" << angle << "_distance" << distances[distance_index] << "_tilt"
                << tilt[tilt_index] << "_shift" << shift[shift_index] << ".pcd";
            pcl_INFO("Writing %d points to file %s\n", cloud->points.size(), ss.str().c_str());
            pcl::io::savePCDFile(ss.str(), *cloud_transformed);

		*/

            // increment angle by step
            angle += angle_step;
	
	

          }

        }
      }
    }

  }

  return 0;
}
int
main (int argc, char *argv[])
{

	PointCloudT::Ptr	cloud (new PointCloudT),
				cloud_inliers_table (new PointCloudT),
				cloud_outliers_table (new PointCloudT),
				cloud_edge (new PointCloudT),
				cloud_edge_projected (new PointCloudT),
				cloud_poligonal_prism (new PointCloudT);


	/*// Load point cloud
	if (pcl::io::loadPCDFile ("caixacomobjectos.pcd", *cloud) < 0) {
		PCL_ERROR ("Could not load PCD file !\n");
		return (-1);
	}*/

	// Load point cloud
	if (pcl::io::loadPCDFile (argv[1], *cloud) < 0) {
		PCL_ERROR ("Could not load PCD file !\n");
		return (-1);
	}

///////////////////////////////////////////
//          Table Plane Extract          //
///////////////////////////////////////////

	// Segment the ground
	pcl::ModelCoefficients::Ptr plane_table (new pcl::ModelCoefficients);
	pcl::PointIndices::Ptr inliers_plane_table (new pcl::PointIndices);
	PointCloudT::Ptr cloud_plane_table (new PointCloudT);

	// Make room for a plane equation (ax+by+cz+d=0)
	plane_table->values.resize (4);

	pcl::SACSegmentation<PointT> seg_table;	// Create the segmentation object
	seg_table.setOptimizeCoefficients (true);			// Optional
	seg_table.setMethodType (pcl::SAC_RANSAC);
	seg_table.setModelType (pcl::SACMODEL_PLANE);
	seg_table.setDistanceThreshold (0.025f);
	seg_table.setInputCloud (cloud);
	seg_table.segment (*inliers_plane_table, *plane_table);

	if (inliers_plane_table->indices.size () == 0) {
		PCL_ERROR ("Could not estimate a planar model for the given dataset.\n");
		return (-1);
	}

	// Extract inliers
	pcl::ExtractIndices<PointT> extract;
	extract.setInputCloud (cloud);
	extract.setIndices (inliers_plane_table);
	extract.setNegative (false);			// Extract the inliers
	extract.filter (*cloud_inliers_table);		// cloud_inliers contains the plane

	// Extract outliers
	//extract.setInputCloud (cloud);		// Already done line 50
	//extract.setIndices (inliers);			// Already done line 51
	extract.setNegative (true);				// Extract the outliers
	extract.filter (*cloud_outliers_table);		// cloud_outliers contains everything but the plane

	printf ("Plane segmentation equation [ax+by+cz+d]=0: [%3.4f | %3.4f | %3.4f | %3.4f]     \t\n", 
			plane_table->values[0], plane_table->values[1], plane_table->values[2] , plane_table->values[3]);




///////////////////////////////////////////
//           Box Edge Extract            //
///////////////////////////////////////////
	
	pcl::PassThrough<PointT> pass;
    	pass.setInputCloud (cloud_outliers_table);
    	pass.setFilterFieldName ("z");
    	pass.setFilterLimits (0.63, 0.68);
    	pass.filter (*cloud_edge);

	pcl::ModelCoefficients::Ptr coefficients_edge (new pcl::ModelCoefficients);
	pcl::PointIndices::Ptr inliers_edge (new pcl::PointIndices);
	// Create the segmentation object
	pcl::SACSegmentation<pcl::PointXYZ> seg_edge;
	// Optional
	seg_edge.setOptimizeCoefficients (true);
	// Mandatory
	seg_edge.setModelType (pcl::SACMODEL_PLANE);
	seg_edge.setMethodType (pcl::SAC_RANSAC);
	seg_edge.setDistanceThreshold (0.01);

	seg_edge.setInputCloud (cloud_edge);
	seg_edge.segment (*inliers_edge, *coefficients_edge);


	///////////////////////////////
	// Project the model inliers //
	///////////////////////////////
	pcl::ProjectInliers<pcl::PointXYZ> proj_edge;
	proj_edge.setModelType (pcl::SACMODEL_PLANE);
	proj_edge.setIndices (inliers_edge);
	proj_edge.setInputCloud (cloud_edge);
	proj_edge.setModelCoefficients (coefficients_edge);
	proj_edge.filter (*cloud_edge_projected);

	double merge = 0.75;
	scale(cloud_edge_projected, merge);

	
	////////////////////////////////////////////////////////////////////
	//       Create a Concave Hull representation of the box edge     //
	////////////////////////////////////////////////////////////////////
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_edge_hull (new pcl::PointCloud<pcl::PointXYZ>);
	pcl::ConvexHull<pcl::PointXYZ> chull;
	chull.setInputCloud (cloud_edge_projected);
	//chull.setAlpha (0.1);
	chull.reconstruct (*cloud_edge_hull);

	///////////////////////////////////////////////
	// Create a Poligonal Prism of the box edge  //
	///////////////////////////////////////////////
	pcl::PointIndices::Ptr inliers_poligonal_prism (new pcl::PointIndices);
	double z_min = -1.0, z_max = 0.0; // we want the points above the plane, no farther than 5 cm from the surface
	pcl::ExtractPolygonalPrismData<pcl::PointXYZ> prism;
	prism.setInputCloud (cloud_outliers_table);
	prism.setInputPlanarHull (cloud_edge_hull);
	prism.setHeightLimits (z_min, z_max);
	prism.segment (*inliers_poligonal_prism);

	// Extract poligonal inliers
	pcl::ExtractIndices<PointT> extract_polygonal_data;
	extract_polygonal_data.setInputCloud (cloud_outliers_table);
	extract_polygonal_data.setIndices (inliers_poligonal_prism);
	extract_polygonal_data.setNegative (false);		// Extract the inliers
	extract_polygonal_data.filter (*cloud_poligonal_prism);	// cloud_poligonal_prism contains the box


	//guarda a nuvem filtrada num novo ficheiro 
	pcl::io::savePCDFileASCII ("test_seg_3pecas.pcd", *cloud_poligonal_prism);

///////////////////
// Visualization //
///////////////////
	pcl::visualization::PCLVisualizer viewer ("PCL visualizer");

	// Table Plane
	pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_inliers_table_handler (cloud, 20, 255, 255); 
	viewer.addPointCloud (cloud_inliers_table, cloud_inliers_table_handler, "cloud inliers");

	// Everything else in GRAY
	pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_outliers_table_handler (cloud, 200, 200, 200); 
	viewer.addPointCloud (cloud_outliers_table, cloud_outliers_table_handler, "cloud outliers");
	
	// Edge in Green
	pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_edge_handler (cloud, 20, 255, 20); 
	viewer.addPointCloud (cloud_edge, cloud_edge_handler, "cloud edge");

	// Edge projected
	pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_edge_projected_handler (cloud, 255, 69, 20); 
	viewer.addPointCloud (cloud_edge_projected, cloud_edge_projected_handler, "cloud edge projected");
	
	// Edge hull
	pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_edge_hull_handler (cloud, 255, 20, 20); 
	viewer.addPointCloud (cloud_edge_hull, cloud_edge_hull_handler, "cloud edge hull");

	// Poligonal Prism data
	pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_poligonal_prism_handler (cloud, 20, 20, 255); 
	viewer.addPointCloud (cloud_poligonal_prism, cloud_poligonal_prism_handler, "cloud Poligonal Prism");
	

	while (!viewer.wasStopped ()) {
		viewer.spinOnce ();
	}
	return (0);
}
Esempio n. 6
0
void
pcl::kinfuLS::WorldModel<PointT>::getWorldAsCubes (const double size, std::vector<typename WorldModel<PointT>::PointCloudPtr> &cubes, std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &transforms, double overlap)
{
  
  if(world_->points.size () == 0)
  {
	PCL_INFO("The world is empty, returning nothing\n");
	return;
  }

  PCL_INFO ("Getting world as cubes. World contains %d points.\n", world_->points.size ());

  // remove nans from world cloud
  world_->is_dense = false;
  std::vector<int> indices;
  pcl::removeNaNFromPointCloud ( *world_, *world_, indices);
	
  PCL_INFO ("World contains %d points after nan removal.\n", world_->points.size ());
  

  // check cube size value
  double cubeSide = size;
  if (cubeSide <= 0.0f)
  {
	PCL_ERROR ("Size of the cube must be positive and non null (%f given). Setting it to 3.0 meters.\n", cubeSide);
	cubeSide = 512.0f;
  }

  std::cout << "cube size is set to " << cubeSide << std::endl;

  // check overlap value
  double step_increment = 1.0f - overlap;
  if (overlap < 0.0)
  {
	PCL_ERROR ("Overlap ratio must be positive or null (%f given). Setting it to 0.0 procent.\n", overlap);
	step_increment = 1.0f;
  }
  if (overlap > 1.0)
  {
	PCL_ERROR ("Overlap ratio must be less or equal to 1.0 (%f given). Setting it to 10 procent.\n", overlap);
	step_increment = 0.1f;
  }

  
  // get world's bounding values on XYZ
  PointT min, max;
  pcl::getMinMax3D(*world_, min, max);

  PCL_INFO ("Bounding box for the world: \n\t [%f - %f] \n\t [%f - %f] \n\t [%f - %f] \n", min.x, max.x, min.y, max.y, min.z, max.z);

  PointT origin = min;
  
  // clear returned vectors
  cubes.clear();
  transforms.clear();

  // iterate with box filter
  while (origin.x < max.x)
  {
	origin.y = min.y;
	while (origin.y < max.y)
	{
	  origin.z = min.z;
	  while (origin.z < max.z)
	  {
		// extract cube here
		PCL_INFO ("Extracting cube at: [%f, %f, %f].\n",  origin.x,  origin.y,  origin.z);

		// pointcloud for current cube.
		PointCloudPtr box (new pcl::PointCloud<PointT>);


		// set conditional filter
		ConditionAndPtr range_cond (new pcl::ConditionAnd<PointT> ());
		range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::GE, origin.x)));
		range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::LT, origin.x + cubeSide)));
		range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::GE, origin.y)));
		range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::LT, origin.y + cubeSide)));
		range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::GE, origin.z)));
		range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::LT, origin.z + cubeSide)));

		// build the filter
		pcl::ConditionalRemoval<PointT> condrem;
		condrem.setCondition (range_cond);
		condrem.setInputCloud (world_);
		condrem.setKeepOrganized(false);
		// apply filter
		condrem.filter (*box);

		// also push transform along with points.
		if(box->points.size() > 0)
		{
		  Eigen::Vector3f transform;
		  transform[0] = origin.x, transform[1] = origin.y, transform[2] = origin.z;
		  transforms.push_back(transform);
		  cubes.push_back(box);        
		}
		else
		{
		  PCL_INFO ("Extracted cube was empty, skiping this one.\n");
		}
		origin.z += cubeSide * step_increment;
	  }
	  origin.y += cubeSide * step_increment;
	}
	origin.x += cubeSide * step_increment;
  }


 /* for(int c = 0 ; c < cubes.size() ; ++c)
  {
	std::stringstream name;
	name << "cloud" << c+1 << ".pcd";
	pcl::io::savePCDFileASCII(name.str(), *(cubes[c]));
	
  }*/

  std::cout << "returning " << cubes.size() << " cubes" << std::endl;

}
Esempio n. 7
0
template <typename PointSource, typename PointTarget> void
pcl::IterativeClosestPoint<PointSource, PointTarget>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess)
{
  // Allocate enough space to hold the results
  std::vector<int> nn_indices (1);
  std::vector<float> nn_dists (1);

  // Point cloud containing the correspondences of each point in <input, indices>
  PointCloudTarget input_corresp;
  input_corresp.points.resize (indices_->size ());

  nr_iterations_ = 0;
  converged_ = false;
  double dist_threshold = corr_dist_threshold_ * corr_dist_threshold_;

  // If the guessed transformation is non identity
  if (guess != Eigen::Matrix4f::Identity ())
  {
    // Initialise final transformation to the guessed one
    final_transformation_ = guess;
    // Apply guessed transformation prior to search for neighbours
    transformPointCloud (output, output, guess);
  }

  // Resize the vector of distances between correspondences 
  std::vector<float> previous_correspondence_distances (indices_->size ());
  correspondence_distances_.resize (indices_->size ());

  while (!converged_)           // repeat until convergence
  {
    // Save the previously estimated transformation
    previous_transformation_ = transformation_;
    // And the previous set of distances
    previous_correspondence_distances = correspondence_distances_;

    int cnt = 0;
    std::vector<int> source_indices (indices_->size ());
    std::vector<int> target_indices (indices_->size ());

    // Iterating over the entire index vector and  find all correspondences
    for (size_t idx = 0; idx < indices_->size (); ++idx)
    {
      if (!this->searchForNeighbors (output, (*indices_)[idx], nn_indices, nn_dists))
      {
        PCL_ERROR ("[pcl::%s::computeTransformation] Unable to find a nearest neighbor in the target dataset for point %d in the source!\n", getClassName ().c_str (), (*indices_)[idx]);
        return;
      }

      // Check if the distance to the nearest neighbor is smaller than the user imposed threshold
      if (nn_dists[0] < dist_threshold)
      {
        source_indices[cnt] = (*indices_)[idx];
        target_indices[cnt] = nn_indices[0];
        cnt++;
      }

      // Save the nn_dists[0] to a global vector of distances
      correspondence_distances_[(*indices_)[idx]] = std::min (nn_dists[0], (float)dist_threshold);
    }
    if (cnt < min_number_correspondences_)
    {
      PCL_ERROR ("[pcl::%s::computeTransformation] Not enough correspondences found. Relax your threshold parameters.\n", getClassName ().c_str ());
      converged_ = false;
      return;
    }

    // Resize to the actual number of valid correspondences
    source_indices.resize (cnt); target_indices.resize (cnt);

    std::vector<int> source_indices_good;
    std::vector<int> target_indices_good;
    {
      // From the set of correspondences found, attempt to remove outliers
      // Create the registration model
      typedef typename SampleConsensusModelRegistration<PointSource>::Ptr SampleConsensusModelRegistrationPtr;
      SampleConsensusModelRegistrationPtr model;
      model.reset (new SampleConsensusModelRegistration<PointSource> (output.makeShared (), source_indices));
      // Pass the target_indices
      model->setInputTarget (target_, target_indices);
      // Create a RANSAC model
      RandomSampleConsensus<PointSource> sac (model, inlier_threshold_);
      sac.setMaxIterations (ransac_iterations_);

      // Compute the set of inliers
      if (!sac.computeModel ())
      {
        source_indices_good = source_indices;
        target_indices_good = target_indices;
      }
      else
      {
        std::vector<int> inliers;
        // Get the inliers
        sac.getInliers (inliers);
        source_indices_good.resize (inliers.size ());
        target_indices_good.resize (inliers.size ());

        boost::unordered_map<int, int> source_to_target;
        for (unsigned int i = 0; i < source_indices.size(); ++i)
          source_to_target[source_indices[i]] = target_indices[i];

        // Copy just the inliers
        std::copy(inliers.begin(), inliers.end(), source_indices_good.begin());
        for (size_t i = 0; i < inliers.size (); ++i)
          target_indices_good[i] = source_to_target[inliers[i]];
      }
    }

    // Check whether we have enough correspondences
    cnt = (int)source_indices_good.size ();
    if (cnt < min_number_correspondences_)
    {
      PCL_ERROR ("[pcl::%s::computeTransformation] Not enough correspondences found. Relax your threshold parameters.\n", getClassName ().c_str ());
      converged_ = false;
      return;
    }

    PCL_DEBUG ("[pcl::%s::computeTransformation] Number of correspondences %d [%f%%] out of %lu points [100.0%%], RANSAC rejected: %lu [%f%%].\n", getClassName ().c_str (), cnt, (cnt * 100.0) / indices_->size (), (unsigned long)indices_->size (), (unsigned long)source_indices.size () - cnt, (source_indices.size () - cnt) * 100.0 / source_indices.size ());
  
    // Estimate the transform
    //rigid_transformation_estimation_(output, source_indices_good, *target_, target_indices_good, transformation_);
    transformation_estimation_->estimateRigidTransformation (output, source_indices_good, *target_, target_indices_good, transformation_);

    // Tranform the data
    transformPointCloud (output, output, transformation_);

    // Obtain the final transformation    
    final_transformation_ = transformation_ * final_transformation_;

    nr_iterations_++;

    // Update the vizualization of icp convergence
    if (update_visualizer_ != 0)
      update_visualizer_(output, source_indices_good, *target_, target_indices_good );

    // Various/Different convergence termination criteria
    // 1. Number of iterations has reached the maximum user imposed number of iterations (via 
    //    setMaximumIterations)
    // 2. The epsilon (difference) between the previous transformation and the current estimated transformation 
    //    is smaller than an user imposed value (via setTransformationEpsilon)
    // 3. The sum of Euclidean squared errors is smaller than a user defined threshold (via 
    //    setEuclideanFitnessEpsilon)

    if (nr_iterations_ >= max_iterations_ ||
        fabs ((transformation_ - previous_transformation_).sum ()) < transformation_epsilon_ ||
        fabs (this->getFitnessScore (correspondence_distances_, previous_correspondence_distances)) <= euclidean_fitness_epsilon_
       )
    {
      converged_ = true;
      PCL_DEBUG ("[pcl::%s::computeTransformation] Convergence reached. Number of iterations: %d out of %d. Transformation difference: %f\n",
                 getClassName ().c_str (), nr_iterations_, max_iterations_, fabs ((transformation_ - previous_transformation_).sum ()));

      PCL_DEBUG ("nr_iterations_ (%d) >= max_iterations_ (%d)\n", nr_iterations_, max_iterations_);
      PCL_DEBUG ("fabs ((transformation_ - previous_transformation_).sum ()) (%f) < transformation_epsilon_ (%f)\n",
                 fabs ((transformation_ - previous_transformation_).sum ()), transformation_epsilon_);
      PCL_DEBUG ("fabs (getFitnessScore (correspondence_distances_, previous_correspondence_distances)) (%f) <= euclidean_fitness_epsilon_ (%f)\n",
                 fabs (this->getFitnessScore (correspondence_distances_, previous_correspondence_distances)),
                 euclidean_fitness_epsilon_);

    }
  }
}
Esempio n. 8
0
Eigen::Matrix4f Calibration::stitch(pcl::PointCloud<PointT>& points, double epsilon, double maxCorrespondanceDistance)
{
	if (this->stitching.size() == 0)
	{
		pcl::copyPointCloud(points, this->stitching);
		return Eigen::Matrix4f::Identity(); // Hardcore hack !!
	}

	//	Compute surface normals and curvature
	pcl::PointCloud<PointT> tempTarget;
	pcl::copyPointCloud(this->stitching, tempTarget);

	pcl::PointCloud<pcl::PointNormal>::Ptr pointsWithNormalSource (new pcl::PointCloud<pcl::PointNormal>);
	pcl::PointCloud<pcl::PointNormal>::Ptr pointsWithNormalTarget (new pcl::PointCloud<pcl::PointNormal>);

	pcl::NormalEstimation<PointT, pcl::PointNormal> normalEstimate;
	pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT> ());
	normalEstimate.setSearchMethod(tree);
	normalEstimate.setKSearch(30);

	normalEstimate.setInputCloud(points.makeShared());
	normalEstimate.compute(*pointsWithNormalSource);
	pcl::copyPointCloud(points, *pointsWithNormalSource);

	normalEstimate.setInputCloud (tempTarget.makeShared());
	normalEstimate.compute(*pointsWithNormalTarget);
	pcl::copyPointCloud (tempTarget, *pointsWithNormalTarget);

	//	Instantiate custom point representation
	MyPointNormal pointNormal;
	//	... and weight the 'curvature' dimension so that it is balanced against x, y, and z
	float alpha[4] = {1.0, 1.0, 1.0, 1.0};
	pointNormal.setRescaleValues(alpha);

	//	Align
	pcl::IterativeClosestPointNonLinear<pcl::PointNormal, pcl::PointNormal> registration;
	registration.setTransformationEpsilon(epsilon);
	//	Set the maximum distance between two correspondences
	registration.setMaxCorrespondenceDistance(maxCorrespondanceDistance);
	//	Set the point representation
	registration.setPointRepresentation (boost::make_shared<const MyPointNormal> (pointNormal));

	registration.setInputSource(pointsWithNormalSource);
	registration.setInputTarget(pointsWithNormalTarget);
	registration.setMaximumIterations(30);

	PCL_ERROR("Source size: %d  --  Target size: %d\n", (int)pointsWithNormalSource.get()->size(), (int)pointsWithNormalTarget.get()->size());

	Eigen::Matrix4f tf = Eigen::Matrix4f::Identity();
	pcl::PointCloud<pcl::PointNormal>::Ptr regResult = pointsWithNormalSource;

	PCL_ERROR("Stitching ... ");
	registration.align(*regResult);
	PCL_ERROR("Done!\n");

	tf = registration.getFinalTransformation().inverse();

//	PCL_ERROR("\nTransform:\n");
//	PCL_ERROR("| %f %f %f %f |\n", tf(0,0), tf(0,1), tf (0,2), tf(0,3));
//	PCL_ERROR("| %f %f %f %f |\n", tf(1,0), tf(1,1), tf (1,2), tf(1,3));
//	PCL_ERROR("| %f %f %f %f |\n", tf(2,0), tf(2,1), tf (2,2), tf(2,3));
//	PCL_ERROR("| %f %f %f %f |\n\n", tf(3,0), tf(3,1), tf (3,2), tf(3,3));

	pcl::transformPointCloud(*pointsWithNormalSource, *regResult, tf);
	*regResult += *pointsWithNormalTarget;

	pcl::copyPointCloud(*regResult, this->stitching);

	return tf;
}
int
main (int argc, char** argv)
{
//  /////////////////////////////////////////////////////////////////////////////////////////////////////

  if (argc != 3)
  {
    std::cerr << "please specify the paths to the two point clouds to be registered" << std::endl;
    exit (0);
  }
//  /////////////////////////////////////////////////////////////////////////////////////////////////////

//  /////////////////////////////////////////////////////////////////////////////////////////////////////
  pcl::PointCloud<pcl::PointXYZ> inputCloud;
  pcl::PointCloud<pcl::PointXYZ> targetCloud;

  if (pcl::io::loadPCDFile<pcl::PointXYZ> (argv[1], inputCloud) == -1) //* load the file
  {
    PCL_ERROR ("Couldn't read the first .pcd file \n");
    return (-1);
  }

  if (pcl::io::loadPCDFile<pcl::PointXYZ> (argv[2], targetCloud) == -1) //* load the file
  {
    PCL_ERROR ("Couldn't read the second .pcd file \n");
    return (-1);
  }
//  /////////////////////////////////////////////////////////////////////////////////////////////////////

//  /////////////////////////////////////////////////////////////////////////////////////////////////////
  pcl::PointCloud<pcl::PointXYZ> inputCloudFiltered;
  pcl::PointCloud<pcl::PointXYZ> targetCloudFiltered;

  pcl::VoxelGrid<pcl::PointXYZ> sor;
//  sor.setLeafSize (0.01, 0.01, 0.01);
  sor.setLeafSize (0.02f, 0.02f, 0.02f);
//  sor.setLeafSize (0.05, 0.05, 0.05);
//  sor.setLeafSize (0.1, 0.1, 0.1);
//  sor.setLeafSize (0.4, 0.4, 0.4);
//  sor.setLeafSize (0.5, 0.5, 0.5);

  sor.setInputCloud (inputCloud.makeShared());
  std::cout<<"\n inputCloud.size()="<<inputCloud.size()<<std::endl;
  sor.filter (inputCloudFiltered);
  std::cout<<"\n inputCloudFiltered.size()="<<inputCloudFiltered.size()<<std::endl;

  sor.setInputCloud (targetCloud.makeShared());
  std::cout<<"\n targetCloud.size()="<<targetCloud.size()<<std::endl;
  sor.filter (targetCloudFiltered);
  std::cout<<"\n targetCloudFiltered.size()="<<targetCloudFiltered.size()<<std::endl;
//  /////////////////////////////////////////////////////////////////////////////////////////////////////

//  /////////////////////////////////////////////////////////////////////////////////////////////////////
//  pcl::PointCloud<pcl::PointXYZ>::ConstPtr source = inputCloud.makeShared();
//  pcl::PointCloud<pcl::PointXYZ>::ConstPtr target = targetCloud.makeShared();

  pcl::PointCloud<pcl::PointXYZ>::ConstPtr source = inputCloudFiltered.makeShared();
  pcl::PointCloud<pcl::PointXYZ>::ConstPtr target = targetCloudFiltered.makeShared();

  pcl::PointCloud<pcl::PointXYZ> source_aligned;
//  /////////////////////////////////////////////////////////////////////////////////////////////////////

//  /////////////////////////////////////////////////////////////////////////////////////////////////////
  pcl::RegistrationVisualizer<pcl::PointXYZ, pcl::PointXYZ> registrationVisualizer;

  registrationVisualizer.startDisplay();

  registrationVisualizer.setMaximumDisplayedCorrespondences (100);
//  /////////////////////////////////////////////////////////////////////////////////////////////////////

//  ///////////////////////////////////////////////////////////////////////////////////////////////////
  pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;

  icp.setMaximumIterations(10000);

//  icp.setMaxCorrespondenceDistance (0.6);
  icp.setMaxCorrespondenceDistance (0.8);
//  icp.setMaxCorrespondenceDistance (1.5);

//  icp.setRANSACOutlierRejectionThreshold (0.1);
  icp.setRANSACOutlierRejectionThreshold (0.6);
//  icp.setRANSACOutlierRejectionThreshold (1.5);
//  icp.setRANSACOutlierRejectionThreshold (5.0);

  icp.setInputCloud  (source);
  icp.setInputTarget (target);

  // Register the registration algorithm to the RegistrationVisualizer
  registrationVisualizer.setRegistration (icp);

  // Start registration process
  icp.align (source_aligned);

  std::cout << "has converged:" << icp.hasConverged () << " score: " << icp.getFitnessScore () << std::endl;
  std::cout << icp.getFinalTransformation () << std::endl;
//  ///////////////////////////////////////////////////////////////////////////////////////////////////

//  ///////////////////////////////////////////////////////////////////////////////////////////////////
//
//  pcl::IterativeClosestPointNonLinear<pcl::PointXYZ, pcl::PointXYZ> icpnl;
//
//  icpnl.setMaximumIterations(10000);
//
//  icpnl.setMaxCorrespondenceDistance (0.6);
////  icpnl.setMaxCorrespondenceDistance (0.8);
////  icpnl.setMaxCorrespondenceDistance (1.5);
//
////  icpnl.setRANSACOutlierRejectionThreshold (0.1);
//  icpnl.setRANSACOutlierRejectionThreshold (0.8);
////  icpnl.setRANSACOutlierRejectionThreshold (1.5);
////  icpnl.setRANSACOutlierRejectionThreshold (5.0);
//
//  icpnl.setInputCloud  (source);
//  icpnl.setInputTarget (target);
//
//  // Register the registration algorithm to the RegistrationVisualizer
//  registrationVisualizer.setRegistration (icpnl);
//
//  // Start registration process
//  icpnl.align (source_aligned);
//
//  std::cout << "has converged:" << icpnl.hasConverged () << " score: " << icpnl.getFitnessScore () << std::endl;
//  std::cout << icpnl.getFinalTransformation () << std::endl;
//
//  ///////////////////////////////////////////////////////////////////////////////////////////////////

}
Esempio n. 10
0
//////////////////////////////////////////////////////////////////////////////
//const pcl::PointCloud<pcl::PointXYZRGBA>::Ptr pcl::StereoMatching::getPointCloud(float uC, float vC, float focal, float baseline)
bool 
pcl::StereoMatching::getPointCloud (
    float u_c, float v_c, float focal, float baseline, 
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
  
  //disp map has not been computed yet..
  if ( disp_map_ == NULL)
  {

    PCL_ERROR(
      "[pcl::StereoMatching::getPointCloud] Error: a disparity map has not been computed yet. The resulting cloud can not be computed..\n"
    );

    return false;
  }

  //cloud needs to be re-allocated
  if (cloud->width != width_ || cloud->height != height_)
  {
    cloud->resize(width_*height_);
    cloud->width = width_;
    cloud->height = height_;
    cloud->is_dense = false;
  }

  if ( cloud->is_dense)
    cloud->is_dense = false;

  //Loop
  pcl::PointXYZ temp_point;
  pcl::PointXYZ nan_point;
  nan_point.x = std::numeric_limits<float>::quiet_NaN();
  nan_point.y = std::numeric_limits<float>::quiet_NaN();
  nan_point.z = std::numeric_limits<float>::quiet_NaN();
  //nan_point.intensity = std::numeric_limits<float>::quiet_NaN();

  //all disparities are multiplied by a constant equal to 16; 
  //this must be taken into account when computing z values
  float depth_scale = baseline * focal * 16.0f;

  for ( int j=0; j<height_; j++)
  {
    for ( int i=0; i<width_; i++)
    {
      if ( disp_map_[ j*width_ + i] > 0 )
      {

        temp_point.z = depth_scale / disp_map_[j * width_ + i];
        temp_point.x = ((static_cast<float> (i) - u_c) * temp_point.z) / focal;
        temp_point.y = ((static_cast<float> (j) - v_c) * temp_point.z) / focal;
        //temp_point.intensity = 255;

        (*cloud)[j * width_ + i] = temp_point;
      }
      //adding NaN value
      else
      {
        (*cloud)[j * width_ + i] = nan_point;
      }
    }
  }

  return (true);
}
Esempio n. 11
0
void 
pcl::GrayStereoMatching::compute (unsigned char* ref_img, unsigned char* trg_img, int width, int height)
{
    
  //Check that a suitable value of max_disp has been selected
  if ( max_disp_ <= 0)
  {
    PCL_ERROR(
      "[pcl::StereoMatching::compute] Error. A positive max_disparity value has not be correctly inserted. Aborting..\n"
    );
    return;
  }

  if ( (disp_map_ != NULL) && (width_ != width || height_ != height) )
  {
    delete [] disp_map_;
    disp_map_ = NULL;

    if ( disp_map_trg_ != NULL)
    {
      delete [] disp_map_trg_;
      disp_map_trg_ = NULL;
    }

    if ( pp_ref_img_ != NULL)
    {
      delete [] pp_ref_img_;
      delete [] pp_trg_img_;
      pp_ref_img_ = NULL;
      pp_trg_img_ = NULL;
    }
  }

  if ( disp_map_ == NULL)
  {
    disp_map_ = new short int[width * height];  
      
    width_ = width;
    height_ = height;
  }
    

  if ( is_lr_check_ && disp_map_trg_ == NULL)
  {
    disp_map_trg_ = new short int[width * height];  
  }

  if ( !is_lr_check_ && disp_map_trg_ != NULL)
  {
    delete [] disp_map_trg_;
    disp_map_trg_ = NULL;
  }

  if ( is_pre_proc_ && pp_ref_img_ == NULL)
  {
    pp_ref_img_ = new unsigned char[width_*height_];
    pp_trg_img_ = new unsigned char[width_*height_];
  }

  if ( !is_pre_proc_ && pp_ref_img_ != NULL)
  {
    delete [] pp_ref_img_;
    delete [] pp_trg_img_;
    pp_ref_img_ = NULL;
    pp_trg_img_ = NULL;
  }

  memset(disp_map_, 0, sizeof(short int)*height_*width_);

  if ( is_pre_proc_)
  {
    preProcessing(ref_img, pp_ref_img_);
    preProcessing(trg_img, pp_trg_img_);
  }

  if (is_lr_check_)
  {

    if ( is_pre_proc_)
    {
      imgFlip(pp_ref_img_);
      imgFlip(pp_trg_img_);

      compute_impl(pp_trg_img_, pp_ref_img_);

      imgFlip(pp_ref_img_);
      imgFlip(pp_trg_img_);
    }
    else
    {
      imgFlip(ref_img);
      imgFlip(trg_img);

      compute_impl(trg_img, ref_img);

      imgFlip(ref_img);
      imgFlip(trg_img);
    }

    for (int j = 0; j < height_; j++)
      for (int i = 0; i < width_; i++)
        disp_map_trg_[j * width_ + i] = disp_map_[j * width_ + width_ - 1 - i];

  }

  if ( is_pre_proc_)
    compute_impl(pp_ref_img_, pp_trg_img_);
  else
    compute_impl(ref_img, trg_img);

  if ( is_lr_check_)
  {
    leftRightCheck();
  }

  //at the end, x_offset (*16) needs to be added to all computed disparities, 
  //so that each fixed point value of the disparity map represents the true disparity value multiplied by 16  
  for (int j = 0; j < height_; j++)
    for (int i = 0; i < width_; i++)
      if ( disp_map_[j * width_ + i] > 0)
	    disp_map_[j * width_ + i] += static_cast<short int> (x_off_ * 16);

}
Esempio n. 12
0
bool 
pcl::StereoMatching::getPointCloud (
    float u_c, float v_c, float focal, float baseline, 
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, 
    pcl::PointCloud<pcl::RGB>::Ptr texture) 
{
  //disp map has not been computed yet..
  if (disp_map_ == NULL)
  {
    PCL_ERROR ("[pcl::StereoMatching::getPointCloud] Error: a disparity map has not been computed yet. The resulting cloud can not be computed..\n");

    return (false);
  }

  if (texture->width != width_ || texture->height != height_)
  {
    PCL_ERROR("[pcl::StereoMatching::getPointCloud] Error: the size of the texture cloud does not match that of the computed range map. The resulting cloud can not be computed..\n");
    return (false);
  }

  //cloud needs to be re-allocated
  if (cloud->width != width_ || cloud->height != height_)
  {
    //cloud.reset(new pcl::PointCloud<pcl::PointXYZRGBA>(width_, height_) );
    cloud->resize (width_ * height_);
    cloud->width = width_;
    cloud->height = height_;
    cloud->is_dense = false;
  }

  //Loop
  pcl::PointXYZRGB temp_point;
  /*pcl::PointXYZRGB nan_point;
  nan_point.x = std::numeric_limits<float>::quiet_NaN();
  nan_point.y = std::numeric_limits<float>::quiet_NaN();
  nan_point.z = std::numeric_limits<float>::quiet_NaN();
  nan_point.r = std::numeric_limits<unsigned char>::quiet_NaN();
  nan_point.g = std::numeric_limits<unsigned char>::quiet_NaN();
  nan_point.b = std::numeric_limits<unsigned char>::quiet_NaN();*/

  //all disparities are multiplied by a constant equal to 16; 
  //this must be taken into account when computing z values
  float depth_scale = baseline * focal * 16.0f;

  for (int j = 0; j < height_; j++)
  {
    for (int i = 0; i < width_; i++)
    {
      if (disp_map_[ j*width_ + i] > 0)
      {
        temp_point.z = (depth_scale) / (disp_map_[ j*width_ + i]);
        temp_point.x = ((static_cast<float> (i) - u_c) * temp_point.z) / focal;
        temp_point.y = ((static_cast<float> (j) - v_c) * temp_point.z) / focal;

        //temp_point.intensity = ( texture->at(j*width_+i).r +texture->at(j*width_+i).g + texture->at(j*width_+i).b) / 3.0f;
        temp_point.r = texture->at (j * width_ + i).r;
        temp_point.g = texture->at (j * width_ + i).g; 
        temp_point.b = texture->at (j * width_ + i).b; 

        (*cloud)[j*width_ + i] = temp_point;
      }
      //adding NaN value
      else
      {
        temp_point.x = std::numeric_limits<float>::quiet_NaN();
        temp_point.y = std::numeric_limits<float>::quiet_NaN();
        temp_point.z = std::numeric_limits<float>::quiet_NaN();
        temp_point.r = texture->at (j * width_ + i).r;
        temp_point.g = texture->at (j * width_ + i).g; 
        temp_point.b = texture->at (j * width_ + i).b; 
        (*cloud)[j*width_ + i] = temp_point;
      }
    }
  }

  return (true);
}
Esempio n. 13
0
bool
pcl::concatenateFields (const sensor_msgs::PointCloud2 &cloud1,
                        const sensor_msgs::PointCloud2 &cloud2,
                        sensor_msgs::PointCloud2 &cloud_out)
{
    // If the cloud's sizes differ (points wise), then exit with error
    if (cloud1.width != cloud2.width || cloud1.height != cloud2.height)
    {
        PCL_ERROR ("[pcl::concatenateFields] Dimensions of input clouds do not match: cloud1 (w, %d, h, %d), cloud2 (w, %d, h, %d)\n", cloud1.width, cloud1.height, cloud2.width, cloud2.height );
        return (false);
    }


    if (cloud1.is_bigendian != cloud2.is_bigendian)
    {
        PCL_ERROR ("[pcl::concatenateFields] Endianness of clouds does not match\n");
        return (false);
    }

    // Else, copy the second cloud (width, height, header stay the same)
    // we do this since fields from the second cloud are supposed to overwrite
    // those of the first
    cloud_out.header = cloud2.header;
    cloud_out.fields = cloud2.fields;
    cloud_out.width = cloud2.width;
    cloud_out.height = cloud2.height;
    cloud_out.is_bigendian = cloud2.is_bigendian;

    //We need to find how many fields overlap between the two clouds
    size_t total_fields = cloud2.fields.size ();

    //for the non-matching fields in cloud1, we need to store the offset
    //from the beginning of the point
    std::vector<const sensor_msgs::PointField*> cloud1_unique_fields;
    std::vector<int> field_sizes;

    //We need to make sure that the fields for cloud 1 are sorted
    //by offset so that we can compute sizes correctly. There is no
    //guarantee that the fields are in the correct order when they come in
    std::vector<const sensor_msgs::PointField*> cloud1_fields_sorted;
    for (size_t i = 0; i < cloud1.fields.size (); ++i)
        cloud1_fields_sorted.push_back (&(cloud1.fields[i]));

    std::sort (cloud1_fields_sorted.begin (), cloud1_fields_sorted.end (), fieldComp);

    for (size_t i = 0; i < cloud1_fields_sorted.size (); ++i)
    {
        bool match = false;
        for (size_t j = 0; j < cloud2.fields.size (); ++j)
        {
            if (cloud1_fields_sorted[i]->name == cloud2.fields[j].name)
                match = true;
        }

        //if the field is new, we'll increment out total fields
        if (!match && cloud1_fields_sorted[i]->name != "_")
        {
            cloud1_unique_fields.push_back (cloud1_fields_sorted[i]);

            int size = 0;
            size_t next_valid_field = i + 1;

            while (next_valid_field < cloud1_fields_sorted.size())
            {
                if (cloud1_fields_sorted[next_valid_field]->name != "_")
                    break;
                next_valid_field++;
            }

            if (next_valid_field < cloud1_fields_sorted.size ())
                //compute the true size of the field, including padding
                size = cloud1_fields_sorted[next_valid_field]->offset - cloud1_fields_sorted[i]->offset;
            else
                //for the last point, we'll just use the point step to compute the size
                size = cloud1.point_step - cloud1_fields_sorted[i]->offset;

            field_sizes.push_back (size);

            total_fields++;
        }
    }

    //we need to compute the size of the additional data added from cloud 1
    uint32_t cloud1_unique_point_step = 0;
    for (size_t i = 0; i < cloud1_unique_fields.size (); ++i)
        cloud1_unique_point_step += field_sizes[i];

    //the total size of extra data should be the size of data per point
    //multiplied by the total number of points in the cloud
    uint32_t cloud1_unique_data_size = cloud1_unique_point_step * cloud1.width * cloud1.height;

    // Point step must increase with the length of each matching field
    cloud_out.point_step = cloud2.point_step + cloud1_unique_point_step;
    // Recalculate row_step
    cloud_out.row_step = cloud_out.point_step * cloud_out.width;

    // Resize data to hold all clouds
    cloud_out.data.resize (cloud2.data.size () + cloud1_unique_data_size);

    // Concatenate fields
    cloud_out.fields.resize (cloud2.fields.size () + cloud1_unique_fields.size ());
    int offset = cloud2.point_step;

    for (size_t d = 0; d < cloud1_unique_fields.size (); ++d)
    {
        const sensor_msgs::PointField& f = *cloud1_unique_fields[d];
        cloud_out.fields[cloud2.fields.size () + d].name = f.name;
        cloud_out.fields[cloud2.fields.size () + d].datatype = f.datatype;
        cloud_out.fields[cloud2.fields.size () + d].count = f.count;
        // Adjust the offset
        cloud_out.fields[cloud2.fields.size () + d].offset = offset;
        offset += field_sizes[d];
    }

    // Iterate over each point and perform the appropriate memcpys
    int point_offset = 0;
    for (size_t cp = 0; cp < cloud_out.width * cloud_out.height; ++cp)
    {
        memcpy (&cloud_out.data[point_offset], &cloud2.data[cp * cloud2.point_step], cloud2.point_step);
        int field_offset = cloud2.point_step;

        // Copy each individual point, we have to do this on a per-field basis
        // since some fields are not unique
        for (size_t i = 0; i < cloud1_unique_fields.size (); ++i)
        {
            const sensor_msgs::PointField& f = *cloud1_unique_fields[i];
            int local_data_size = f.count * pcl::getFieldSize (f.datatype);
            int padding_size = field_sizes[i] - local_data_size;

            memcpy (&cloud_out.data[point_offset + field_offset], &cloud1.data[cp * cloud1.point_step + f.offset], local_data_size);
            field_offset +=  local_data_size;

            //make sure that we add padding when its needed
            if (padding_size > 0)
                memset (&cloud_out.data[point_offset + field_offset], 0, padding_size);
            field_offset += padding_size;
        }
        point_offset += field_offset;
    }

    if (!cloud1.is_dense || !cloud2.is_dense)
        cloud_out.is_dense = false;
    else
        cloud_out.is_dense = true;

    return (true);
}
Esempio n. 14
0
template <typename PointXYZT, typename PointRGBT> bool
pcl::LineRGBD<PointXYZT, PointRGBT>::loadTemplates (const std::string &file_name, const size_t object_id)
{
  // Open the file
  int ltm_fd = pcl_open (file_name.c_str (), O_RDONLY);
  if (ltm_fd == -1)
    return (false);
  
  int ltm_offset = 0;

  pcl::io::TARHeader ltm_header;
  PCDReader pcd_reader;

  std::string pcd_ext (".pcd");
  std::string sqmmt_ext (".sqmmt");

  // While there still is an LTM header to be read
  while (readLTMHeader (ltm_fd, ltm_header))
  {
    ltm_offset += 512;

    // Search for extension
    std::string chunk_name (ltm_header.file_name);
    std::transform (chunk_name.begin (), chunk_name.end (), chunk_name.begin (), ::tolower);
    std::string::size_type it;

    if ((it = chunk_name.find (pcd_ext)) != std::string::npos &&
        (pcd_ext.size () - (chunk_name.size () - it)) == 0)
    {
      PCL_DEBUG ("[pcl::LineRGBD::loadTemplates] Reading and parsing %s as a PCD file.\n", chunk_name.c_str ());
      // Read the next PCD file
      template_point_clouds_.resize (template_point_clouds_.size () + 1);
      pcd_reader.read (file_name, template_point_clouds_[template_point_clouds_.size () - 1], ltm_offset);

      // Increment the offset for the next file
      ltm_offset += (ltm_header.getFileSize ()) + (512 - ltm_header.getFileSize () % 512);
    }
    else if ((it = chunk_name.find (sqmmt_ext)) != std::string::npos &&
             (sqmmt_ext.size () - (chunk_name.size () - it)) == 0)
    {
      PCL_DEBUG ("[pcl::LineRGBD::loadTemplates] Reading and parsing %s as a SQMMT file.\n", chunk_name.c_str ());

      unsigned int fsize = ltm_header.getFileSize ();
      char *buffer = new char[fsize];
      int result = static_cast<int> (::read (ltm_fd, reinterpret_cast<char*> (&buffer[0]), fsize));
      if (result == -1)
      {
        delete [] buffer;
        PCL_ERROR ("[pcl::LineRGBD::loadTemplates] Error reading SQMMT template from file!\n");
        break;
      }

      // Read a SQMMT file
      std::stringstream stream (std::stringstream::in | std::stringstream::out | std::stringstream::binary);
      stream.write (buffer, fsize);
      SparseQuantizedMultiModTemplate sqmmt;
      sqmmt.deserialize (stream);

      linemod_.addTemplate (sqmmt);
      object_ids_.push_back (object_id);

      // Increment the offset for the next file
      ltm_offset += (ltm_header.getFileSize ()) + (512 - ltm_header.getFileSize () % 512);

      delete [] buffer;
    }

    if (static_cast<int> (pcl_lseek (ltm_fd, ltm_offset, SEEK_SET)) < 0)
      break;
  }

  // Close the file
  pcl_close (ltm_fd);

  // Compute 3D bounding boxes from the template point clouds
  bounding_boxes_.resize (template_point_clouds_.size ());
  for (size_t i = 0; i < template_point_clouds_.size (); ++i)
  {
    PointCloud<PointXYZRGBA> & template_point_cloud = template_point_clouds_[i];
    BoundingBoxXYZ & bb = bounding_boxes_[i];
    bb.x = bb.y = bb.z = std::numeric_limits<float>::max ();
    bb.width = bb.height = bb.depth = 0.0f;

    float center_x = 0.0f;
    float center_y = 0.0f;
    float center_z = 0.0f;
    float min_x = std::numeric_limits<float>::max ();
    float min_y = std::numeric_limits<float>::max ();
    float min_z = std::numeric_limits<float>::max ();
    float max_x = -std::numeric_limits<float>::max ();
    float max_y = -std::numeric_limits<float>::max ();
    float max_z = -std::numeric_limits<float>::max ();
    size_t counter = 0;
    for (size_t j = 0; j < template_point_cloud.size (); ++j)
    {
      const PointXYZRGBA & p = template_point_cloud.points[j];

      if (!pcl_isfinite (p.x) || !pcl_isfinite (p.y) || !pcl_isfinite (p.z))
        continue;

      min_x = std::min (min_x, p.x);
      min_y = std::min (min_y, p.y);
      min_z = std::min (min_z, p.z);
      max_x = std::max (max_x, p.x);
      max_y = std::max (max_y, p.y);
      max_z = std::max (max_z, p.z);

      center_x += p.x;
      center_y += p.y;
      center_z += p.z;

      ++counter;
    }

    center_x /= static_cast<float> (counter);
    center_y /= static_cast<float> (counter);
    center_z /= static_cast<float> (counter);

    bb.width  = max_x - min_x;
    bb.height = max_y - min_y;
    bb.depth  = max_z - min_z;

    bb.x = (min_x + bb.width / 2.0f) - center_x - bb.width / 2.0f;
    bb.y = (min_y + bb.height / 2.0f) - center_y - bb.height / 2.0f;
    bb.z = (min_z + bb.depth / 2.0f) - center_z - bb.depth / 2.0f;

    for (size_t j = 0; j < template_point_cloud.size (); ++j)
    {
      PointXYZRGBA p = template_point_cloud.points[j];

      if (!pcl_isfinite (p.x) || !pcl_isfinite (p.y) || !pcl_isfinite (p.z))
        continue;

      p.x -= center_x;
      p.y -= center_y;
      p.z -= center_z;

      template_point_cloud.points[j] = p;
    }
  }

  return (true);
}
void
pcl::RadiusOutlierRemoval<sensor_msgs::PointCloud2>::applyFilter (PointCloud2 &output)
{
  output.is_dense = true;
  // If fields x/y/z are not present, we cannot filter
  if (x_idx_ == -1 || y_idx_ == -1 || z_idx_ == -1)
  {
    PCL_ERROR ("[pcl::%s::applyFilter] Input dataset doesn't have x-y-z coordinates!\n", getClassName ().c_str ());
    output.width = output.height = 0;
    output.data.clear ();
    return;
  }

  if (search_radius_ == 0.0)
  {
    PCL_ERROR ("[pcl::%s::applyFilter] No radius defined!\n", getClassName ().c_str ());
    output.width = output.height = 0;
    output.data.clear ();
    return;
  }
  // Send the input dataset to the spatial locator
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::fromROSMsg (*input_, *cloud);

  // Initialize the spatial locator
  if (!tree_)
  {
    if (cloud->isOrganized ())
      tree_.reset (new pcl::search::OrganizedNeighbor<pcl::PointXYZ> ());
    else
      tree_.reset (new pcl::search::KdTree<pcl::PointXYZ> (false));
  }
  tree_->setInputCloud (cloud);

  // Allocate enough space to hold the results
  std::vector<int> nn_indices (indices_->size ());
  std::vector<float> nn_dists (indices_->size ());

  // Copy the common fields
  output.is_bigendian = input_->is_bigendian;
  output.point_step = input_->point_step;
  output.height = 1;

  output.data.resize (input_->width * input_->point_step); // reserve enough space
  removed_indices_->resize (input_->data.size ());

  int nr_p = 0;
  int nr_removed_p = 0;
  // Go over all the points and check which doesn't have enough neighbors
  for (int cp = 0; cp < static_cast<int> (indices_->size ()); ++cp)
  {
    int k = tree_->radiusSearch ((*indices_)[cp], search_radius_, nn_indices, nn_dists);
    // Check if the number of neighbors is larger than the user imposed limit
    if (k < min_pts_radius_)
    {
      if (extract_removed_indices_)
      {
        (*removed_indices_)[nr_removed_p] = cp;
        nr_removed_p++;
      }
      continue;
    }

    memcpy (&output.data[nr_p * output.point_step], &input_->data[(*indices_)[cp] * output.point_step],
            output.point_step);
    nr_p++;
  }

  output.width = nr_p;
  output.height = 1;
  output.data.resize (output.width * output.point_step);
  output.row_step = output.point_step * output.width;

  removed_indices_->resize (nr_removed_p);
}
Esempio n. 16
0
template <typename PointT> bool
pcl::registration::ELCH<PointT>::initCompute ()
{
  /*if (!PCLBase<PointT>::initCompute ())
  {
    PCL_ERROR ("[pcl::registration:ELCH::initCompute] Init failed.\n");
    return (false);
  }*/ //TODO

  if (!loop_start_)
  {
    PCL_ERROR ("[pcl::registration::ELCH::initCompute] no start of loop defined is empty!\n");
    deinitCompute ();
    return (false);
  }

  if (!loop_end_)
  {
    PCL_ERROR ("[pcl::registration::ELCH::initCompute] no end of loop defined is empty!\n");
    deinitCompute ();
    return (false);
  }

  //compute transformation if it's not given
  if (compute_loop_)
  {
    PointCloudPtr meta_start (new PointCloud);
    PointCloudPtr meta_end (new PointCloud);
    *meta_start = *(*loop_graph_)[loop_start_].cloud;
    *meta_end = *(*loop_graph_)[loop_end_].cloud;

    typename boost::graph_traits<LoopGraph>::adjacency_iterator si, si_end;
    for (boost::tuples::tie (si, si_end) = adjacent_vertices (loop_start_, *loop_graph_); si != si_end; si++)
      *meta_start += *(*loop_graph_)[*si].cloud;

    for (boost::tuples::tie (si, si_end) = adjacent_vertices (loop_end_, *loop_graph_); si != si_end; si++)
      *meta_end += *(*loop_graph_)[*si].cloud;

    //TODO use real pose instead of centroid
    //Eigen::Vector4f pose_start;
    //pcl::compute3DCentroid (*(*loop_graph_)[loop_start_].cloud, pose_start);

    //Eigen::Vector4f pose_end;
    //pcl::compute3DCentroid (*(*loop_graph_)[loop_end_].cloud, pose_end);

    PointCloudPtr tmp (new PointCloud);
    //Eigen::Vector4f diff = pose_start - pose_end;
    //Eigen::Translation3f translation (diff.head (3));
    //Eigen::Affine3f trans = translation * Eigen::Quaternionf::Identity ();
    //pcl::transformPointCloud (*(*loop_graph_)[loop_end_].cloud, *tmp, trans);

    reg_->setInputTarget (meta_start);

    reg_->setInputCloud (meta_end);

    reg_->align (*tmp);

    loop_transform_ = reg_->getFinalTransformation ();
    //TODO hack
    //loop_transform_ *= trans.matrix ();

  }

  return (true);
}
Esempio n. 17
0
File: mlesac.hpp Progetto: 2php/pcl
template <typename PointT> bool
pcl::MaximumLikelihoodSampleConsensus<PointT>::computeModel (int debug_verbosity_level)
{
  // Warn and exit if no threshold was set
  if (threshold_ == std::numeric_limits<double>::max())
  {
    PCL_ERROR ("[pcl::MaximumLikelihoodSampleConsensus::computeModel] No threshold set!\n");
    return (false);
  }

  iterations_ = 0;
  double d_best_penalty = std::numeric_limits<double>::max();
  double k = 1.0;

  std::vector<int> best_model;
  std::vector<int> selection;
  Eigen::VectorXf model_coefficients;
  std::vector<double> distances;

  // Compute sigma - remember to set threshold_ correctly !
  sigma_ = computeMedianAbsoluteDeviation (sac_model_->getInputCloud (), sac_model_->getIndices (), threshold_);
  if (debug_verbosity_level > 1)
    PCL_DEBUG ("[pcl::MaximumLikelihoodSampleConsensus::computeModel] Estimated sigma value: %f.\n", sigma_);

  // Compute the bounding box diagonal: V = sqrt (sum (max(pointCloud) - min(pointCloud)^2))
  Eigen::Vector4f min_pt, max_pt;
  getMinMax (sac_model_->getInputCloud (), sac_model_->getIndices (), min_pt, max_pt);
  max_pt -= min_pt;
  double v = sqrt (max_pt.dot (max_pt));

  int n_inliers_count = 0;
  size_t indices_size;
  unsigned skipped_count = 0;
  // supress infinite loops by just allowing 10 x maximum allowed iterations for invalid model parameters!
  const unsigned max_skip = max_iterations_ * 10;
  
  // Iterate
  while (iterations_ < k && skipped_count < max_skip)
  {
    // Get X samples which satisfy the model criteria
    sac_model_->getSamples (iterations_, selection);

    if (selection.empty ()) break;

    // Search for inliers in the point cloud for the current plane model M
    if (!sac_model_->computeModelCoefficients (selection, model_coefficients))
    {
      //iterations_++;
      ++ skipped_count;
      continue;
    }

    // Iterate through the 3d points and calculate the distances from them to the model
    sac_model_->getDistancesToModel (model_coefficients, distances);

    if (distances.empty ())
    {
      //iterations_++;
      ++skipped_count;
      continue;
    }
    
    // Use Expectiation-Maximization to find out the right value for d_cur_penalty
    // ---[ Initial estimate for the gamma mixing parameter = 1/2
    double gamma = 0.5;
    double p_outlier_prob = 0;

    indices_size = sac_model_->getIndices ()->size ();
    std::vector<double> p_inlier_prob (indices_size);
    for (int j = 0; j < iterations_EM_; ++j)
    {
      // Likelihood of a datum given that it is an inlier
      for (size_t i = 0; i < indices_size; ++i)
        p_inlier_prob[i] = gamma * exp (- (distances[i] * distances[i] ) / 2 * (sigma_ * sigma_) ) /
                           (sqrt (2 * M_PI) * sigma_);

      // Likelihood of a datum given that it is an outlier
      p_outlier_prob = (1 - gamma) / v;

      gamma = 0;
      for (size_t i = 0; i < indices_size; ++i)
        gamma += p_inlier_prob [i] / (p_inlier_prob[i] + p_outlier_prob);
      gamma /= static_cast<double>(sac_model_->getIndices ()->size ());
    }

    // Find the log likelihood of the model -L = -sum [log (pInlierProb + pOutlierProb)]
    double d_cur_penalty = 0;
    for (size_t i = 0; i < indices_size; ++i)
      d_cur_penalty += log (p_inlier_prob[i] + p_outlier_prob);
    d_cur_penalty = - d_cur_penalty;

    // Better match ?
    if (d_cur_penalty < d_best_penalty)
    {
      d_best_penalty = d_cur_penalty;

      // Save the current model/coefficients selection as being the best so far
      model_              = selection;
      model_coefficients_ = model_coefficients;

      n_inliers_count = 0;
      // Need to compute the number of inliers for this model to adapt k
      for (size_t i = 0; i < distances.size (); ++i)
        if (distances[i] <= 2 * sigma_)
          n_inliers_count++;

      // Compute the k parameter (k=log(z)/log(1-w^n))
      double w = static_cast<double> (n_inliers_count) / static_cast<double> (sac_model_->getIndices ()->size ());
      double p_no_outliers = 1 - pow (w, static_cast<double> (selection.size ()));
      p_no_outliers = (std::max) (std::numeric_limits<double>::epsilon (), p_no_outliers);       // Avoid division by -Inf
      p_no_outliers = (std::min) (1 - std::numeric_limits<double>::epsilon (), p_no_outliers);   // Avoid division by 0.
      k = log (1 - probability_) / log (p_no_outliers);
    }

    ++iterations_;
    if (debug_verbosity_level > 1)
      PCL_DEBUG ("[pcl::MaximumLikelihoodSampleConsensus::computeModel] Trial %d out of %d. Best penalty is %f.\n", iterations_, static_cast<int> (ceil (k)), d_best_penalty);
    if (iterations_ > max_iterations_)
    {
      if (debug_verbosity_level > 0)
        PCL_DEBUG ("[pcl::MaximumLikelihoodSampleConsensus::computeModel] MLESAC reached the maximum number of trials.\n");
      break;
    }
  }

  if (model_.empty ())
  {
    if (debug_verbosity_level > 0)
      PCL_DEBUG ("[pcl::MaximumLikelihoodSampleConsensus::computeModel] Unable to find a solution!\n");
    return (false);
  }

  // Iterate through the 3d points and calculate the distances from them to the model again
  sac_model_->getDistancesToModel (model_coefficients_, distances);
  std::vector<int> &indices = *sac_model_->getIndices ();
  if (distances.size () != indices.size ())
  {
    PCL_ERROR ("[pcl::MaximumLikelihoodSampleConsensus::computeModel] Estimated distances (%lu) differs than the normal of indices (%lu).\n", distances.size (), indices.size ());
    return (false);
  }

  inliers_.resize (distances.size ());
  // Get the inliers for the best model found
  n_inliers_count = 0;
  for (size_t i = 0; i < distances.size (); ++i)
    if (distances[i] <= 2 * sigma_)
      inliers_[n_inliers_count++] = indices[i];

  // Resize the inliers vector
  inliers_.resize (n_inliers_count);

  if (debug_verbosity_level > 0)
    PCL_DEBUG ("[pcl::MaximumLikelihoodSampleConsensus::computeModel] Model: %lu size, %d inliers.\n", model_.size (), n_inliers_count);

  return (true);
}
Esempio n. 18
0
template <typename PointSource, typename PointTarget, typename FeatureT> void 
pcl::SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess)
{
  if (!input_features_)
  {
    PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ());
    PCL_ERROR ("No source features were given! Call setSourceFeatures before aligning.\n");
    return;
  }
  if (!target_features_)
  {
    PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ());
    PCL_ERROR ("No target features were given! Call setTargetFeatures before aligning.\n");
    return;
  }

  if (!error_functor_)
  {
    error_functor_.reset (new TruncatedError (static_cast<float> (corr_dist_threshold_)));
  }

  std::vector<int> sample_indices (nr_samples_);
  std::vector<int> corresponding_indices (nr_samples_);
  PointCloudSource input_transformed;
  float error, lowest_error (0);

  final_transformation_ = guess;
  int i_iter = 0;
  if (!guess.isApprox(Eigen::Matrix4f::Identity (), 0.01f)) 
  { //If guess is not the Identity matrix we check it.
	  transformPointCloud (*input_, input_transformed, final_transformation_);
	  lowest_error = computeErrorMetric (input_transformed, static_cast<float> (corr_dist_threshold_));
	  i_iter = 1;
  }

  for (; i_iter < max_iterations_; ++i_iter)
  {
    // Draw nr_samples_ random samples
    selectSamples (*input_, nr_samples_, min_sample_distance_, sample_indices);

    // Find corresponding features in the target cloud
    findSimilarFeatures (*input_features_, sample_indices, corresponding_indices);

    // Estimate the transform from the samples to their corresponding points
    transformation_estimation_->estimateRigidTransformation (*input_, sample_indices, *target_, corresponding_indices, transformation_);

    // Tranform the data and compute the error
    transformPointCloud (*input_, input_transformed, transformation_);
    error = computeErrorMetric (input_transformed, static_cast<float> (corr_dist_threshold_));

    // If the new error is lower, update the final transformation
    if (i_iter == 0 || error < lowest_error)
    {
      lowest_error = error;
      final_transformation_ = transformation_;
    }
  }

  // Apply the final transformation
  transformPointCloud (*input_, output, final_transformation_);
}
Esempio n. 19
0
template <typename PointT, typename PointNT> bool
pcl::SmoothedSurfacesKeypoint<PointT, PointNT>::initCompute ()
{
    PCL_INFO ("SmoothedSurfacesKeypoint initCompute () called\n");
    if ( !Keypoint<PointT, PointT>::initCompute ())
    {
        PCL_ERROR ("[pcl::SmoothedSurfacesKeypoints::initCompute] Keypoint::initCompute failed\n");
        return false;
    }

    if (!normals_)
    {
        PCL_ERROR ("[pcl::SmoothedSurfacesKeypoints::initCompute] Input normals were not set\n");
        return false;
    }
    if (clouds_.size () == 0)
    {
        PCL_ERROR ("[pcl::SmoothedSurfacesKeypoints::initCompute] No other clouds were set apart from the input\n");
        return false;
    }

    if (input_->points.size () != normals_->points.size ())
    {
        PCL_ERROR ("[pcl::SmoothedSurfacesKeypoints::initCompute] The input cloud and the input normals differ in size\n");
        return false;
    }

    for (size_t cloud_i = 0; cloud_i < clouds_.size (); ++cloud_i)
    {
        if (clouds_[cloud_i]->points.size () != input_->points.size ())
        {
            PCL_ERROR ("[pcl::SmoothedSurfacesKeypoints::initCompute] Cloud %d does not have the same number of points as the input cloud\n", cloud_i);
            return false;
        }

        if (cloud_normals_[cloud_i]->points.size () != input_->points.size ())
        {
            PCL_ERROR ("[pcl::SmoothedSurfacesKeypoints::initCompute] Normals for cloud %d do not have the same number of points as the input cloud\n", cloud_i);
            return false;
        }
    }

    // Add the input cloud as the last index
    scales_.push_back (std::pair<float, size_t> (input_scale_, scales_.size ()));
    clouds_.push_back (input_);
    cloud_normals_.push_back (normals_);
    cloud_trees_.push_back (tree_);
    // Sort the clouds by their scales
    sort (scales_.begin (), scales_.end (), compareScalesFunction);

    // Find the index of the input after sorting
    for (size_t i = 0; i < scales_.size (); ++i)
        if (scales_[i].second == scales_.size () - 1)
        {
            input_index_ = i;
            break;
        }

    PCL_INFO ("Scales: ");
    for (size_t i = 0; i < scales_.size (); ++i) PCL_INFO ("(%d %f), ", scales_[i].second, scales_[i].first);
    PCL_INFO ("\n");

    return (true);
}
Esempio n. 20
0
bool
pcl::ProjectInliers<sensor_msgs::PointCloud2>::initSACModel (int model_type)
{
  // Convert the input data
  PointCloud<PointXYZ> cloud;
  fromROSMsg (*input_, cloud);
  PointCloud<PointXYZ>::Ptr cloud_ptr = cloud.makeShared ();

  // Build the model
  switch (model_type)
  {
    case SACMODEL_PLANE:
    {
      //PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PLANE\n", getClassName ().c_str ());
      sacmodel_.reset (new SampleConsensusModelPlane<pcl::PointXYZ> (cloud_ptr));
      break;
    }
    case SACMODEL_LINE:
    {
      //PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_LINE\n", getClassName ().c_str ());
      sacmodel_.reset (new SampleConsensusModelLine<pcl::PointXYZ> (cloud_ptr));
      break;
    }
    case SACMODEL_CIRCLE2D:
    {
      //PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_CIRCLE2D\n", getClassName ().c_str ());
      sacmodel_.reset (new SampleConsensusModelCircle2D<pcl::PointXYZ> (cloud_ptr));
      break;
    }
    case SACMODEL_SPHERE:
    {
      //PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_SPHERE\n", getClassName ().c_str ());
      sacmodel_.reset (new SampleConsensusModelSphere<pcl::PointXYZ> (cloud_ptr));
      break;
    }
    case SACMODEL_PARALLEL_LINE:
    {
      //PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PARALLEL_LINE\n", getClassName ().c_str ());
      sacmodel_.reset (new SampleConsensusModelParallelLine<pcl::PointXYZ> (cloud_ptr));
      break;
    }
    case SACMODEL_PERPENDICULAR_PLANE:
    {
      //PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PERPENDICULAR_PLANE\n", getClassName ().c_str ());
      sacmodel_.reset (new SampleConsensusModelPerpendicularPlane<pcl::PointXYZ> (cloud_ptr));
      break;
    }
    case SACMODEL_CYLINDER:
    {
      //PCL_DEBUG ("[pcl::%s::segment] Using a model of type: SACMODEL_CYLINDER\n", getClassName ().c_str ());
      sacmodel_.reset (new SampleConsensusModelCylinder<pcl::PointXYZ, Normal> (cloud_ptr));
      break;
    }
    case SACMODEL_NORMAL_PLANE:
    {
      //PCL_DEBUG ("[pcl::%s::segment] Using a model of type: SACMODEL_NORMAL_PLANE\n", getClassName ().c_str ());
      sacmodel_.reset (new SampleConsensusModelNormalPlane<pcl::PointXYZ, Normal> (cloud_ptr));
      break;
    }
    case SACMODEL_CONE:
    {
      //PCL_DEBUG ("[pcl::%s::segment] Using a model of type: SACMODEL_CONE\n", getClassName ().c_str ());
      sacmodel_.reset (new SampleConsensusModelCone<pcl::PointXYZ, Normal> (cloud_ptr));
      break;
    }
    case SACMODEL_NORMAL_SPHERE:
    {
      //PCL_DEBUG ("[pcl::%s::segment] Using a model of type: SACMODEL_NORMAL_SPHERE\n", getClassName ().c_str ());
      sacmodel_.reset (new SampleConsensusModelNormalSphere<pcl::PointXYZ, Normal> (cloud_ptr));
      break;
    }
    case SACMODEL_NORMAL_PARALLEL_PLANE:
    {
      //PCL_DEBUG ("[pcl::%s::segment] Using a model of type: SACMODEL_NORMAL_PARALLEL_PLANE\n", getClassName ().c_str ());
      sacmodel_.reset (new SampleConsensusModelNormalParallelPlane<pcl::PointXYZ, Normal> (cloud_ptr));
      break;
    }
    case SACMODEL_PARALLEL_PLANE:
    {
      //PCL_DEBUG ("[pcl::%s::segment] Using a model of type: SACMODEL_PARALLEL_PLANE\n", getClassName ().c_str ());
      sacmodel_.reset (new SampleConsensusModelParallelPlane<pcl::PointXYZ> (cloud_ptr));
      break;
    }
    default:
    {
      PCL_ERROR ("[pcl::%s::initSACModel] No valid model given!\n", getClassName ().c_str ());
      return (false);
    }
  }
  return (true);
}
Esempio n. 21
0
template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> bool
pcl::registration::FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::initCompute ()
{
  std::srand (static_cast <unsigned int> (std::time (NULL)));

  // basic pcl initialization
  if (!pcl::PCLBase <PointSource>::initCompute ())
    return (false);

  // check if source and target are given
  if (!input_ || !target_)
  {
    PCL_ERROR ("[%s::initCompute] Source or target dataset not given!\n", reg_name_.c_str ());
    return (false);
  }

  if (!target_indices_ || target_indices_->size () == 0)
  {
    target_indices_.reset (new std::vector <int> (static_cast <int> (target_->size ())));
    int index = 0;
    for (std::vector <int>::iterator it = target_indices_->begin (), it_e = target_indices_->end (); it != it_e; it++)
      *it = index++;
    target_cloud_updated_ = true;
  }

  // if a sample size for the point clouds is given; prefarably no sampling of target cloud
  if (nr_samples_ != 0)
  {
    const int ss = static_cast <int> (indices_->size ());
    const int sample_fraction_src = std::max (1, static_cast <int> (ss / nr_samples_));

    source_indices_ = pcl::IndicesPtr (new std::vector <int>);
    for (int i = 0; i < ss; i++)
    if (rand () % sample_fraction_src == 0)
      source_indices_->push_back ((*indices_) [i]);
  }
  else
    source_indices_ = indices_;

  // check usage of normals
  if (source_normals_ && target_normals_  && source_normals_->size () == input_->size () && target_normals_->size () == target_->size ())
    use_normals_ = true;

  // set up tree structures
  if (target_cloud_updated_)
  {
    tree_->setInputCloud (target_, target_indices_);
    target_cloud_updated_ = false;
  }

  // set predefined variables
  const int min_iterations = 4;
  const float diameter_fraction = 0.3f;

  // get diameter of input cloud (distance between farthest points)
  Eigen::Vector4f pt_min, pt_max;
  pcl::getMinMax3D (*target_, *target_indices_, pt_min, pt_max);
  diameter_ = (pt_max - pt_min).norm ();

  // derive the limits for the random base selection
  float max_base_diameter = diameter_* approx_overlap_ * 2.f;
  max_base_diameter_sqr_ = max_base_diameter * max_base_diameter;

  // normalize the delta
  if (normalize_delta_)
  {
    float mean_dist = getMeanPointDensity <PointTarget> (target_, *target_indices_, 0.05f * diameter_, nr_threads_);
    delta_ *= mean_dist;
  }

  // heuristic determination of number of trials to have high probabilty of finding a good solution
  if (max_iterations_ == 0)
  {
    float first_est = std::log (small_error_) / std::log (1.0 - std::pow ((double) approx_overlap_, (double) min_iterations));
    max_iterations_ = static_cast <int> (first_est / (diameter_fraction * approx_overlap_ * 2.f));
  }

  // set further parameter
  if (score_threshold_ == FLT_MAX)
    score_threshold_ = 1.f - approx_overlap_;

  if (max_iterations_ < 4)
    max_iterations_ = 4;

  if (max_runtime_ < 1)
    max_runtime_ = INT_MAX;

  // calculate internal parameters based on the the estimated point density
  max_pair_diff_ = delta_ * 2.f;
  max_edge_diff_ = delta_ * 4.f;
  coincidation_limit_ = delta_ * 2.f; // EDITED: originally std::sqrt (delta_ * 2.f)
  max_mse_ = powf (delta_* 2.f, 2.f);
  max_inlier_dist_sqr_ = powf (delta_ * 2.f, 2.f);

  // reset fitness_score
  fitness_score_ = FLT_MAX;

  return (true);
}
Esempio n. 22
0
void
pcl::ProjectInliers<sensor_msgs::PointCloud2>::applyFilter (PointCloud2 &output)
{
  if (indices_->empty ())
  {
    PCL_WARN ("[pcl::%s::applyFilter] No indices given or empty indices!\n", getClassName ().c_str ());
    output.width = output.height = 0;
    output.data.clear ();
    return;
  }

  //Eigen::Map<Eigen::VectorXf, Eigen::Aligned> model_coefficients (&model_->values[0], model_->values.size ());
  // More expensive than a map but safer (32bit architectures seem to complain)
  Eigen::VectorXf model_coefficients (model_->values.size ());
  for (size_t i = 0; i < model_->values.size (); ++i)
    model_coefficients[i] = model_->values[i];

  // Construct the model and project
  if (!initSACModel (model_type_))
  {
    PCL_ERROR ("[pcl::%s::segment] Error initializing the SAC model!\n", getClassName ().c_str ());
    output.width = output.height = 0;
    output.data.clear ();
    return;
  }
  pcl::PointCloud<pcl::PointXYZ> cloud_out;

  if (!copy_all_fields_)
    sacmodel_->projectPoints (*indices_, model_coefficients, cloud_out, false);
  else
    sacmodel_->projectPoints (*indices_, model_coefficients, cloud_out, true);

  if (copy_all_data_)
  {
    output.height       = input_->height;
    output.width        = input_->width;
    output.is_bigendian = input_->is_bigendian;
    output.point_step   = input_->point_step;
    output.row_step     = input_->row_step;
    output.data         = input_->data;
    output.is_dense     = input_->is_dense;

    // Get the distance field index
    int x_idx = -1, y_idx = -1, z_idx = -1;
    for (int d = 0; d < static_cast<int> (output.fields.size ()); ++d)
    {
      if (output.fields[d].name == "x") x_idx = d;
      if (output.fields[d].name == "y") y_idx = d;
      if (output.fields[d].name == "z") z_idx = d;
    }
    if (x_idx == -1 || y_idx == -1 || z_idx == -1)
    {
      PCL_ERROR ("[pcl::%s::segment] X (%d) Y (%d) Z (%d) field dimensions not found!\n", getClassName ().c_str (), x_idx, y_idx, z_idx);
      output.width = output.height = 0;
      output.data.clear ();
      return;
    }

    // Copy the projected points
    for (size_t i = 0; i < indices_->size (); ++i)
    {
      memcpy (&output.data[(*indices_)[i] * output.point_step + output.fields[x_idx].offset], &cloud_out.points[(*indices_)[i]].x, sizeof (float));
      memcpy (&output.data[(*indices_)[i] * output.point_step + output.fields[y_idx].offset], &cloud_out.points[(*indices_)[i]].y, sizeof (float));
      memcpy (&output.data[(*indices_)[i] * output.point_step + output.fields[z_idx].offset], &cloud_out.points[(*indices_)[i]].z, sizeof (float));
    }
  }
  else
  {
    if (!copy_all_fields_)
    {
      pcl::toROSMsg<pcl::PointXYZ> (cloud_out, output);
    }
    else
    {
      // Copy everything
      output.height       = 1;
      output.width        = static_cast<uint32_t> (indices_->size ());
      output.point_step   = input_->point_step;
      output.data.resize (output.width * output.point_step);
      output.is_bigendian = input_->is_bigendian;
      output.row_step     = output.point_step * output.width;
      // All projections should return valid data, so is_dense = true
      output.is_dense     = true;

      // Get the distance field index
      int x_idx = -1, y_idx = -1, z_idx = -1;
      for (int d = 0; d < static_cast<int> (output.fields.size ()); ++d)
      {
        if (output.fields[d].name == "x") x_idx = d;
        if (output.fields[d].name == "y") y_idx = d;
        if (output.fields[d].name == "z") z_idx = d;
      }

      if (x_idx == -1 || y_idx == -1 || z_idx == -1)
      {
        PCL_ERROR ("[pcl::%s::segment] X (%d) Y (%d) Z (%d) field dimensions not found!\n", getClassName ().c_str (), x_idx, y_idx, z_idx);
        output.width = output.height = 0;
        output.data.clear ();
        return;
      }

      // Copy the projected points
      for (size_t i = 0; i < indices_->size (); ++i)
      {
        memcpy (&output.data[i * output.point_step], &input_->data[(*indices_)[i] * input_->point_step], output.point_step);
        memcpy (&output.data[i * output.point_step + output.fields[x_idx].offset], &cloud_out.points[(*indices_)[i]].x, sizeof (float));
        memcpy (&output.data[i * output.point_step + output.fields[y_idx].offset], &cloud_out.points[(*indices_)[i]].y, sizeof (float));
        memcpy (&output.data[i * output.point_step + output.fields[z_idx].offset], &cloud_out.points[(*indices_)[i]].z, sizeof (float));
      }
    }
  }
}
Esempio n. 23
0
int
pcl::io::saveVTKFile (const std::string &file_name, 
                      const pcl::PolygonMesh &triangles, unsigned precision)
{
  if (triangles.cloud.data.empty ())
  {
    PCL_ERROR ("[pcl::io::saveVTKFile] Input point cloud has no data!\n");
    return (-1);
  }

  // Open file
  std::ofstream fs;
  fs.precision (precision);
  fs.open (file_name.c_str ());

  unsigned int nr_points  = triangles.cloud.width * triangles.cloud.height;
  unsigned int point_size = static_cast<unsigned int> (triangles.cloud.data.size () / nr_points);

  // Write the header information
  fs << "# vtk DataFile Version 3.0\nvtk output\nASCII\nDATASET POLYDATA\nPOINTS " << nr_points << " float" << std::endl;

  // Iterate through the points
  for (unsigned int i = 0; i < nr_points; ++i)
  {
    int xyz = 0;
    for (size_t d = 0; d < triangles.cloud.fields.size (); ++d)
    {
      int count = triangles.cloud.fields[d].count;
      if (count == 0)
        count = 1;          // we simply cannot tolerate 0 counts (coming from older converter code)
      int c = 0;
      if ((triangles.cloud.fields[d].datatype == sensor_msgs::PointField::FLOAT32) && (
           triangles.cloud.fields[d].name == "x" || 
           triangles.cloud.fields[d].name == "y" || 
           triangles.cloud.fields[d].name == "z"))
      {
        float value;
        memcpy (&value, &triangles.cloud.data[i * point_size + triangles.cloud.fields[d].offset + c * sizeof (float)], sizeof (float));
        fs << value;
        if (++xyz == 3)
          break;
      }
      fs << " ";
    }
    if (xyz != 3)
    {
      PCL_ERROR ("[pcl::io::saveVTKFile] Input point cloud has no XYZ data!\n");
      return (-2);
    }
    fs << std::endl;
  }

  // Write vertices
  fs << "\nVERTICES " << nr_points << " " << 2*nr_points << std::endl;
  for (unsigned int i = 0; i < nr_points; ++i)
    fs << "1 " << i << std::endl;

  // Write polygons
  // compute the correct number of values:
  size_t triangle_size = triangles.polygons.size ();
  size_t correct_number = triangle_size;
  for (size_t i = 0; i < triangle_size; ++i)
    correct_number += triangles.polygons[i].vertices.size ();
  fs << "\nPOLYGONS " << triangle_size << " " << correct_number << std::endl;
  for (size_t i = 0; i < triangle_size; ++i)
  {
    fs << triangles.polygons[i].vertices.size () << " ";
    size_t j = 0;
    for (j = 0; j < triangles.polygons[i].vertices.size () - 1; ++j)
      fs << triangles.polygons[i].vertices[j] << " ";
    fs << triangles.polygons[i].vertices[j] << std::endl;
  }

  // Write RGB values
  int field_index = getFieldIndex (triangles.cloud, "rgb");
  if (field_index != -1)
  {
    fs << "\nPOINT_DATA " << nr_points << "\nCOLOR_SCALARS scalars 3\n";
    for (unsigned int i = 0; i < nr_points; ++i)
    {
      int count = triangles.cloud.fields[field_index].count;
      if (count == 0)
        count = 1;          // we simply cannot tolerate 0 counts (coming from older converter code)
      int c = 0;
      if (triangles.cloud.fields[field_index].datatype == sensor_msgs::PointField::FLOAT32)
      {
        pcl::RGB color;
        memcpy (&color, &triangles.cloud.data[i * point_size + triangles.cloud.fields[field_index].offset + c * sizeof (float)], sizeof (RGB));
        int r = color.r;
        int g = color.g;
        int b = color.b;
        fs << static_cast<float> (r) / 255.0f << " " << static_cast<float> (g) / 255.0f << " " << static_cast<float> (b) / 255.0f;
      }
      fs << std::endl;
    }
  }

  // Close file
  fs.close ();
  return (0);
}
Esempio n. 24
0
template <typename PointFeature> bool
pcl::PyramidFeatureHistogram<PointFeature>::initializeHistogram ()
{
  // a few consistency checks before starting the computations
  if (!PCLBase<PointFeature>::initCompute ())
  {
    PCL_ERROR ("[pcl::PyramidFeatureHistogram::initializeHistogram] PCLBase initCompute failed\n");
    return false;
  }

  if (dimension_range_input_.size () == 0)
  {
    PCL_ERROR ("[pcl::PyramidFeatureHistogram::initializeHistogram] Input dimension range was not set\n");
    return false;
  }

  if (dimension_range_target_.size () == 0)
  {
    PCL_ERROR ("[pcl::PyramidFeatureHistogram::initializeHistogram] Target dimension range was not set\n");
    return false;
  }

  if (dimension_range_input_.size () != dimension_range_target_.size ())
  {
    PCL_ERROR ("[pcl::PyramidFeatureHistogram::initializeHistogram] Input and target dimension ranges do not agree in size: %u vs %u\n",
               dimension_range_input_.size (), dimension_range_target_.size ());
    return false;
  }


  nr_dimensions = dimension_range_target_.size ();
  nr_features = input_->points.size ();
  float D = 0.0f;
  for (std::vector<std::pair<float, float> >::iterator range_it = dimension_range_target_.begin (); range_it != dimension_range_target_.end (); ++range_it)
  {
    float aux = range_it->first - range_it->second;
    D += aux * aux;
  }
  D = sqrt (D);
  nr_levels = ceil (Log2 (D));
  PCL_DEBUG ("[pcl::PyramidFeatureHistogram::initializeHistogram] Pyramid will have %u levels with a hyper-parallelepiped diagonal size of %f\n", nr_levels, D);


  hist_levels.resize (nr_levels);
  for (size_t level_i = 0; level_i < nr_levels; ++level_i)
  {
    std::vector<size_t> bins_per_dimension (nr_dimensions);
    std::vector<float> bin_step (nr_dimensions);
    for (size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i) {
      bins_per_dimension[dim_i] = ceil ( (dimension_range_target_[dim_i].second - dimension_range_target_[dim_i].first) / (pow (2.0f, (int) level_i) * sqrt ((float) nr_dimensions)));
      bin_step[dim_i] = pow (2.0f, (int) level_i) * sqrt ((float) nr_dimensions);
    }
    hist_levels[level_i] = PyramidFeatureHistogramLevel (bins_per_dimension, bin_step);

    PCL_DEBUG ("[pcl::PyramidFeatureHistogram::initializeHistogram] Created vector of size %u at level %u\nwith #bins per dimension:", hist_levels.back ().hist.size (), level_i);
    for (size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i)
      PCL_DEBUG ("%u ", bins_per_dimension[dim_i]);
    PCL_DEBUG ("\n");
  }

  return true;
}
int main (int argc, char** argv)
{
    std::string fileName = argv[1];
    std::cout << "Reading " << fileName << std::endl;

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

    if (pcl::io::loadPCDFile<pcl::PointXYZ> (fileName, *cloud) == -1) //* load the file
    {
        PCL_ERROR ("Couldn't read file");
        return (-1);
    }

    std::cout << "Loaded " << cloud->points.size() << " points." << std::endl;

    // Compute the normals
    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normalEstimation;
    normalEstimation.setInputCloud (cloud);

    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
    normalEstimation.setSearchMethod (tree);

    pcl::PointCloud<pcl::Normal>::Ptr cloudWithNormals (new pcl::PointCloud<pcl::Normal>);

    normalEstimation.setRadiusSearch (0.03);

    normalEstimation.compute (*cloudWithNormals);

    std::cout << "Computed " << cloudWithNormals->points.size() << " normals." << std::endl;

    // Setup the feature computation
    typedef pcl::VFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::VFHSignature308> VFHEstimationType;
    VFHEstimationType vfhEstimation;

    // Provide the original point cloud (without normals)
    vfhEstimation.setInputCloud (cloud);

    // Provide the point cloud with normals
    vfhEstimation.setInputNormals(cloudWithNormals);

    // Use the same KdTree from the normal estimation
    vfhEstimation.setSearchMethod (tree);

    //vfhEstimation.setRadiusSearch (0.2); // With this, error: "Both radius (.2) and K (1) defined! Set one of them to zero first and then re-run compute()"

    // Actually compute the VFH features
    pcl::PointCloud<pcl::VFHSignature308>::Ptr vfhFeatures(new pcl::PointCloud<pcl::VFHSignature308>);
    vfhEstimation.compute (*vfhFeatures);

    std::cout << "output points.size (): " << vfhFeatures->points.size () << std::endl; // This outputs 1 - should be 397!

    // Display and retrieve the shape context descriptor vector for the 0th point.
    pcl::VFHSignature308 descriptor = vfhFeatures->points[0];
    VFHEstimationType::PointCloudOut::PointType descriptor2 = vfhFeatures->points[0];
    std::cout << "VFH:" << descriptor << std::endl;
    std::cout << "Numero de Elementos del VFH = " << sizeof(descriptor.histogram)/sizeof(descriptor.histogram[0]) << std::endl;


    // Create *_vfh.pcd file
    std::stringstream vfh_file;
    vfh_file << "# .PCD v.6 - Point Cloud Data file format" << std::endl;
    vfh_file << "FIELDS vfh" << std::endl;
    vfh_file << "SIZE 4" << std::endl;
    vfh_file << "TYPE F" << std::endl;
    vfh_file << "COUNT 308" << std::endl;
    vfh_file << "WIDTH 1" << std::endl;
    vfh_file << "HEIGHT 1" << std::endl;
    vfh_file << "POINTS 1" << std::endl;
    vfh_file << "DATA ascii" << std::endl;
    int vfh_length = sizeof(descriptor.histogram)/sizeof(descriptor.histogram[0]);
    for (int i = 0; i < vfh_length; i++)
    {
        vfh_file << descriptor.histogram[i] << " ";
    }

    std::ofstream outFile;
    outFile.open("Prueba_vfh.pcd");
    outFile << vfh_file.str();
    outFile.close();

    return 0;
}
Esempio n. 26
0
template <typename PointFeature> float
pcl::PyramidFeatureHistogram<PointFeature>::comparePyramidFeatureHistograms (const PyramidFeatureHistogramPtr &pyramid_a,
                                                                             const PyramidFeatureHistogramPtr &pyramid_b)
{
  // do a few consistency checks before and during the computation
  if (pyramid_a->nr_dimensions != pyramid_b->nr_dimensions)
  {
    PCL_ERROR ("[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] The two given pyramids have different numbers of dimensions: %u vs %u\n", pyramid_a->nr_dimensions, pyramid_b->nr_dimensions);
    return -1;
  }
  if (pyramid_a->nr_levels != pyramid_b->nr_levels)
  {
    PCL_ERROR ("[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] The two given pyramids have different numbers of levels: %u vs %u\n", pyramid_a->nr_levels, pyramid_b->nr_levels);
    return -1;
  }


  // calculate for level 0 first
  if (pyramid_a->hist_levels[0].hist.size () != pyramid_b->hist_levels[0].hist.size ())
  {
    PCL_ERROR ("[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] The two given pyramids have different numbers of bins on level 0: %u vs %u\n", pyramid_a->hist_levels[0].hist.size (), pyramid_b->hist_levels[0].hist.size ());
    return -1;
  }
  float match_count_level = 0.0f, match_count_prev_level = 0.0f;
  for (size_t bin_i = 0; bin_i < pyramid_a->hist_levels[0].hist.size (); ++bin_i)
  {
    if (pyramid_a->hist_levels[0].hist[bin_i] < pyramid_b->hist_levels[0].hist[bin_i])
      match_count_level += pyramid_a->hist_levels[0].hist[bin_i];
    else
      match_count_level += pyramid_b->hist_levels[0].hist[bin_i];
  }


  float match_count = match_count_level;
  for (size_t level_i = 1; level_i < pyramid_a->nr_levels; ++level_i)
  {
    if (pyramid_a->hist_levels[level_i].hist.size () != pyramid_b->hist_levels[level_i].hist.size ())
    {
      PCL_ERROR ("[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] The two given pyramids have different numbers of bins on level %u: %u vs %u\n", level_i, pyramid_a->hist_levels[level_i].hist.size (), pyramid_b->hist_levels[level_i].hist.size ());
      return -1;
    }

    match_count_prev_level = match_count_level;
    match_count_level = 0.0f;
    for (size_t bin_i = 0; bin_i < pyramid_a->hist_levels[level_i].hist.size (); ++bin_i)
    {
      if (pyramid_a->hist_levels[level_i].hist[bin_i] < pyramid_b->hist_levels[level_i].hist[bin_i])
        match_count_level += pyramid_a->hist_levels[level_i].hist[bin_i];
      else
        match_count_level += pyramid_b->hist_levels[level_i].hist[bin_i];
    }

    float level_normalization_factor = pow(2.0f, (int) level_i);
    match_count += (match_count_level - match_count_prev_level) / level_normalization_factor;
  }


  // include self-similarity factors
  float self_similarity_a = pyramid_a->nr_features,
      self_similarity_b = pyramid_b->nr_features;
  PCL_DEBUG ("[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] Self similarity measures: %f, %f\n", self_similarity_a, self_similarity_b);
  match_count /= sqrt (self_similarity_a * self_similarity_b);

  return match_count;
}
Esempio n. 27
0
template <typename PointInT, typename PointOutT, typename PointRFT> bool
pcl::UniqueShapeContext<PointInT, PointOutT, PointRFT>::initCompute ()
{
  if (!Feature<PointInT, PointOutT>::initCompute ())
  {
    PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
    return (false);
  }

  // Default LRF estimation alg: SHOTLocalReferenceFrameEstimation
  typename SHOTLocalReferenceFrameEstimation<PointInT, PointRFT>::Ptr lrf_estimator(new SHOTLocalReferenceFrameEstimation<PointInT, PointRFT>());
  lrf_estimator->setRadiusSearch (local_radius_);
  lrf_estimator->setInputCloud (input_);
  lrf_estimator->setIndices (indices_);
  if (!fake_surface_)
    lrf_estimator->setSearchSurface(surface_);

  if (!FeatureWithLocalReferenceFrames<PointInT, PointRFT>::initLocalReferenceFrames (indices_->size (), lrf_estimator))
  {
    PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
    return (false);
  }

  if (search_radius_< min_radius_)
  {
    PCL_ERROR ("[pcl::%s::initCompute] search_radius_ must be GREATER than min_radius_.\n", getClassName ().c_str ());
    return (false);
  }

  // Update descriptor length
  descriptor_length_ = elevation_bins_ * azimuth_bins_ * radius_bins_;

  // Compute radial, elevation and azimuth divisions
  float azimuth_interval = 360.0f / static_cast<float> (azimuth_bins_);
  float elevation_interval = 180.0f / static_cast<float> (elevation_bins_);

  // Reallocate divisions and volume lut
  radii_interval_.clear ();
  phi_divisions_.clear ();
  theta_divisions_.clear ();
  volume_lut_.clear ();

  // Fills radii interval based on formula (1) in section 2.1 of Frome's paper
  radii_interval_.resize (radius_bins_ + 1);
  for (size_t j = 0; j < radius_bins_ + 1; j++)
    radii_interval_[j] = static_cast<float> (exp (log (min_radius_) + ((static_cast<float> (j) / static_cast<float> (radius_bins_)) * log (search_radius_/min_radius_))));

  // Fill theta didvisions of elevation
  theta_divisions_.resize (elevation_bins_+1);
  for (size_t k = 0; k < elevation_bins_+1; k++)
    theta_divisions_[k] = static_cast<float> (k) * elevation_interval;

  // Fill phi didvisions of elevation
  phi_divisions_.resize (azimuth_bins_+1);
  for (size_t l = 0; l < azimuth_bins_+1; l++)
    phi_divisions_[l] = static_cast<float> (l) * azimuth_interval;

  // LookUp Table that contains the volume of all the bins
  // "phi" term of the volume integral
  // "integr_phi" has always the same value so we compute it only one time
  float integr_phi  = pcl::deg2rad (phi_divisions_[1]) - pcl::deg2rad (phi_divisions_[0]);
  // exponential to compute the cube root using pow
  float e = 1.0f / 3.0f;
  // Resize volume look up table
  volume_lut_.resize (radius_bins_ * elevation_bins_ * azimuth_bins_);
  // Fill volumes look up table
  for (size_t j = 0; j < radius_bins_; j++)
  {
    // "r" term of the volume integral
    float integr_r = (radii_interval_[j+1]*radii_interval_[j+1]*radii_interval_[j+1] / 3) - (radii_interval_[j]*radii_interval_[j]*radii_interval_[j]/ 3);

    for (size_t k = 0; k < elevation_bins_; k++)
    {
      // "theta" term of the volume integral
      float integr_theta = cosf (deg2rad (theta_divisions_[k])) - cosf (deg2rad (theta_divisions_[k+1]));
      // Volume
      float V = integr_phi * integr_theta * integr_r;
      // Compute cube root of the computed volume commented for performance but left
      // here for clarity
      // float cbrt = pow(V, e);
      // cbrt = 1 / cbrt;

      for (size_t l = 0; l < azimuth_bins_; l++)
        // Store in lut 1/cbrt
        //volume_lut_[ (l*elevation_bins_*radius_bins_) + k*radius_bins_ + j ] = cbrt;
        volume_lut_[(l*elevation_bins_*radius_bins_) + k*radius_bins_ + j] = 1.0f / powf (V, e);
    }
  }
  return (true);
}
void PcdGtAnnotator<PointT>::annotate (const std::string &scenes_dir, const std::string &scene_id)
{
    std::string scene_full_path = scenes_dir + "/" + scene_id;

    std::vector < std::string > scene_view;
    if (v4r::io::getFilesInDirectory( scene_full_path, scene_view, "", ".*.pcd", true) != -1)
    {
        std::cout << "Number of viewpoints in directory is:" << scene_view.size () << std::endl;

        std::vector < std::string > gt_file;
        std::string annotations_dir = gt_dir_ + "/" + scene_id;

        if( v4r::io::getFilesInDirectory( annotations_dir, gt_file, "", ".*.txt", true) == -1)
        {
            std::cerr << "Could not find any annotations in " << annotations_dir << ". " << std::endl;
        }
        std::sort(gt_file.begin(), gt_file.end());

        std::sort(scene_view.begin(), scene_view.end());
        for (size_t s_id = 0; s_id < scene_view.size (); s_id++)
        {
            if (first_view_only_ && s_id > 0)
                    continue;

            std::vector<std::string> strs;
            boost::split (strs, scene_view[s_id], boost::is_any_of ("."));
            std::string scene_file_wo_ext = strs[0] + "_";
            std::string gt_occ_check = scene_file_wo_ext + "occlusion_";

            std::string scene_full_file_path = scene_full_path + "/" + scene_view[s_id];
            pcl::io::loadPCDFile(scene_full_file_path, *reconstructed_scene_);
            View view;
            pcl::copyPointCloud(*reconstructed_scene_, *view.cloud_);
            view.cloud_->sensor_origin_ = reconstructed_scene_->sensor_origin_;
            view.cloud_->sensor_orientation_ = reconstructed_scene_->sensor_orientation_;

            for(size_t gt_id=0; gt_id < gt_file.size(); gt_id++)
            {
                const std::string gt_fn = gt_file[gt_id];
                if( gt_fn.compare(0, scene_file_wo_ext.size(), scene_file_wo_ext) == 0 &&
                        gt_fn.compare(0, gt_occ_check.size(), gt_occ_check))
                {
                    std::cout << gt_fn << std::endl;
                    std::string model_instance = gt_fn.substr(scene_file_wo_ext.size());
                    std::vector<std::string> str_split;
                    boost::split (str_split, model_instance, boost::is_any_of ("."));
                    model_instance = str_split[0];

                    size_t found = model_instance.find_last_of("_");
                    std::string times_text ("times.txt");
                    if ( model_instance.compare(times_text) == 0 )
                    {
                        std::cout << "skipping this one" << std::endl;
                        continue;
                    }
                    std::string model_name = model_instance.substr(0,found) + ".pcd";
                    std::cout << "Model: " << model_name << std::endl;
                    ModelTPtr pModel;
                    source_->getModelById(model_name, pModel);

                    std::string gt_full_file_path = annotations_dir + "/" + gt_fn;
                    Eigen::Matrix4f transform = v4r::io::readMatrixFromFile(gt_full_file_path);

                    typename pcl::PointCloud<PointT>::ConstPtr model_cloud = pModel->getAssembled(0.003f);
                    typename pcl::PointCloud<PointT>::Ptr model_aligned(new pcl::PointCloud<PointT>());
                    pcl::transformPointCloud(*model_cloud, *model_aligned, transform);

                    bool model_exists = false;
                    size_t current_model_id = 0;
                    for (size_t m_id=0; m_id < model_id_.size(); m_id++)
                    {
                        if ( model_id_[m_id].compare( model_instance ) == 0)
                        {
                            model_exists = true;
                            current_model_id = m_id;
                            transform_to_scene_[m_id] = transform;
                            break;
                        }
                    }
                    if ( !model_exists )
                    {
                        std::vector<bool> visible_model_pts_tmp (model_aligned->points.size(), false);
                        visible_model_points_.push_back(visible_model_pts_tmp);
                        model_id_.push_back( model_instance );
                        transform_to_scene_.push_back(transform);
                        std::vector<bool> obj_mask (reconstructed_scene_->points.size(), false);
                        pixel_annotated_obj_in_first_view_.push_back ( obj_mask );
                        current_model_id = model_id_.size()-1;
                    }


                    const float cx = (static_cast<float> (reconstructed_scene_->width) / 2.f - 0.5f);
                    const float cy = (static_cast<float> (reconstructed_scene_->height) / 2.f - 0.5f);

                    for(size_t m_pt_id=0; m_pt_id < model_aligned->points.size(); m_pt_id++)
                    {
                        PointT mp = model_aligned->points[m_pt_id];
                        const int u = static_cast<int> (f_ * mp.x / mp.z + cx);
                        const int v = static_cast<int> (f_ * mp.y / mp.z + cy);

                        if(u<0 || u >= (int)reconstructed_scene_->width || v<0 || v >= (int)reconstructed_scene_->height) // model point outside field of view
                            continue;

                        PointT sp = reconstructed_scene_->at(u,v);

                        if ( !pcl::isFinite(sp) )   // Model point is not visible from the view point (shiny spot, noise,...)
                            continue;

                        if( std::abs(mp.z - sp.z) < threshold_ )
                        {
                            visible_model_points_[current_model_id][m_pt_id] = true;
                            if( s_id == 0 )
                                pixel_annotated_obj_in_first_view_[current_model_id][v*reconstructed_scene_->width + u] = true;
                        }
                    }
                }
            }
            std::cout << std::endl;

//            if(s_id==0)
//            {
//                pcl::visualization::PCLVisualizer vis("obj_mask");
//                vis.addPointCloud(pScenePCl_, "cloud");
//                for (size_t m_id=0; m_id < model_id_.size(); m_id++)
//                {
//                    typename pcl::PointCloud<PointT>::Ptr highlight (new pcl::PointCloud<PointT>());
//                    pcl::copyPointCloud(*pScenePCl_, pixel_annotated_obj_in_first_view_[m_id], *highlight);

//                    const float r=50+rand()%205;
//                    const float g=50+rand()%205;
//                    const float b=50+rand()%205;

//                    std::stringstream model_name;
//                    model_name << m_id;
//                    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> color (highlight, r, g, b);
//                    vis.addPointCloud(highlight, color, model_name.str());
//                    vis.addText(model_name.str(), 40, 15*m_id, 15, r/255, g/255, b/255,model_name.str());
//                }
//                vis.spin();
//            }

            view.transform_to_scene_ = transform_to_scene_;
            views_.push_back(view);
        }
//        visualize();
    }
    else
    {
        PCL_ERROR("You should pass a directory\n");
        return;
    }
}
Esempio n. 29
0
template <typename PointT, typename PointNT> bool
pcl::SampleConsensusModelCylinder<PointT, PointNT>::computeModelCoefficients (
      const std::vector<int> &samples, Eigen::VectorXf &model_coefficients)
{
  // Need 2 samples
  if (samples.size () != 2)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelCylinder::computeModelCoefficients] Invalid set of samples given (%zu)!\n", samples.size ());
    return (false);
  }

  if (!normals_)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelCylinder::computeModelCoefficients] No input dataset containing normals was given!\n");
    return (false);
  }

  if (fabs (input_->points[samples[0]].x - input_->points[samples[1]].x) <= std::numeric_limits<float>::epsilon () && 
      fabs (input_->points[samples[0]].y - input_->points[samples[1]].y) <= std::numeric_limits<float>::epsilon () && 
      fabs (input_->points[samples[0]].z - input_->points[samples[1]].z) <= std::numeric_limits<float>::epsilon ()) 
  {
    return (false);
  }
  
  Eigen::Vector4f p1 (input_->points[samples[0]].x, input_->points[samples[0]].y, input_->points[samples[0]].z, 0);
  Eigen::Vector4f p2 (input_->points[samples[1]].x, input_->points[samples[1]].y, input_->points[samples[1]].z, 0);

  Eigen::Vector4f n1 (normals_->points[samples[0]].normal[0], normals_->points[samples[0]].normal[1], normals_->points[samples[0]].normal[2], 0);
  Eigen::Vector4f n2 (normals_->points[samples[1]].normal[0], normals_->points[samples[1]].normal[1], normals_->points[samples[1]].normal[2], 0);
  Eigen::Vector4f w = n1 + p1 - p2;

  float a = n1.dot (n1);
  float b = n1.dot (n2);
  float c = n2.dot (n2);
  float d = n1.dot (w);
  float e = n2.dot (w);
  float denominator = a*c - b*b;
  float sc, tc;
  // Compute the line parameters of the two closest points
  if (denominator < 1e-8)          // The lines are almost parallel
  {
    sc = 0.0f;
    tc = (b > c ? d / b : e / c);  // Use the largest denominator
  }
  else
  {
    sc = (b*e - c*d) / denominator;
    tc = (a*e - b*d) / denominator;
  }

  // point_on_axis, axis_direction
  Eigen::Vector4f line_pt  = p1 + n1 + sc * n1;
  Eigen::Vector4f line_dir = p2 + tc * n2 - line_pt;
  line_dir.normalize ();

  model_coefficients.resize (7);
  // model_coefficients.template head<3> ()    = line_pt.template head<3> ();
  model_coefficients[0] = line_pt[0];
  model_coefficients[1] = line_pt[1];
  model_coefficients[2] = line_pt[2];
  // model_coefficients.template segment<3> (3) = line_dir.template head<3> ();
  model_coefficients[3] = line_dir[0];
  model_coefficients[4] = line_dir[1];
  model_coefficients[5] = line_dir[2];
  // cylinder radius
  model_coefficients[6] = static_cast<float> (sqrt (pcl::sqrPointToLineDistance (p1, line_pt, line_dir)));

  if (model_coefficients[6] > radius_max_ || model_coefficients[6] < radius_min_)
    return (false);

  return (true);
}
Esempio n. 30
0
File: io.cpp Progetto: tfili/pcl
bool
pcl::concatenatePointCloud (const pcl::PCLPointCloud2 &cloud1,
                            const pcl::PCLPointCloud2 &cloud2,
                            pcl::PCLPointCloud2 &cloud_out)
{
  //if one input cloud has no points, but the other input does, just return the cloud with points
  if (cloud1.width*cloud1.height == 0 && cloud2.width*cloud2.height > 0)
  {
    cloud_out = cloud2;
    return (true);
  }
  else if (cloud1.width*cloud1.height > 0 && cloud2.width*cloud2.height == 0)
  {
    cloud_out = cloud1;
    return (true);
  }

  bool strip = false;
  for (size_t i = 0; i < cloud1.fields.size (); ++i)
    if (cloud1.fields[i].name == "_")
      strip = true;

  for (size_t i = 0; i < cloud2.fields.size (); ++i)
    if (cloud2.fields[i].name == "_")
      strip = true;

  if (!strip && cloud1.fields.size () != cloud2.fields.size ())
  {
    PCL_ERROR ("[pcl::concatenatePointCloud] Number of fields in cloud1 (%u) != Number of fields in cloud2 (%u)\n", cloud1.fields.size (), cloud2.fields.size ());
    return (false);
  }
  
  // Copy cloud1 into cloud_out
  cloud_out = cloud1;
  size_t nrpts = cloud_out.data.size ();
  // Height = 1 => no more organized
  cloud_out.width    = cloud1.width * cloud1.height + cloud2.width * cloud2.height;
  cloud_out.height   = 1;
  if (!cloud1.is_dense || !cloud2.is_dense)
    cloud_out.is_dense = false;
  else
    cloud_out.is_dense = true;

  // We need to strip the extra padding fields
  if (strip)
  {
    // Get the field sizes for the second cloud
    std::vector<pcl::PCLPointField> fields2;
    std::vector<int> fields2_sizes;
    for (size_t j = 0; j < cloud2.fields.size (); ++j)
    {
      if (cloud2.fields[j].name == "_")
        continue;

      fields2_sizes.push_back (cloud2.fields[j].count * 
                               pcl::getFieldSize (cloud2.fields[j].datatype));
      fields2.push_back (cloud2.fields[j]);
    }

    cloud_out.data.resize (nrpts + (cloud2.width * cloud2.height) * cloud_out.point_step);

    // Copy the second cloud
    for (size_t cp = 0; cp < cloud2.width * cloud2.height; ++cp)
    {
      int i = 0;
      for (size_t j = 0; j < fields2.size (); ++j)
      {
        if (cloud1.fields[i].name == "_")
        {
          ++i;
          continue;
        }

        // We're fine with the special RGB vs RGBA use case
        if ((cloud1.fields[i].name == "rgb" && fields2[j].name == "rgba") ||
            (cloud1.fields[i].name == "rgba" && fields2[j].name == "rgb") ||
            (cloud1.fields[i].name == fields2[j].name))
        {
          memcpy (reinterpret_cast<char*> (&cloud_out.data[nrpts + cp * cloud1.point_step + cloud1.fields[i].offset]), 
                  reinterpret_cast<const char*> (&cloud2.data[cp * cloud2.point_step + cloud2.fields[j].offset]), 
                  fields2_sizes[j]);
          ++i;  // increment the field size i
        }
      }
    }
  }
  else
  {
    for (size_t i = 0; i < cloud1.fields.size (); ++i)
    {
      // We're fine with the special RGB vs RGBA use case
      if ((cloud1.fields[i].name == "rgb" && cloud2.fields[i].name == "rgba") ||
          (cloud1.fields[i].name == "rgba" && cloud2.fields[i].name == "rgb"))
        continue;
      // Otherwise we need to make sure the names are the same
      if (cloud1.fields[i].name != cloud2.fields[i].name)
      {
        PCL_ERROR ("[pcl::concatenatePointCloud] Name of field %d in cloud1, %s, does not match name in cloud2, %s\n", i, cloud1.fields[i].name.c_str (), cloud2.fields[i].name.c_str ());      
        return (false);
      }
    }
    
    cloud_out.data.resize (nrpts + cloud2.data.size ());
    memcpy (&cloud_out.data[nrpts], &cloud2.data[0], cloud2.data.size ());
  }
  return (true);
}