Esempio n. 1
0
template <typename PointT> bool
pcl::PackedRGBComparison<PointT>::evaluate (const PointT &point) const
{
  // extract the component value
  uint8_t* pt_data = (uint8_t*)&point;
  uint8_t my_val = *(pt_data + component_offset_);

  // now do the comparison
  switch (this->op_) 
  {
    case pcl::ComparisonOps::GT :
      return (my_val > this->compare_val_);
    case pcl::ComparisonOps::GE :
      return (my_val >= this->compare_val_);
    case pcl::ComparisonOps::LT :
      return (my_val < this->compare_val_);
    case pcl::ComparisonOps::LE :
      return (my_val <= this->compare_val_);
    case pcl::ComparisonOps::EQ :
      return (my_val == this->compare_val_);
    default:
      PCL_WARN ("[pcl::PackedRGBComparison::evaluate] unrecognized op_!\n");
      return (false);
  }
}
Esempio n. 2
0
template <typename GeneratorT> int
pcl::common::CloudGenerator<pcl::PointXY, GeneratorT>::fill (int width, int height, pcl::PointCloud<pcl::PointXY>& cloud)
{
  if (width < 1)
  {
    PCL_ERROR ("[pcl::common::CloudGenerator] Cloud width must be >= 1\n!");
    return (-1);
  }
  
  if (height < 1)
  {
    PCL_ERROR ("[pcl::common::CloudGenerator] Cloud height must be >= 1\n!");
    return (-1);
  }
  
  if (!cloud.empty ())
    PCL_WARN ("[pcl::common::CloudGenerator] Cloud data will be erased with new data\n!");

  cloud.width = width;
  cloud.height = height;
  cloud.resize (cloud.width * cloud.height);
  cloud.is_dense = true;

  for (pcl::PointCloud<pcl::PointXY>::iterator points_it = cloud.begin ();
       points_it != cloud.end ();
       ++points_it)
  {
    points_it->x = x_generator_.run ();
    points_it->y = y_generator_.run ();
  }
  return (0);
}
Esempio n. 3
0
pcl::FeatureHistogram::FeatureHistogram (size_t const number_of_bins,
    const float min, const float max) : 
    histogram_ (number_of_bins, 0)
{
  // Initialize thresholds.
  if (min < max)
  {
    threshold_min_ = min;
    threshold_max_ = max;
    step_ = (max - min) / static_cast<float> (number_of_bins_);
  }
  else
  {
    threshold_min_ = 0.0f;
    threshold_max_ = static_cast<float> (number_of_bins);
    step_ = 1.0f;
    PCL_WARN ("[FeatureHistogram::setThresholds] Variable \"max\" must be greater then \"min\".\n");
  }

  // Initialize sum.
  number_of_elements_ = 0;

  // Initialize size;
  number_of_bins_ = number_of_bins;
}
void
SACNormalsPlaneExtractor<PointT>::compute()
{
    CHECK ( cloud_  ) << "Input cloud is not organized!";

    all_planes_.clear();

    // ---[ PassThroughFilter
    typename pcl::PointCloud<PointT>::Ptr cloud_filtered (new pcl::PointCloud<PointT> ());
    pcl::PassThrough<PointT> pass;
    pass.setFilterLimits (0, max_z_bounds_);
    pass.setFilterFieldName ("z");
    pass.setInputCloud (cloud_);
    pass.filter (*cloud_filtered);

    if ( cloud_filtered->points.size () < k_)
    {
      PCL_WARN ("[DominantPlaneSegmentation] Filtering returned %lu points! Aborting.",
          cloud_filtered->points.size ());
      return;
    }

    // Downsample the point cloud
    typename pcl::PointCloud<PointT>::Ptr cloud_downsampled (new pcl::PointCloud<PointT> ());
    pcl::VoxelGrid<PointT> grid;
    grid.setLeafSize (downsample_leaf_, downsample_leaf_, downsample_leaf_);
    grid.setDownsampleAllData (false);
    grid.setInputCloud (cloud_filtered);
    grid.filter (*cloud_downsampled);

    // ---[ Estimate the point normals
    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal> ());
    pcl::NormalEstimation<PointT, pcl::Normal> n3d;
    typename pcl::search::KdTree<PointT>::Ptr normals_tree_ (new pcl::search::KdTree<PointT>);
    n3d.setKSearch ( (int) k_);
    n3d.setSearchMethod (normals_tree_);
    n3d.setInputCloud (cloud_downsampled);
    n3d.compute (*cloud_normals);

    // ---[ Perform segmentation
    pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg;
    seg.setDistanceThreshold (sac_distance_threshold_);
    seg.setMaxIterations (2000);
    seg.setNormalDistanceWeight (0.1);
    seg.setOptimizeCoefficients (true);
    seg.setModelType (pcl::SACMODEL_NORMAL_PLANE);
    seg.setMethodType (pcl::SAC_RANSAC);
    seg.setProbability (0.99);
    seg.setInputCloud (cloud_downsampled);
    seg.setInputNormals (cloud_normals);
    pcl::PointIndices table_inliers;
    pcl::ModelCoefficients coefficients;
    seg.segment ( table_inliers, coefficients);

    Eigen::Vector4f plane = Eigen::Vector4f(coefficients.values[0], coefficients.values[1],
                    coefficients.values[2], coefficients.values[3]);

    all_planes_.resize(1);
    all_planes_[0] = plane;
}
Esempio n. 5
0
      void
      ScreenshotManager::saveImage(const Eigen::Affine3f &camPose, pcl::gpu::PtrStepSz<const PixelRGB> rgb24)
      {

        PCL_WARN ("[o] [o] [o] [o] Saving screenshot [o] [o] [o] [o]\n");

        std::string file_extension_image = ".png";
        std::string file_extension_pose = ".txt";
        std::string filename_image = "KinFuSnapshots/";
        std::string filename_pose = "KinFuSnapshots/";

        // Get Pose
        Eigen::Matrix<float, 3, 3, Eigen::RowMajor> erreMats = camPose.linear ();
                    Eigen::Vector3f teVecs = camPose.translation ();

                    // Create filenames
                    filename_pose += std::to_string(screenshot_counter) + file_extension_pose;
                    filename_image += std::to_string(screenshot_counter) + file_extension_image;

                    // Write files
                    writePose (filename_pose, teVecs, erreMats);
          
        // Save Image
        pcl::io::saveRgbPNGFile (filename_image, (unsigned char*)rgb24.data, 640,480);
          
        screenshot_counter++;
      }
