Small fixes and code improvements

- in PCLPointCloud2.h, a cast is necessary, otherwise the elements are printed as characters instead of numbers
- in setNumberOfThreads, add debug output/warning if OpenMP is not available
- resize is more efficient than substr
- implicit_shape_model.hpp: move peak_counter increment inside if-statement. This does not change much in practice because all of the peak_densities are larger than -1.0, but it silences the warning that strongest_peak may be uninitialized
- region_growing.hpp and region_growing_rgb.hpp: switch range-for-loop, while also fixing bug (i_point was used in nearestKSearch, but point_index should have been used instead)
- pcl_visualizer.hpp: make sure that point and normals are finite, otherwise VTK displays strange things

common/include/pcl/PCLPointCloud2.h

@@ -143,7 +143,7 @@ namespace pcl
     for (std::size_t i = 0; i < v.data.size (); ++i)
     {
       s << "  data[" << i << "]: ";
-      s << "  " << v.data[i] << std::endl;
+      s << "  " << static_cast<int>(v.data[i]) << std::endl;
     }
     s << "is_dense: ";
     s << "  " << v.is_dense << std::endl;

common/include/pcl/impl/pcl_base.hpp

@@ -162,6 +162,7 @@ pcl::PCLBase<PointT>::initCompute ()
     catch (const std::bad_alloc&)
     {
       PCL_ERROR ("[initCompute] Failed to allocate %lu indices.\n", input_->size ());
+      return (false);
     }
     for (auto i = indices_size; i < indices_->size (); ++i) { (*indices_)[i] = static_cast<int>(i); }
   }

common/src/pcl_base.cpp

@@ -124,6 +124,7 @@ pcl::PCLBase<pcl::PCLPointCloud2>::initCompute ()
     catch (const std::bad_alloc&)
     {
       PCL_ERROR ("[initCompute] Failed to allocate %lu indices.\n", (input_->width * input_->height));
+      return (false);
     }
     if (indices_size < indices_->size ())
       std::iota(indices_->begin () + indices_size, indices_->end (), indices_size);

features/include/pcl/features/impl/normal_3d_omp.hpp

@@ -47,14 +47,17 @@
 template <typename PointInT, typename PointOutT> void
 pcl::NormalEstimationOMP<PointInT, PointOutT>::setNumberOfThreads (unsigned int nr_threads)
 {
-  if (nr_threads == 0)
 #ifdef _OPENMP
+  if (nr_threads == 0)
     threads_ = omp_get_num_procs();
-#else
-    threads_ = 1;
-#endif
   else
     threads_ = nr_threads;
+  PCL_DEBUG ("[pcl::NormalEstimationOMP::setNumberOfThreads] Setting number of threads to %u.\n", threads_);
+#else
+  threads_ = 1;
+  if (nr_threads != 1)
+    PCL_WARN ("[pcl::NormalEstimationOMP::setNumberOfThreads] Parallelization is requested, but OpenMP is not available! Continuing without parallelization.\n");
+#endif // _OPENMP
 }
 
 ///////////////////////////////////////////////////////////////////////////////////////////

io/include/pcl/io/io.h

@@ -38,5 +38,6 @@
  */
 
 #pragma once
+#include <pcl/pcl_macros.h> // for PCL_DEPRECATED_HEADER
 PCL_DEPRECATED_HEADER(1, 15, "Please include pcl/common/io.h directly.")
 #include <pcl/common/io.h>

io/src/lzf_image_io.cpp

@@ -135,7 +135,7 @@ pcl::io::LZFImageWriter::compress (const char* input,
     if (itype.size () > 16)
     {
       PCL_WARN ("[pcl::io::LZFImageWriter::compress] Image type should be a string of maximum 16 characters! Cutting %s to %s.\n", image_type.c_str (), image_type.substr (0, 15).c_str ());
-      itype = itype.substr (0, 15);
+      itype.resize(16);
     }
     if (itype.size () < 16)
       itype.insert (itype.end (), 16 - itype.size (), ' ');

recognition/include/pcl/recognition/impl/implicit_shape_model.hpp

@@ -192,8 +192,8 @@ pcl::features::ISMVoteList<PointT>::findStrongestPeaks (
         best_density = peak_densities[i];
         strongest_peak = peaks[i];
         best_peak_ind = i;
+        ++peak_counter;
       }
-      ++peak_counter;
     }
 
     if( peak_counter == 0 )

