Small fixes and improvements

common/include/pcl/PCLPointField.h

@@ -45,12 +45,15 @@ namespace pcl
     s << "  " << v.offset << std::endl;
     s << "datatype: ";
     switch(v.datatype) {
+      case ::pcl::PCLPointField::PointFieldTypes::BOOL: s << "  BOOL" << std::endl; break;
       case ::pcl::PCLPointField::PointFieldTypes::INT8: s << "  INT8" << std::endl; break;
       case ::pcl::PCLPointField::PointFieldTypes::UINT8: s << "  UINT8" << std::endl; break;
       case ::pcl::PCLPointField::PointFieldTypes::INT16: s << "  INT16" << std::endl; break;
       case ::pcl::PCLPointField::PointFieldTypes::UINT16: s << "  UINT16" << std::endl; break;
       case ::pcl::PCLPointField::PointFieldTypes::INT32: s << "  INT32" << std::endl; break;
       case ::pcl::PCLPointField::PointFieldTypes::UINT32: s << "  UINT32" << std::endl; break;
+      case ::pcl::PCLPointField::PointFieldTypes::INT64: s << "  INT64" << std::endl; break;
+      case ::pcl::PCLPointField::PointFieldTypes::UINT64: s << "  UINT64" << std::endl; break;
       case ::pcl::PCLPointField::PointFieldTypes::FLOAT32: s << "  FLOAT32" << std::endl; break;
       case ::pcl::PCLPointField::PointFieldTypes::FLOAT64: s << "  FLOAT64" << std::endl; break;
       default: s << "  " << static_cast<int>(v.datatype) << std::endl;

common/src/PCLPointCloud2.cpp

@@ -137,6 +137,7 @@ pcl::PCLPointCloud2::concatenate (pcl::PCLPointCloud2 &cloud1, const pcl::PCLPoi
   cloud1.is_dense = cloud1.is_dense && cloud2.is_dense;
   cloud1.height = 1;
   cloud1.width = size1 + size2;
+  cloud1.row_step = cloud1.width * cloud1.point_step; // changed width
 
   if (simple_layout)
   {

examples/features/example_difference_of_normals.cpp

@@ -46,8 +46,9 @@ int main (int argc, char *argv[])
   bool approx = false;
   constexpr double decimation = 100;
 
-  if(argc < 2){
+  if(argc < 3){
     std::cerr << "Expected 2 arguments: inputfile outputfile" << std::endl;
+    return 0;
   }
 
   ///The file to read from.

features/include/pcl/features/impl/intensity_gradient.hpp

@@ -148,6 +148,13 @@ pcl::IntensityGradientEstimation<PointInT, PointNT, PointOutT, IntensitySelector
   std::vector<float> nn_dists (k_);
   output.is_dense = true;
 
+#ifdef _OPENMP
+  if (threads_ == 0) {
+    threads_ = omp_get_num_procs();
+    PCL_DEBUG ("[pcl::IntensityGradientEstimation::computeFeature] Setting number of threads to %u.\n", threads_);
+  }
+#endif // _OPENMP
+
   // If the data is dense, we don't need to check for NaN
   if (surface_->is_dense)
   {

features/include/pcl/features/impl/shot_lrf_omp.hpp

@@ -84,9 +84,6 @@ pcl::SHOTLocalReferenceFrameEstimationOMP<PointInT, PointOutT>::computeFeature (
     //output_rf.confidence = getLocalRF ((*indices_)[i], rf);
     //if (output_rf.confidence == std::numeric_limits<float>::max ())
 
-    pcl::Indices n_indices;
-    std::vector<float> n_sqr_distances;
-    this->searchForNeighbors ((*indices_)[i], search_parameter_, n_indices, n_sqr_distances);
     if (getLocalRF ((*indices_)[i], rf) == std::numeric_limits<float>::max ())
     {
       output.is_dense = false;

filters/include/pcl/filters/convolution_3d.h

@@ -40,6 +40,7 @@
 #pragma once
 
 #include <pcl/pcl_base.h>
+#include <pcl/search/search.h> // for pcl::search::Search
 #include <boost/optional.hpp>
 #include <pcl/search/search.h>
 

filters/include/pcl/filters/impl/convolution_3d.hpp

@@ -40,6 +40,7 @@
 #ifndef PCL_FILTERS_CONVOLUTION_3D_IMPL_HPP
 #define PCL_FILTERS_CONVOLUTION_3D_IMPL_HPP
 
+#include <pcl/common/point_tests.h> // for isFinite
 #include <pcl/search/organized.h>
 #include <pcl/search/kdtree.h>
 #include <pcl/pcl_config.h>

filters/include/pcl/filters/impl/radius_outlier_removal.hpp

@@ -138,7 +138,8 @@ pcl::RadiusOutlierRemoval<PointT>::applyFilterIndices (Indices &indices)
     {
       // Perform the radius search
       // Note: k includes the query point, so is always at least 1
-      int k = searcher_->radiusSearch (index, search_radius_, nn_indices, nn_dists);
+      // last parameter (max_nn) is the maximum number of neighbors returned. If enough neighbors are found so that point can not be an outlier, we stop searching.
+      int k = searcher_->radiusSearch (index, search_radius_, nn_indices, nn_dists, min_pts_radius_ + 1);
 
       // Points having too few neighbors are outliers and are passed to removed indices
       // Unless negative was set, then it's the opposite condition

filters/include/pcl/filters/impl/uniform_sampling.hpp

@@ -48,7 +48,7 @@ pcl::UniformSampling<PointT>::applyFilter (PointCloud &output)
   // Has the input dataset been set already?
   if (!input_)
   {
-    PCL_WARN ("[pcl::%s::detectKeypoints] No input dataset given!\n", getClassName ().c_str ());
+    PCL_WARN ("[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().c_str ());
     output.width = output.height = 0;
     output.clear ();
     return;

filters/include/pcl/filters/impl/voxel_grid_covariance.hpp

@@ -442,11 +442,11 @@ pcl::VoxelGridCovariance<PointT>::getAllNeighborsAtPoint(const PointT& reference
 
 //////////////////////////////////////////////////////////////////////////////////////////
 template<typename PointT> void
-pcl::VoxelGridCovariance<PointT>::getDisplayCloud (pcl::PointCloud<PointXYZ>& cell_cloud)
+pcl::VoxelGridCovariance<PointT>::getDisplayCloud (pcl::PointCloud<PointXYZ>& cell_cloud, int pnt_per_cell) const
 {
   cell_cloud.clear ();
 
-  int pnt_per_cell = 1000;
+  // for now, we use random generator and normal distribution from boost instead of std because switching to std would make this function up to 2.8 times slower
   boost::mt19937 rng;
   boost::normal_distribution<> nd (0.0, 1.0);
   boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > var_nor (rng, nd);
@@ -463,7 +463,7 @@ pcl::VoxelGridCovariance<PointT>::getDisplayCloud (pcl::PointCloud<PointXYZ>& ce
   // Generate points for each occupied voxel with sufficient points.
   for (auto it = leaves_.begin (); it != leaves_.end (); ++it)
   {
-    Leaf& leaf = it->second;
+    const Leaf& leaf = it->second;
 
     if (leaf.nr_points >= min_points_per_voxel_)
     {

filters/include/pcl/filters/voxel_grid_covariance.h

@@ -434,9 +434,10 @@ namespace pcl
 
       /** \brief Get a cloud to visualize each voxels normal distribution.
        * \param[out] cell_cloud a cloud created by sampling the normal distributions of each voxel
+       * \param[in] pnt_per_cell how many points should be generated for each cell
        */
       void
-      getDisplayCloud (pcl::PointCloud<PointXYZ>& cell_cloud);
+      getDisplayCloud (pcl::PointCloud<PointXYZ>& cell_cloud, int pnt_per_cell = 1000) const;
 
       /** \brief Search for the k-nearest occupied voxels for the given query point.
        * \note Only voxels containing a sufficient number of points are used.

gpu/features/src/normal_3d.cu

@@ -55,7 +55,8 @@ namespace pcl
                 CTA_SIZE = 256,
                 WAPRS = CTA_SIZE / Warp::WARP_SIZE,
 
-                MIN_NEIGHBOORS = 1
+                // if there are fewer than 3 neighbors, the normal is definitely garbage
+                MIN_NEIGHBOORS = 3
             };
 
             struct plus 
@@ -86,6 +87,7 @@ namespace pcl
                 {
                     constexpr float NaN = std::numeric_limits<float>::quiet_NaN();
                     normals.data[idx] = make_float4(NaN, NaN, NaN, NaN);
+                    return;
                 }
 
                 const int *ibeg = indices.ptr(idx);

recognition/CMakeLists.txt

@@ -2,12 +2,7 @@ set(SUBSYS_NAME recognition)
 set(SUBSYS_DESC "Point cloud recognition library")
 set(SUBSYS_DEPS common io search kdtree octree features filters registration sample_consensus ml)
 
-if(${PCL_VERSION} VERSION_LESS "1.15.0" AND NOT TARGET Boost::filesystem)
-  set(DEFAULT OFF)
-  set(REASON "Boost filesystem is not available.")
-else()
-  set(DEFAULT ON)
-endif()
+set(DEFAULT ON)
 
 PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ${DEFAULT} "${REASON}")
 PCL_SUBSYS_DEPEND(build NAME ${SUBSYS_NAME} DEPS ${SUBSYS_DEPS})

registration/include/pcl/registration/gicp.h

@@ -52,6 +52,22 @@ namespace pcl {
  * The original code uses GSL and ANN while in ours we use FLANN and Newton's method
  * for optimization (call `useBFGS` to switch to BFGS optimizer, however Newton
  * is usually faster and more accurate).
+ * Basic usage example:
+ * \code
+ * pcl::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> reg;
+ * reg.setInputSource(src);
+ * reg.setInputTarget(tgt);
+ * // use default parameters or set them yourself, for example:
+ * // reg.setMaximumIterations(...);
+ * // reg.setTransformationEpsilon(...);
+ * // reg.setRotationEpsilon(...);
+ * // reg.setCorrespondenceRandomness(...);
+ * pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
+ * // supply a better guess, if possible:
+ * Eigen::Matrix4f guess = Eigen::Matrix4f::Identity();
+ * reg.align(*output, guess);
+ * std::cout << reg.getFinalTransformation() << std::endl;
+ * \endcode
  * \author Nizar Sallem
  * \ingroup registration
  */

registration/include/pcl/registration/impl/correspondence_estimation.hpp

@@ -43,6 +43,7 @@
 
 #include <pcl/common/copy_point.h>
 #include <pcl/common/io.h>
+#include <pcl/common/point_tests.h> // for isXYZFinite
 
 namespace pcl {
 
@@ -169,6 +170,8 @@ CorrespondenceEstimation<PointSource, PointTarget, Scalar>::determineCorresponde
     // Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT
     // macro!
     const auto& pt = detail::pointCopyOrRef<PointTarget, PointSource>(input_, idx);
+    if (!input_->is_dense && !pcl::isXYZFinite(pt))
+      continue;
     tree_->nearestKSearch(pt, 1, index, distance);
     if (distance[0] > max_dist_sqr)
       continue;
@@ -251,7 +254,8 @@ CorrespondenceEstimation<PointSource, PointTarget, Scalar>::
     // macro!
 
     const auto& pt_src = detail::pointCopyOrRef<PointTarget, PointSource>(input_, idx);
-
+    if (!input_->is_dense && !pcl::isXYZFinite(pt_src))
+      continue;
     tree_->nearestKSearch(pt_src, 1, index, distance);
     if (distance[0] > max_dist_sqr)
       continue;

registration/include/pcl/registration/impl/icp_nl.hpp

@@ -37,7 +37,4 @@
  * $Id$
  *
  */
-#ifndef PCL_REGISTRATION_ICP_NL_HPP_
-#define PCL_REGISTRATION_ICP_NL_HPP_
-
-#endif /* PCL_REGISTRATION_ICP_NL_HPP_ */
+PCL_DEPRECATED_HEADER(1, 18, "Do not include icp_nl.hpp, it is empty.")

sample_consensus/include/pcl/sample_consensus/impl/sac_model_circle3d.hpp

@@ -194,7 +194,9 @@ pcl::SampleConsensusModelCircle3D<PointT>::selectWithinDistance (
     return;
   }
   inliers.clear ();
+  error_sqr_dists_.clear ();
   inliers.reserve (indices_->size ());
+  error_sqr_dists_.reserve (indices_->size ());
 
   const auto squared_threshold = threshold * threshold;
   // Iterate through the 3d points and calculate the distances from them to the sphere
@@ -221,10 +223,12 @@ pcl::SampleConsensusModelCircle3D<PointT>::selectWithinDistance (
     Eigen::Vector3d K = C + r * helper_vectorP_projC.normalized ();
     Eigen::Vector3d distanceVector =  P - K;
 
-    if (distanceVector.squaredNorm () < squared_threshold)
+    const double sqr_dist = distanceVector.squaredNorm ();
+    if (sqr_dist < squared_threshold)
     {
       // Returns the indices of the points whose distances are smaller than the threshold
       inliers.push_back ((*indices_)[i]);
+      error_sqr_dists_.push_back (sqr_dist);
     }
   }
 }
@@ -338,11 +342,11 @@ pcl::SampleConsensusModelCircle3D<PointT>::projectPoints (
       pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> ((*input_)[i], projected_points[i]));
 
     // Iterate through the 3d points and calculate the distances from them to the plane
-    for (std::size_t i = 0; i < inliers.size (); ++i)
+    for (const auto &inlier : inliers)
     {
       // what i have:
       // P : Sample Point
-      Eigen::Vector3d P ((*input_)[inliers[i]].x, (*input_)[inliers[i]].y, (*input_)[inliers[i]].z);
+      Eigen::Vector3d P ((*input_)[inlier].x, (*input_)[inlier].y, (*input_)[inlier].z);
       // C : Circle Center
       Eigen::Vector3d C (model_coefficients[0], model_coefficients[1], model_coefficients[2]);
       // N : Circle (Plane) Normal
@@ -361,9 +365,9 @@ pcl::SampleConsensusModelCircle3D<PointT>::projectPoints (
       // K : Point on Circle
       Eigen::Vector3d K = C + r * helper_vectorP_projC.normalized ();
 
-      projected_points[i].x = static_cast<float> (K[0]);
-      projected_points[i].y = static_cast<float> (K[1]);
-      projected_points[i].z = static_cast<float> (K[2]);
+      projected_points[inlier].x = static_cast<float> (K[0]);
+      projected_points[inlier].y = static_cast<float> (K[1]);
+      projected_points[inlier].z = static_cast<float> (K[2]);
     }
   }
   else

sample_consensus/include/pcl/sample_consensus/impl/sac_model_cone.hpp

@@ -87,6 +87,11 @@ pcl::SampleConsensusModelCone<PointT, PointNT>::computeModelCoefficients (
   Eigen::Vector4f ortho31 = n3.cross3(n1);
 
   float denominator = n1.dot(ortho23);
+  if (std::abs(denominator) < Eigen::NumTraits<float>::dummy_precision ())
+  {
+    PCL_ERROR ("[pcl::SampleConsensusModelCone::computeModelCoefficients] Impossible to compute stable model with these points.\n");
+    return (false);
+  }
 
   float d1 = p1.dot (n1);
   float d2 = p2.dot (n2);

sample_consensus/include/pcl/sample_consensus/impl/sac_model_ellipse3d.hpp

@@ -235,7 +235,7 @@ pcl::SampleConsensusModelEllipse3D<PointT>::computeModelCoefficients (const Indi
   model_coefficients[10] = static_cast<float>(x_axis[2]);
 
 
-  PCL_DEBUG ("[pcl::SampleConsensusModelEllipse3D::computeModelCoefficients] Model is (%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g).\n",
+  PCL_DEBUG ("[pcl::SampleConsensusModelEllipse3D::computeModelCoefficients] Model is (%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g).\n",
              model_coefficients[0], model_coefficients[1], model_coefficients[2], model_coefficients[3],
              model_coefficients[4], model_coefficients[5], model_coefficients[6], model_coefficients[7],
              model_coefficients[8], model_coefficients[9], model_coefficients[10]);
@@ -312,12 +312,14 @@ pcl::SampleConsensusModelEllipse3D<PointT>::selectWithinDistance (
     Indices &inliers)
 {
   inliers.clear();
+  error_sqr_dists_.clear();
   // Check if the model is valid given the user constraints
   if (!isModelValid (model_coefficients))
   {
     return;
   }
   inliers.reserve (indices_->size ());
+  error_sqr_dists_.reserve (indices_->size ());
 
   // c : Ellipse Center
   const Eigen::Vector3f c(model_coefficients[0], model_coefficients[1], model_coefficients[2]);
@@ -358,10 +360,12 @@ pcl::SampleConsensusModelEllipse3D<PointT>::selectWithinDistance (
     float th_opt;
     const Eigen::Vector2f distanceVector = dvec2ellipse(params, p_(0), p_(1), th_opt);
 
-    if (distanceVector.squaredNorm() < squared_threshold)
+    const double sqr_dist = distanceVector.squaredNorm();
+    if (sqr_dist < squared_threshold)
     {
       // Returns the indices of the points whose distances are smaller than the threshold
       inliers.push_back ((*indices_)[i]);
+      error_sqr_dists_.push_back (sqr_dist);
     }
   }
 }
@@ -453,7 +457,7 @@ pcl::SampleConsensusModelEllipse3D<PointT>::optimizeModelCoefficients (
     optimized_coefficients[i] = static_cast<float> (coeff[i]);
 
   // Compute the L2 norm of the residuals
-  PCL_DEBUG ("[pcl::SampleConsensusModelEllipse3D::optimizeModelCoefficients] LM solver finished with exit code %i, having a residual norm of %g. \nInitial solution: %g %g %g %g %g %g %g %g %g %g %g %g %g \nFinal solution: %g %g %g %g %g %g %g %g %g %g %g %g %g\n",
+  PCL_DEBUG ("[pcl::SampleConsensusModelEllipse3D::optimizeModelCoefficients] LM solver finished with exit code %i, having a residual norm of %g. \nInitial solution: %g %g %g %g %g %g %g %g %g %g %g\nFinal solution: %g %g %g %g %g %g %g %g %g %g %g\n",
             info, lm.fvec.norm (),
 
             model_coefficients[0],

sample_consensus/include/pcl/sample_consensus/sac_model_circle3d.h

@@ -65,6 +65,7 @@ namespace pcl
       using SampleConsensusModel<PointT>::indices_;
       using SampleConsensusModel<PointT>::radius_min_;
       using SampleConsensusModel<PointT>::radius_max_;
+      using SampleConsensusModel<PointT>::error_sqr_dists_;
 
       using PointCloud = typename SampleConsensusModel<PointT>::PointCloud;
       using PointCloudPtr = typename SampleConsensusModel<PointT>::PointCloudPtr;
@@ -232,18 +233,19 @@ namespace pcl
          */
         int operator() (const Eigen::VectorXd &x, Eigen::VectorXd &fvec) const
         {
+          // Same for all points, so define outside of loop:
+          // C : Circle Center
+          const Eigen::Vector3d C (x[0], x[1], x[2]);
+          // N : Circle (Plane) Normal
+          const Eigen::Vector3d N (x[4], x[5], x[6]);
+          // r : Radius
+          const double r = x[3];
           for (int i = 0; i < values (); ++i)
           {
             // what i have:
             // P : Sample Point
             Eigen::Vector3d P =
                 (*model_->input_)[indices_[i]].getVector3fMap().template cast<double>();
-            // C : Circle Center
-            Eigen::Vector3d C (x[0], x[1], x[2]);
-            // N : Circle (Plane) Normal
-            Eigen::Vector3d N (x[4], x[5], x[6]);
-            // r : Radius
-            double r = x[3];
 
             Eigen::Vector3d helperVectorPC = P - C;
             // 1.1. get line parameter

sample_consensus/include/pcl/sample_consensus/sac_model_ellipse3d.h

@@ -46,6 +46,7 @@ namespace pcl
       using SampleConsensusModel<PointT>::indices_;
       using SampleConsensusModel<PointT>::radius_min_;
       using SampleConsensusModel<PointT>::radius_max_;
+      using SampleConsensusModel<PointT>::error_sqr_dists_;
 
       using PointCloud = typename SampleConsensusModel<PointT>::PointCloud;
       using PointCloudPtr = typename SampleConsensusModel<PointT>::PointCloudPtr;

tools/pcd_viewer.cpp

@@ -513,9 +513,6 @@ main (int argc, char** argv)
       if (useEDLRendering)
         p->enableEDLRendering();
 
-      // Set whether or not we should be using the vtkVertexBufferObjectMapper
-      p->setUseVbos (use_vbos);
-
       if (!p->cameraParamsSet () && !p->cameraFileLoaded ())
       {
         Eigen::Matrix3f rotation;

visualization/include/pcl/visualization/interactor_style.h

@@ -155,6 +155,7 @@ namespace pcl
           * buffer objects by default, transparently for the user.
           * \param[in] use_vbos set to true to use VBOs
           */
+        PCL_DEPRECATED(1, 18, "this function has no effect")
         inline void
         setUseVbos (const bool use_vbos) { use_vbos_ = use_vbos; }
 

visualization/include/pcl/visualization/pcl_visualizer.h

@@ -1961,6 +1961,7 @@ namespace pcl
           * buffer objects by default, transparently for the user.
           * \param[in] use_vbos set to true to use VBOs
           */
+        PCL_DEPRECATED(1, 18, "this function has no effect")
         void
         setUseVbos (bool use_vbos);