Esempio n. 6
0
/** @brief Asks the user to press enter to continue
 * @param[in] str Message to display */
void
waitForUser (std::string str = "Press enter to continue")
{
  PCL_WARN (str.c_str ());
  std::cout.flush ();
  getc (stdin);
}
Esempio n. 7
0
template <typename PointT> bool
pcl::visualization::PCLHistogramVisualizer::updateFeatureHistogram (
    const pcl::PointCloud<PointT> &cloud, int hsize, 
    const std::string &id)
{
  RenWinInteractMap::iterator am_it = wins_.find (id);
  if (am_it == wins_.end ())
  {
    PCL_WARN ("[updateFeatureHistogram] A window with id <%s> does not exists!.\n", id.c_str ());
    return (false);
  }
  RenWinInteract* renwinupd = &wins_[id];
  
  vtkSmartPointer<vtkDoubleArray> xy_array = vtkSmartPointer<vtkDoubleArray>::New ();
  xy_array->SetNumberOfComponents (2);
  xy_array->SetNumberOfTuples (hsize);
  
  // Parse the cloud data and store it in the array
  double xy[2];
  for (int d = 0; d < hsize; ++d)
  {
    xy[0] = d;
    xy[1] = cloud.points[0].histogram[d];
    xy_array->SetTuple (d, xy);
  }
  reCreateActor (xy_array, renwinupd, hsize);
  return (true);
}
Esempio n. 8
0
/** @brief Main function
 * @param argc
 * @param argv
 * @return Exit status */