segmentation/include/pcl/segmentation/impl/region_growing.hpp

@@ -342,30 +342,27 @@ pcl::RegionGrowing<PointT, NormalT>::prepareForSegmentation ()
 template <typename PointT, typename NormalT> void
 pcl::RegionGrowing<PointT, NormalT>::findPointNeighbours ()
 {
-  int point_number = static_cast<int> (indices_->size ());
   pcl::Indices neighbours;
   std::vector<float> distances;
 
   point_neighbours_.resize (input_->size (), neighbours);
   if (input_->is_dense)
   {
-    for (int i_point = 0; i_point < point_number; i_point++)
+    for (const auto& point_index: (*indices_))
     {
-      const auto point_index = (*indices_)[i_point];
       neighbours.clear ();
-      search_->nearestKSearch (i_point, neighbour_number_, neighbours, distances);
+      search_->nearestKSearch (point_index, neighbour_number_, neighbours, distances);
       point_neighbours_[point_index].swap (neighbours);
     }
   }
   else
   {
-    for (int i_point = 0; i_point < point_number; i_point++)
+    for (const auto& point_index: (*indices_))
     {
-      neighbours.clear ();
-      const auto point_index = (*indices_)[i_point];
       if (!pcl::isFinite ((*input_)[point_index]))
         continue;
-      search_->nearestKSearch (i_point, neighbour_number_, neighbours, distances);
+      neighbours.clear ();
+      search_->nearestKSearch (point_index, neighbour_number_, neighbours, distances);
       point_neighbours_[point_index].swap (neighbours);
     }
   }

segmentation/include/pcl/segmentation/impl/region_growing_rgb.hpp

@@ -272,19 +272,17 @@ pcl::RegionGrowingRGB<PointT, NormalT>::prepareForSegmentation ()
 template <typename PointT, typename NormalT> void
 pcl::RegionGrowingRGB<PointT, NormalT>::findPointNeighbours ()
 {
-  int point_number = static_cast<int> (indices_->size ());
   pcl::Indices neighbours;
   std::vector<float> distances;
 
   point_neighbours_.resize (input_->size (), neighbours);
   point_distances_.resize (input_->size (), distances);
 
-  for (int i_point = 0; i_point < point_number; i_point++)
+  for (const auto& point_index: (*indices_))
   {
-    int point_index = (*indices_)[i_point];
     neighbours.clear ();
     distances.clear ();
-    search_->nearestKSearch (i_point, region_neighbour_number_, neighbours, distances);
+    search_->nearestKSearch (point_index, region_neighbour_number_, neighbours, distances);
     point_neighbours_[point_index].swap (neighbours);
     point_distances_[point_index].swap (distances);
   }

visualization/include/pcl/visualization/impl/pcl_visualizer.hpp

@@ -906,6 +906,8 @@ pcl::visualization::PCLVisualizer::addPointCloudNormals (
       for (vtkIdType x = 0; x < normals->width; x += point_step)
       {
         PointT p = (*cloud)(x, y);
+        if (!pcl::isFinite(p) || !pcl::isNormalFinite((*normals)(x, y)))
+          continue;
         p.x += (*normals)(x, y).normal[0] * scale;
         p.y += (*normals)(x, y).normal[1] * scale;
         p.z += (*normals)(x, y).normal[2] * scale;
@@ -928,8 +930,10 @@ pcl::visualization::PCLVisualizer::addPointCloudNormals (
     nr_normals = (cloud->size () - 1) / level + 1 ;
     pts = new float[2 * nr_normals * 3];
 
-    for (vtkIdType i = 0, j = 0; j < nr_normals; j++, i = j * level)
+    for (vtkIdType i = 0, j = 0; (j < nr_normals) && (i < static_cast<vtkIdType>(cloud->size())); i += level)
     {
+      if (!pcl::isFinite((*cloud)[i]) || !pcl::isNormalFinite((*normals)[i]))
+        continue;
       PointT p = (*cloud)[i];
       p.x += (*normals)[i].normal[0] * scale;
       p.y += (*normals)[i].normal[1] * scale;
@@ -945,6 +949,7 @@ pcl::visualization::PCLVisualizer::addPointCloudNormals (
       lines->InsertNextCell (2);
       lines->InsertCellPoint (2 * j);
       lines->InsertCellPoint (2 * j + 1);
+      ++j;
     }
   }