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);
  }
}

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);
}

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;
}

      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++;
      }

/** @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);
}

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);
}

/** @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);
}

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);
}

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");
}

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);
 }

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);
}

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);
}

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");
}

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;
}

    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);
}

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);
}

/**
 * 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 ());
}