int
main (int argc,
      char *argv[])
{
  if (argc != 2)
  {
    PCL_ERROR ("Usage:\n%s 192.168.100.65\n", argv[0]);
    return (-1);
  }

  davidsdk_ptr.reset (new pcl::DavidSDKGrabber);
  davidsdk_ptr->connect (argv[1]);

  if (!davidsdk_ptr->isConnected ())
    return (-1);
  PCL_WARN ("davidSDK connected\n");

  boost::function<void
  (const boost::shared_ptr<pcl::PCLImage> &)> f = boost::bind (&grabberCallback, _1);
  davidsdk_ptr->registerCallback (f);
  davidsdk_ptr->start ();
  waitForUser ("Press enter to quit");

  return (0);
}
Esempio n. 9
0
template<typename PointType, typename PointRfType> void
pcl::Hough3DGrouping<PointModelT, PointSceneT, PointModelRfT, PointSceneRfT>::computeRf (const boost::shared_ptr<const pcl::PointCloud<PointType> > &input, pcl::PointCloud<PointRfType> &rf)
{
  if (local_rf_search_radius_ == 0)
  {
    PCL_WARN ("[pcl::Hough3DGrouping::computeRf()] Warning! Reference frame search radius not set. Computing with default value. Results might be incorrect, algorithm might be slow.\n");
    local_rf_search_radius_ = static_cast<float> (hough_bin_size_);
  }
  pcl::PointCloud<Normal>::Ptr normal_cloud (new pcl::PointCloud<Normal> ());
  NormalEstimation<PointType, Normal> norm_est;
  norm_est.setInputCloud (input);
  if (local_rf_normals_search_radius_ <= 0.0f)
  {
    norm_est.setKSearch (15);
  }
  else
  {
    norm_est.setRadiusSearch (local_rf_normals_search_radius_);
  }  
  norm_est.compute (*normal_cloud);

  BOARDLocalReferenceFrameEstimation<PointType, Normal, PointRfType> rf_est;
  rf_est.setInputCloud (input);
  rf_est.setInputNormals (normal_cloud);
  rf_est.setFindHoles (true);
  rf_est.setRadiusSearch (local_rf_search_radius_);
  rf_est.compute (rf);
}
Esempio n. 10
0
void
computeFacesImpl <pcl::PointXYZ, pcl::PointXYZ>
(v4r::Model<pcl::PointXYZ> & model)
{
    (void) model;
    PCL_WARN("Not implemented for pcl::PointXYZ... this function would be available for PCL1.7.2 or higher\n");
}
Esempio n. 11
0
template <typename PointT> void
pcl::SupervoxelClustering<PointT>::reseedSupervoxels ()
{
  //Go through each supervoxel and remove all it's leaves
  for (typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ++sv_itr)
  {
    sv_itr->removeAllLeaves ();
  }
  
  std::vector<int> closest_index;
  std::vector<float> distance;
  //Now go through each supervoxel, find voxel closest to its center, add it in
  for (typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ++sv_itr)
  {
    PointT point;
    sv_itr->getXYZ (point.x, point.y, point.z);
    voxel_kdtree_->nearestKSearch (point, 1, closest_index, distance);
    
    LeafContainerT* seed_leaf = adjacency_octree_->at (closest_index[0]);
    if (seed_leaf)
    {
      sv_itr->addLeaf (seed_leaf);
    }
    else
    {
      PCL_WARN ("Could not find leaf in pcl::SupervoxelClustering<PointT>::reseedSupervoxels - supervoxel will be deleted \n");
    }
  }
  
}
 void setCylinderModel(const Cylinder &cylinderModel){
     cylinderModel_.color=cylinderModel.color;
     cylinderModel_.diameter=cylinderModel.diameter;
     cylinderModel_.height=cylinderModel.height;
     cylinderModelSet_=true;        
     if (debug_) PCL_WARN("Set cylinder model to diameter: %f, height: %f\n",cylinderModel_.diameter,cylinderModel_.height);
 }
Esempio n. 13
0
template <typename PointT> bool
pcl::visualization::PCLHistogramVisualizer::addFeatureHistogram (
    const pcl::PointCloud<PointT> &cloud, int hsize, 
    const std::string &id, int win_width, int win_height)
{
  RenWinInteractMap::iterator am_it = wins_.find (id);
  if (am_it != wins_.end ())
  {
    PCL_WARN ("[addFeatureHistogram] A window with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
    return (false);
  }

  vtkSmartPointer<vtkDoubleArray> xy_array = vtkSmartPointer<vtkDoubleArray>::New ();
  xy_array->SetNumberOfComponents (2);
  xy_array->SetNumberOfTuples (hsize);

  // Parse the cloud data and store it in the array
  double xy[2];
  for (int d = 0; d < hsize; ++d)
  {
    xy[0] = d;
    xy[1] = cloud.points[0].histogram[d];
    xy_array->SetTuple (d, xy);
  }
  RenWinInteract renwinint;
  createActor (xy_array, renwinint, id, win_width, win_height);

  // Save the pointer/ID pair to the global window map
  wins_[id] = renwinint;

  resetStoppedFlag ();
  return (true);
}
Esempio n. 14
0
template <typename PointT> void
pcl::ProjectInliers<PointT>::applyFilter (PointCloud &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.points.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];

  // Initialize the Sample Consensus model and set its parameters
  if (!initSACModel (model_type_))
  {
    PCL_ERROR ("[pcl::%s::segment] Error initializing the SAC model!\n", getClassName ().c_str ());
    output.width = output.height = 0;
    output.points.clear ();
    return;
  }
  if (copy_all_data_)
    sacmodel_->projectPoints (*indices_, model_coefficients, output, true);
  else
    sacmodel_->projectPoints (*indices_, model_coefficients, output, false);
}
Esempio n. 15
0
void
pcl::ImageGrabberBase::ImageGrabberImpl::loadDepthAndRGBFiles (const std::string &depth_dir, const std::string &rgb_dir)
{
  if (!boost::filesystem::exists (depth_dir) || !boost::filesystem::is_directory (depth_dir))
  {
    PCL_ERROR ("[pcl::ImageGrabber::loadDepthAndRGBFiles] Error: attempted to instantiate a pcl::ImageGrabber from a path which"
               " is not a directory: %s", depth_dir.c_str ());
    return;
  }
  if (!boost::filesystem::exists (rgb_dir) || !boost::filesystem::is_directory (rgb_dir))
  {
    PCL_ERROR ("[pcl::ImageGrabber::loadDepthAndRGBFiles] Error: attempted to instantiate a pcl::ImageGrabber from a path which"
               " is not a directory: %s", rgb_dir.c_str ());
    return;
  }
  std::string pathname;
  std::string extension;
  std::string basename;
  boost::filesystem::directory_iterator end_itr;
  // First iterate over depth images
  for (boost::filesystem::directory_iterator itr (depth_dir); itr != end_itr; ++itr)
  {
    extension = boost::algorithm::to_upper_copy (boost::filesystem::extension (itr->path ()));
    pathname = itr->path ().string ();
    basename = boost::filesystem::basename (itr->path ());
    if (!boost::filesystem::is_directory (itr->status ())
        && isValidExtension (extension))
    {
      if (basename.find ("depth") < std::string::npos)
      {
        depth_image_files_.push_back (pathname);
      }
    }
  }
  // Then iterate over RGB images
  for (boost::filesystem::directory_iterator itr (rgb_dir); itr != end_itr; ++itr)
  {
    extension = boost::algorithm::to_upper_copy (boost::filesystem::extension (itr->path ()));
    pathname = itr->path ().string ();
    basename = boost::filesystem::basename (itr->path ());
    if (!boost::filesystem::is_directory (itr->status ())
        && isValidExtension (extension))
    {
      if (basename.find ("rgb") < std::string::npos)
      {
        rgb_image_files_.push_back (pathname);
      }
    }
  }
  if (depth_image_files_.size () != rgb_image_files_.size () )
    PCL_WARN ("[pcl::ImageGrabberBase::ImageGrabberImpl::loadDepthAndRGBFiles] : Watch out not same amount of depth and rgb images");
  if (!depth_image_files_.empty ())
    sort (depth_image_files_.begin (), depth_image_files_.end ());
  else
    PCL_ERROR ("[pcl::ImageGrabberBase::ImageGrabberImpl::loadDepthAndRGBFiles] : no depth images added");
  if (!rgb_image_files_.empty ())
    sort (rgb_image_files_.begin (), rgb_image_files_.end ());
  else
    PCL_ERROR ("[pcl::ImageGrabberBase::ImageGrabberImpl::loadDepthAndRGBFiles] : no rgb images added");
}
Esempio n. 16
0
void 
ctrlC (int)
{
  boost::mutex::scoped_lock io_lock (io_mutex);
  std::cout << std::endl;
  PCL_WARN ("Ctrl-C detected, exit condition set to true\n");
  is_done = true;
}
Esempio n. 17
0
    template <typename PointT> bool
PCLVisualizer::addCorrespondences (
   const typename pcl::PointCloud<PointT>::ConstPtr &source_points,
   const typename pcl::PointCloud<PointT>::ConstPtr &target_points,
   const std::vector<pcl::Correspondence> &correspondences,
   const std::string &id,
   int viewport)
{
  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
  ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
  if (am_it != shape_actor_map_->end ())
  {
    PCL_WARN ("[addCorrespondences] A set of correspondences with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
    return (false);
  }

  vtkSmartPointer<vtkAppendPolyData> polydata = vtkSmartPointer<vtkAppendPolyData>::New ();

  vtkSmartPointer<vtkUnsignedCharArray> line_colors = vtkSmartPointer<vtkUnsignedCharArray>::New ();
  line_colors->SetNumberOfComponents (3);
  line_colors->SetName ("Colors");
  unsigned char rgb[3];
  // Use Red by default (can be changed later)
  rgb[0] = 1 * 255.0;
  rgb[1] = 0 * 255.0;
  rgb[2] = 0 * 255.0;

  // Draw lines between the best corresponding points
  for (size_t i = 0; i < correspondences.size (); ++i)
  {
    const PointT &p_src = source_points->points[correspondences[i].index_query];
    const PointT &p_tgt = target_points->points[correspondences[i].index_match];
    
    // Add the line
    vtkSmartPointer<vtkLineSource> line = vtkSmartPointer<vtkLineSource>::New ();
    line->SetPoint1 (p_src.x, p_src.y, p_src.z);
    line->SetPoint2 (p_tgt.x, p_tgt.y, p_tgt.z);
    line->Update ();
    polydata->AddInput (line->GetOutput ());
    line_colors->InsertNextTupleValue (rgb);
  }
  polydata->Update ();
  vtkSmartPointer<vtkPolyData> line_data = polydata->GetOutput ();
  line_data->GetCellData ()->SetScalars (line_colors);

  // Create an Actor
  vtkSmartPointer<vtkLODActor> actor;
  createActorFromVTKDataSet (line_data, actor);
  actor->GetProperty ()->SetRepresentationToWireframe ();
  actor->GetProperty ()->SetOpacity (0.5);
  addActorToRenderer (actor, viewport);

  // Save the pointer/ID pair to the global actor map
  (*shape_actor_map_)[id] = actor;

  return (true);
}
Esempio n. 18
0
    template <typename PointT> bool
    PCLVisualizer::fromHandlersToScreen (
      const GeometryHandlerConstPtr &geometry_handler,
      const PointCloudColorHandler<PointT> &color_handler, 
      const std::string &id,
      int viewport)
    {
      if (!geometry_handler->isCapable ())
      {
        PCL_WARN ("[fromHandlersToScreen] PointCloud <%s> requested with an invalid geometry handler (%s)!\n", id.c_str (), geometry_handler->getName ().c_str ());
        return (false);
      }

      if (!color_handler.isCapable ())
      {
        PCL_WARN ("[fromHandlersToScreen] PointCloud <%s> requested with an invalid color handler (%s)!\n", id.c_str (), color_handler.getName ().c_str ());
        return (false);
      }

      vtkSmartPointer<vtkPolyData> polydata;
      vtkSmartPointer<vtkIdTypeArray> initcells;
      // Convert the PointCloud to VTK PolyData
      convertPointCloudToVTKPolyData (geometry_handler, polydata, initcells);
      // use the given geometry handler
      polydata->Update ();

      // Get the colors from the handler
      vtkSmartPointer<vtkDataArray> scalars;
      color_handler.getColor (scalars);
      polydata->GetPointData ()->SetScalars (scalars);

      // Create an Actor
      vtkSmartPointer<vtkLODActor> actor;
      createActorFromVTKDataSet (polydata, actor);

      // Add it to all renderers
      addActorToRenderer (actor, viewport);

      // Save the pointer/ID pair to the global actor map
      (*cloud_actor_map_)[id].actor = actor;
      (*cloud_actor_map_)[id].cells = initcells;
      (*cloud_actor_map_)[id].geometry_handlers.push_back (geometry_handler);
      return (true);
    }
Esempio n. 19
0
template <typename PointT> bool
pcl::visualization::PCLHistogramVisualizer::addFeatureHistogram (
    const pcl::PointCloud<PointT> &cloud, 
    const std::string &field_name,
    const int index, 
    const std::string &id, int win_width, int win_height)
{
  if (index < 0 || index >= cloud.points.size ())
  {
    PCL_ERROR ("[addFeatureHistogram] Invalid point index (%d) given!\n", index);
    return (false);
  }

  // Get the fields present in this cloud
  std::vector<sensor_msgs::PointField> fields;
  // Check if our field exists
  int field_idx = pcl::getFieldIndex<PointT> (cloud, field_name, fields);
  if (field_idx == -1)
  {
    PCL_ERROR ("[addFeatureHistogram] The specified field <%s> does not exist!\n", field_name.c_str ());
    return (false);
  }

  RenWinInteractMap::iterator am_it = wins_.find (id);
  if (am_it != wins_.end ())
  {
    PCL_WARN ("[addFeatureHistogram] A window with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
    return (false);
  }

  vtkSmartPointer<vtkDoubleArray> xy_array = vtkSmartPointer<vtkDoubleArray>::New ();
  xy_array->SetNumberOfComponents (2);
  xy_array->SetNumberOfTuples (fields[field_idx].count);

  // Parse the cloud data and store it in the array
  double xy[2];
  for (int d = 0; d < fields[field_idx].count; ++d)
  {
    xy[0] = d;
    //xy[1] = cloud.points[index].histogram[d];
    float data;
    memcpy (&data, (const char*)&cloud.points[index] + fields[field_idx].offset + d * sizeof (float), sizeof (float));
    xy[1] = data;
    xy_array->SetTuple (d, xy);
  }
  RenWinInteract renwinint;
  createActor (xy_array, renwinint, id, win_width, win_height);

  // Save the pointer/ID pair to the global window map
  wins_[id] = renwinint;
#if ((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION <= 4))
  resetStoppedFlag ();
#endif
  return (true);
}
Esempio n. 20
0
/**
 * Saves a mesh into the specified file and output type. The file format is automatically parsed.
 * @param input[in] The mesh to be saved
 * @param output_file[out] The output file to be written
 * @param output_type[in]  The output file type
 * @return True on success, false otherwise.
 */
bool
saveMesh (pcl::PolygonMesh& input,
          std::string output_file,
          int output_type)
{
  if (boost::filesystem::path (output_file).extension () == ".obj")
  {
    if (output_type == BINARY || output_type == BINARY_COMPRESSED)
      PCL_WARN ("OBJ file format only supports ASCII.\n");

    //TODO: Support precision
    //FIXME: Color is lost during conversion (OBJ supports color)
    PCL_INFO ("Saving file %s as ASCII.\n", output_file.c_str ());
    if (pcl::io::saveOBJFile (output_file, input) != 0)
      return (false);
  }
  else if (boost::filesystem::path (output_file).extension () == ".pcd")
  {
    if (!input.polygons.empty ())
      PCL_WARN ("PCD file format does not support meshes! Only points be saved.\n");
    pcl::PCLPointCloud2::Ptr cloud = boost::make_shared<pcl::PCLPointCloud2> (input.cloud);
    if (!savePointCloud (cloud, output_file, output_type))
      return (false);
  }
  else  // PLY, STL and VTK
  {
    if (output_type == BINARY_COMPRESSED)
      PCL_WARN ("PLY, STL and VTK file formats only supports ASCII and binary output file types.\n");

    if (input.polygons.empty() && boost::filesystem::path (output_file).extension () == ".stl")
    {
      PCL_ERROR ("STL file format does not support point clouds! Aborting.\n");
      return (false);
    }

    PCL_INFO ("Saving file %s as %s.\n", output_file.c_str (), (output_type == ASCII) ? "ASCII" : "binary");
    if (!pcl::io::savePolygonFile (output_file, input, (output_type == ASCII) ? false : true))
      return (false);
  }

  return (true);
}
template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> bool
pcl::registration::CorrespondenceEstimationNormalShooting<PointSource, PointTarget, NormalT, Scalar>::initCompute ()
{
  if (!source_normals_)
  {
    PCL_WARN ("[pcl::registration::%s::initCompute] Datasets containing normals for source have not been given!\n", getClassName ().c_str ());
    return (false);
  }

  return (CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::initCompute ());
}
template <typename PointSource, typename PointTarget, typename NormalT> bool
pcl::registration::CorrespondenceEstimationBackProjection<PointSource, PointTarget, NormalT>::initCompute ()
{
  if (!source_normals_ || !target_normals_)
  {
    PCL_WARN ("[pcl::%s::compute] Datasets containing normals for source/target have not been given!\n", getClassName ().c_str ());
    return (false);
  }

  return (CorrespondenceEstimation<PointSource, PointTarget>::initCompute ());
}
Esempio n. 23
0
void ensensoExceptionHandling (const NxLibException &ex,
                          std::string func_nam)
{
  PCL_ERROR ("%s: NxLib error %s (%d) occurred while accessing item %s.\n", func_nam.c_str (), ex.getErrorText ().c_str (), ex.getErrorCode (),
            ex.getItemPath ().c_str ());
  if (ex.getErrorCode () == NxLibExecutionFailed)
  {
    NxLibCommand cmd ("");
    PCL_WARN ("\n%s\n", cmd.result ().asJson (true, 4, false).c_str ());
  }
}
Esempio n. 24
0
pcl::FieldComparison<PointT>::FieldComparison (
    std::string field_name, ComparisonOps::CompareOp op, double compare_val) :
      compare_val_(compare_val), point_data_(NULL)
{
  field_name_ = field_name;
  op_ = op;

  // Get all fields
  std::vector<sensor_msgs::PointField> point_fields; 
  // Use a dummy cloud to get the field types in a clever way
  PointCloud<PointT> dummyCloud;
  pcl::getFields (dummyCloud, point_fields);

  // Find field_name
  if (point_fields.empty ())
  {
    PCL_WARN ("[pcl::FieldComparison::FieldComparison] no fields found!\n");
    capable_ = false;
    return;
  }

  // Get the field index
  size_t d;
  for (d = 0; d < point_fields.size (); ++d)
  {
    if (point_fields[d].name == field_name) 
      break;
  }
  
  if (d == point_fields.size ())
  {
    PCL_WARN ("[pcl::FieldComparison::FieldComparison] field not found!\n");
    capable_ = false;
    return;
  }
  uint8_t datatype = point_fields[d].datatype;
  uint32_t offset = point_fields[d].offset;

  point_data_ = new PointDataAtOffset<PointT>(datatype, offset);
  capable_ = true;
}
Esempio n. 25
0
/** @brief Main function
 * @param[in] argc
 * @param[in] argv
 * @return Exit status */
int
main (int argc,
      char *argv[])
{
    if (argc != 2)
    {
        PCL_ERROR ("Usage:\n%s 192.168.100.65\n", argv[0]);
        return (-1);
    }

    viewer_ptr.reset (new CloudViewer ("davidSDK 3D cloud viewer"));
    davidsdk_ptr.reset (new pcl::DavidSDKGrabber);
    davidsdk_ptr->connect (argv[1]);

    if (!davidsdk_ptr->isConnected ())
        return (-1);
    PCL_WARN ("davidSDK connected\n");

#ifndef _WIN32// || _WIN64
    PCL_WARN ("Linux / Mac OSX detected, setting local_path_ to /var/tmp/davidsdk/ and remote_path_ to \\\\m6700\\davidsdk\\\n");
    davidsdk_ptr->setLocalAndRemotePaths ("/var/tmp/davidsdk/", "\\\\m6700\\davidsdk\\");
#endif

    //davidsdk_ptr->setFileFormatToPLY();
    std::cout << "Using " << davidsdk_ptr->getFileFormat () << " file format" << std::endl;

    boost::function<void
    (const PointCloudXYZ::Ptr&)> f = boost::bind (&grabberCallback, _1);
    davidsdk_ptr->registerCallback (f);
    davidsdk_ptr->start ();

    while (!viewer_ptr->wasStopped ())
    {
        boost::this_thread::sleep (boost::posix_time::seconds (20));
        std::cout << "FPS: " << davidsdk_ptr->getFramesPerSecond () << std::endl;
    }

    davidsdk_ptr->stop ();
    return (0);
}
int
main (int argc, char** argv)
{
  if (pcl::console::find_switch (argc, argv, "--help") || pcl::console::find_switch (argc, argv, "-h"))
    return print_help ();

  //Reading input cloud
  pcl::PointCloud<pcl::PointXYZI>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZI>);

  if (argc < 2) {
    PCL_ERROR ("No pcd file to read... Exiting...\n");
    print_help ();
    return (-1);
  }

  if (pcl::io::loadPCDFile<pcl::PointXYZI> (argv[1], *cloud) == -1) //* load the file
  {
    PCL_ERROR ("Couldn't read file %s \n", argv[1]);
    print_help ();
    return (-1);
  }
  
  // Creating world model object
  pcl::kinfuLS::WorldModel<pcl::PointXYZI> wm;
  
  //Adding current cloud to the world model
  wm.addSlice (cloud);
  
  std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> clouds;
  std::vector<Eigen::Vector3f> transforms;
  
  //Get world as a vector of cubes 
  wm.getWorldAsCubes (pcl::device::kinfuLS::VOLUME_X, clouds, transforms, 0.025); // 2.5% overlapp (12 cells with a 512-wide cube)

  //Creating the standalone marching cubes instance
  float volume_size = pcl::device::kinfuLS::VOLUME_SIZE;
  pcl::console::parse_argument (argc, argv, "--volume_size", volume_size);
  pcl::console::parse_argument (argc, argv, "-vs", volume_size);

  PCL_WARN ("Processing world with volume size set to %.2f meters\n", volume_size);

  pcl::gpu::kinfuLS::StandaloneMarchingCubes<pcl::PointXYZI> m_cubes (pcl::device::kinfuLS::VOLUME_X, pcl::device::kinfuLS::VOLUME_Y, pcl::device::kinfuLS::VOLUME_Z, volume_size);

  //~ //Creating the output
  //~ boost::shared_ptr<pcl::PolygonMesh> mesh_ptr_;
  //~ std::vector< boost::shared_ptr<pcl::PolygonMesh> > meshes;

  m_cubes.getMeshesFromTSDFVector (clouds, transforms);

 PCL_INFO ("Done!\n");
  return (0);
}
Esempio n. 27
0
template <typename PointSource, typename PointTarget> inline void
pcl::Registration<PointSource, PointTarget>::align (PointCloudSource &output, const Eigen::Matrix4f& guess)
{
  if (!initCompute ()) return;

  if (!target_)
  {
    PCL_WARN ("[pcl::%s::compute] No input target dataset was given!\n", getClassName ().c_str ());
    return;
  }

  // Resize the output dataset
  if (output.points.size () != indices_->size ())
    output.points.resize (indices_->size ());
  // Copy the header
  output.header   = input_->header;
  // Check if the output will be computed for all points or only a subset
  if (indices_->size () != input_->points.size ())
  {
    output.width    = (int) indices_->size ();
    output.height   = 1;
  }
  else
  {
    output.width    = input_->width;
    output.height   = input_->height;
  }
  output.is_dense = input_->is_dense;

  // Copy the point data to output
  for (size_t i = 0; i < indices_->size (); ++i)
    output.points[i] = input_->points[(*indices_)[i]];

  // Set the internal point representation of choice
  if (point_representation_)
    tree_->setPointRepresentation (point_representation_);

  // Perform the actual transformation computation
  converged_ = false;
  final_transformation_ = transformation_ = previous_transformation_ = Eigen::Matrix4f::Identity ();

  // Right before we estimate the transformation, we set all the point.data[3] values to 1 to aid the rigid 
  // transformation
  for (size_t i = 0; i < indices_->size (); ++i)
    output.points[i].data[3] = 1.0;

  computeTransformation (output, guess);

  deinitCompute ();
}
Esempio n. 28
0
template<typename PointT> typename pcl::registration::LUM<PointT>::Vertex
pcl::registration::LUM<PointT>::addPointCloud (PointCloudPtr cloud, const Eigen::Vector6f &pose)
{
  Vertex v = add_vertex (*slam_graph_);
  (*slam_graph_)[v].cloud_ = cloud;
  if (v == 0 && pose != Eigen::Vector6f::Zero ())
  {
    PCL_WARN("[pcl::registration::LUM::addPointCloud] The pose estimate is ignored for the first cloud in the graph since that will become the reference pose.\n");
    (*slam_graph_)[v].pose_ = Eigen::Vector6f::Zero ();
    return (v);
  }
  (*slam_graph_)[v].pose_ = pose;
  return (v);
}
Esempio n. 29
0
template <typename PointT> void
pcl::getPointCloudDifference (
    const pcl::PointCloud<PointT> &src, 
    const pcl::PointCloud<PointT> &, 
    double threshold, const boost::shared_ptr<pcl::search::Search<PointT> > &tree,
    pcl::PointCloud<PointT> &output)
{
  // We're interested in a single nearest neighbor only
  std::vector<int> nn_indices (1);
  std::vector<float> nn_distances (1);

  // The src indices that do not have a neighbor in tgt
  std::vector<int> src_indices;

  // Iterate through the source data set
  for (int i = 0; i < static_cast<int> (src.points.size ()); ++i)
  {
    if (!isFinite (src.points[i]))
      continue;
    // Search for the closest point in the target data set (number of neighbors to find = 1)
    if (!tree->nearestKSearch (src.points[i], 1, nn_indices, nn_distances))
    {
      PCL_WARN ("No neighbor found for point %zu (%f %f %f)!\n", i, src.points[i].x, src.points[i].y, src.points[i].z);
      continue;
    }

    if (nn_distances[0] > threshold)
      src_indices.push_back (i);
  }
 
  // Allocate enough space and copy the basics
  output.points.resize (src_indices.size ());
  output.header   = src.header;
  output.width    = static_cast<uint32_t> (src_indices.size ());
  output.height   = 1;
  //if (src.is_dense)
    output.is_dense = true;
  //else
    // It's not necessarily true that is_dense is false if cloud_in.is_dense is false
    // To verify this, we would need to iterate over all points and check for NaNs
    //output.is_dense = false;

  // Copy all the data fields from the input cloud to the output one
  typedef typename pcl::traits::fieldList<PointT>::type FieldList;
  // Iterate over each point
  for (size_t i = 0; i < src_indices.size (); ++i)
    // Iterate over each dimension
    pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (src.points[src_indices[i]], output.points[i]));
}
Esempio n. 30
0
template <typename PointT> bool
pcl::visualization::PCLHistogramVisualizer::updateFeatureHistogram (
    const pcl::PointCloud<PointT> &cloud, const std::string &field_name, const int index, 
    const std::string &id)
{
  if (index < 0 || index >= cloud.points.size ())
  {
    PCL_ERROR ("[updateFeatureHistogram] Invalid point index (%d) given!\n", index);
    return (false);
  }
  
  // Get the fields present in this cloud
  std::vector<sensor_msgs::PointField> fields;
  // Check if our field exists
  int field_idx = pcl::getFieldIndex<PointT> (cloud, field_name, fields);
  if (field_idx == -1)
  {
    PCL_ERROR ("[updateFeatureHistogram] The specified field <%s> does not exist!\n", field_name.c_str ());
    return (false);
  }

  RenWinInteractMap::iterator am_it = wins_.find (id);
  if (am_it == wins_.end ())
  {
    PCL_WARN ("[updateFeatureHistogram] A window with id <%s> does not exists!.\n", id.c_str ());
    return (false);
  }
  RenWinInteract* renwinupd = &wins_[id];
    
  vtkSmartPointer<vtkDoubleArray> xy_array = vtkSmartPointer<vtkDoubleArray>::New ();
  xy_array->SetNumberOfComponents (2);
  xy_array->SetNumberOfTuples (fields[field_idx].count);

  // Parse the cloud data and store it in the array
  double xy[2];
  for (int d = 0; d < fields[field_idx].count; ++d)
  {
    xy[0] = d;
    //xy[1] = cloud.points[index].histogram[d];
    float data;
    memcpy (&data, (const char*)&cloud.points[index] + fields[field_idx].offset + d * sizeof (float), sizeof (float));
    xy[1] = data;
    xy_array->SetTuple (d, xy);
  }
  
  reCreateActor (xy_array, renwinupd, cloud.fields[field_idx].count - 1);
  return (true);
}