From 7f49dedde3644cfefa71f5c8d68f8974c2e278ab Mon Sep 17 00:00:00 2001 From: Kunal Tyagi Date: Thu, 9 Jan 2020 16:13:08 +0900 Subject: [PATCH 1/8] board.hpp: Code improvement via auto and modern for loops --- features/include/pcl/features/impl/board.hpp | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/features/include/pcl/features/impl/board.hpp b/features/include/pcl/features/impl/board.hpp index de67e9b8b12..da4f54caeb5 100644 --- a/features/include/pcl/features/impl/board.hpp +++ b/features/include/pcl/features/impl/board.hpp @@ -138,7 +138,7 @@ pcl::BOARDLocalReferenceFrameEstimation::planeFitt // Plane Fitting using Singular Value Decomposition (SVD) // ----------------------------------------------------- - int n_points = static_cast (points.rows ()); + const auto n_points = points.rows (); if (n_points == 0) { return; @@ -254,9 +254,8 @@ pcl::BOARDLocalReferenceFrameEstimation::computePo Eigen::Vector3f best_margin_point; bool best_point_found_on_margins = false; - float radius2 = tangent_radius_ * tangent_radius_; - - float margin_distance2 = margin_thresh_ * margin_thresh_ * radius2; + const float radius2 = tangent_radius_ * tangent_radius_; + const float margin_distance2 = margin_thresh_ * margin_thresh_ * radius2; float max_boundary_angle = 0; @@ -400,9 +399,9 @@ pcl::BOARDLocalReferenceFrameEstimation::computePo //check if there is at least a hole bool is_hole_present = false; - for (int i = 0; i < check_margin_array_size_; i++) + for (const auto check_margin: check_margin_array_) { - if (!check_margin_array_[i]) + if (!check_margin) { is_hole_present = true; break; @@ -471,7 +470,7 @@ pcl::BOARDLocalReferenceFrameEstimation::computePo float max_hole_prob = -std::numeric_limits::max (); //find holes - for (int ch = first_no_border; ch < check_margin_array_size_; ch++) + for (auto ch = first_no_border; ch < check_margin_array_size_; ch++) { if (!check_margin_array_[ch]) { From 97bdd2ba42e413f0720ed561dc912fa1d567a0fb Mon Sep 17 00:00:00 2001 From: Kunal Tyagi Date: Thu, 9 Jan 2020 16:13:08 +0900 Subject: [PATCH 2/8] brisk_2d.hpp: Code improvement via auto and modern for loops --- features/include/pcl/features/impl/brisk_2d.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/features/include/pcl/features/impl/brisk_2d.hpp b/features/include/pcl/features/impl/brisk_2d.hpp index 285ed86b1d7..9c12de9278c 100644 --- a/features/include/pcl/features/impl/brisk_2d.hpp +++ b/features/include/pcl/features/impl/brisk_2d.hpp @@ -103,11 +103,11 @@ pcl::BRISK2DEstimation::generateKern d_min_ = d_min; // get the total number of points - const int rings = int (radius_list.size ()); + const auto rings = radius_list.size (); assert (radius_list.size () != 0 && radius_list.size () == number_list.size ()); points_ = 0; // remember the total number of points - for (int ring = 0; ring < rings; ring++) - points_ += number_list[ring]; + for (const auto number: number_list) + points_ += number; // set up the patterns pattern_points_ = new BriskPatternPoint[points_*scales_*n_rot_]; From b5247468c32c8a6c5025825beb6d2b75db01ec0b Mon Sep 17 00:00:00 2001 From: Kunal Tyagi Date: Thu, 9 Jan 2020 16:13:08 +0900 Subject: [PATCH 3/8] cvfh.h: Code improvement via auto and modern for loops --- features/include/pcl/features/cvfh.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/features/include/pcl/features/cvfh.h b/features/include/pcl/features/cvfh.h index edc63c41661..7788f0bda65 100644 --- a/features/include/pcl/features/cvfh.h +++ b/features/include/pcl/features/cvfh.h @@ -147,8 +147,8 @@ namespace pcl inline void getCentroidClusters (std::vector > & centroids) { - for (std::size_t i = 0; i < centroids_dominant_orientations_.size (); ++i) - centroids.push_back (centroids_dominant_orientations_[i]); + for (const auto& centroid: centroids_dominant_orientations_) + centroids.push_back (centroid); } /** \brief Get the normal centroids used to compute different CVFH descriptors @@ -157,8 +157,8 @@ namespace pcl inline void getCentroidNormalClusters (std::vector > & centroids) { - for (std::size_t i = 0; i < dominant_normals_.size (); ++i) - centroids.push_back (dominant_normals_[i]); + for (const auto& normal: dominant_normals_) + centroids.push_back (normal); } /** \brief Sets max. Euclidean distance between points to be added to the cluster From 0d4159ea861d0b560c9ab670cb9a2989ebfad032 Mon Sep 17 00:00:00 2001 From: Kunal Tyagi Date: Thu, 9 Jan 2020 16:13:08 +0900 Subject: [PATCH 4/8] cppf.hpp: Code improvement via auto and modern for loops --- features/include/pcl/features/impl/cppf.hpp | 19 +++++++++---------- 1 file changed, 9 insertions(+), 10 deletions(-) diff --git a/features/include/pcl/features/impl/cppf.hpp b/features/include/pcl/features/impl/cppf.hpp index 74324e009f9..74782d4f86d 100755 --- a/features/include/pcl/features/impl/cppf.hpp +++ b/features/include/pcl/features/impl/cppf.hpp @@ -60,19 +60,17 @@ pcl::CPPFEstimation::CPPFEstimation () template void pcl::CPPFEstimation::computeFeature (PointCloudOut &output) { - // Initialize output container - overwrite the sizes done by Feature::initCompute () - output.points.resize (indices_->size () * input_->points.size ()); - output.height = 1; - output.width = static_cast (output.points.size ()); + // Initialize output container + output.points.clear (); + output.points.reserve (indices_->size () * input_->points.size ()); output.is_dense = true; - // Compute point pair features for every pair of points in the cloud - for (std::size_t index_i = 0; index_i < indices_->size (); ++index_i) + for (const auto& i: *indices_) { - std::size_t i = (*indices_)[index_i]; for (std::size_t j = 0 ; j < input_->points.size (); ++j) { PointOutT p; + // No need to calculate feature for identity pair (i, j) as they aren't used in future calculations if (i != j) { if ( @@ -105,17 +103,18 @@ pcl::CPPFEstimation::computeFeature (PointCloudOut output.is_dense = false; } } - // Do not calculate the feature for identity pairs (i, i) as they are not used - // in the following computations else { p.f1 = p.f2 = p.f3 = p.f4 = p.f5 = p.f6 = p.f7 = p.f8 = p.f9 = p.f10 = p.alpha_m = std::numeric_limits::quiet_NaN (); output.is_dense = false; } - output.points[index_i*input_->points.size () + j] = p; + output.points.push_back (p); } } + // overwrite the sizes done by Feature::initCompute () + output.height = 1; + output.width = static_cast (output.points.size ()); } #define PCL_INSTANTIATE_CPPFEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::CPPFEstimation; From 655f59a071e32838090466aa2995127ed6844bc6 Mon Sep 17 00:00:00 2001 From: Kunal Tyagi Date: Thu, 9 Jan 2020 16:13:08 +0900 Subject: [PATCH 5/8] crh.hpp: Code improvement via auto and modern for loops --- features/include/pcl/features/impl/crh.hpp | 29 +++++++++++----------- 1 file changed, 15 insertions(+), 14 deletions(-) diff --git a/features/include/pcl/features/impl/crh.hpp b/features/include/pcl/features/impl/crh.hpp index 8bdedd010e5..2bbb21fe0d5 100644 --- a/features/include/pcl/features/impl/crh.hpp +++ b/features/include/pcl/features/impl/crh.hpp @@ -96,8 +96,7 @@ pcl::CRHEstimation::computeFeature (PointCloudOut //fill spatial data vector and the zero-initialize or "value-initialize" an array on c++, // the initialization is made with () after the [nbins] - kiss_fft_scalar * spatial_data = new kiss_fft_scalar[nbins](); - + std::vector spatial_data(nbins); float sum_w = 0, w = 0; int bin = 0; @@ -109,29 +108,31 @@ pcl::CRHEstimation::computeFeature (PointCloudOut spatial_data[bin] += w; } - for (int i = 0; i < nbins; ++i) - spatial_data[i] /= sum_w; + for (auto& data: spatial_data) + data /= sum_w; - kiss_fft_cpx * freq_data = new kiss_fft_cpx[nbins / 2 + 1]; + std::vector freq_data(nbins / 2 + 1); kiss_fftr_cfg mycfg = kiss_fftr_alloc (nbins, 0, nullptr, nullptr); - kiss_fftr (mycfg, spatial_data, freq_data); + kiss_fftr (mycfg, spatial_data.data (), freq_data.data ()); + + for (auto& data: freq_data) + { + data.r /= freq_data[0].r; + data.i /= freq_data[0].r; + } output.points.resize (1); output.width = output.height = 1; - output.points[0].histogram[0] = freq_data[0].r / freq_data[0].r; //dc + output.points[0].histogram[0] = freq_data[0].r; //dc int k = 1; for (int i = 1; i < (nbins / 2); i++, k += 2) { - output.points[0].histogram[k] = freq_data[i].r / freq_data[0].r; - output.points[0].histogram[k + 1] = freq_data[i].i / freq_data[0].r; + output.points[0].histogram[k] = freq_data[i].r; + output.points[0].histogram[k + 1] = freq_data[i].i; } - output.points[0].histogram[nbins - 1] = freq_data[nbins / 2].r / freq_data[0].r; //nyquist - - delete[] spatial_data; - delete[] freq_data; - + output.points[0].histogram[nbins - 1] = freq_data[nbins / 2].r; //nyquist } #define PCL_INSTANTIATE_CRHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::CRHEstimation; From bc55a20c4a635b9cef9647f1adf6f11d702cf6ac Mon Sep 17 00:00:00 2001 From: Kunal Tyagi Date: Thu, 9 Jan 2020 16:13:08 +0900 Subject: [PATCH 6/8] cvfh.hpp: Code improvement via auto and modern for loops --- features/include/pcl/features/impl/cvfh.hpp | 41 +++++++++------------ 1 file changed, 17 insertions(+), 24 deletions(-) diff --git a/features/include/pcl/features/impl/cvfh.hpp b/features/include/pcl/features/impl/cvfh.hpp index b8abc063cc5..499d78e8157 100644 --- a/features/include/pcl/features/impl/cvfh.hpp +++ b/features/include/pcl/features/impl/cvfh.hpp @@ -102,57 +102,50 @@ pcl::CVFHEstimation::extractEuclideanClustersSmoot { if (processed[i]) continue; + processed[i] = true; - std::vector seed_queue; - std::size_t sq_idx = 0; - seed_queue.push_back (i); + pcl::PointIndices r; + r.header = cloud.header; + auto& seed_queue = r.indices; - processed[i] = true; + seed_queue.push_back (i); - while (sq_idx < seed_queue.size ()) + // loop has an emplace_back, making it difficult to use modern loops + for (std::size_t idx = 0; idx != seed_queue.size (); ++idx) { - // Search for sq_idx - if (!tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances)) + // Search for seed_queue[index] + if (!tree->radiusSearch (seed_queue[idx], tolerance, nn_indices, nn_distances)) { - sq_idx++; continue; } - for (std::size_t j = 1; j < nn_indices.size (); ++j) // nn_indices[0] should be sq_idx + // skip index 0, since nn_indices[0] == idx, worth it? + for (std::size_t j = 1; j < nn_indices.size (); ++j) { if (processed[nn_indices[j]]) // Has this point been processed before ? continue; //processed[nn_indices[j]] = true; // [-1;1] + const double dot_p = normals.points[seed_queue[idx]].getNormalVector3fMap().dot( + normals.points[nn_indices[j]].getNormalVector3fMap()); - double dot_p = normals.points[seed_queue[sq_idx]].normal[0] * normals.points[nn_indices[j]].normal[0] - + normals.points[seed_queue[sq_idx]].normal[1] * normals.points[nn_indices[j]].normal[1] - + normals.points[seed_queue[sq_idx]].normal[2] * normals.points[nn_indices[j]].normal[2]; - - if (std::abs (std::acos (dot_p)) < eps_angle) + if (std::acos (dot_p) < eps_angle) { processed[nn_indices[j]] = true; - seed_queue.push_back (nn_indices[j]); + seed_queue.emplace_back (nn_indices[j]); } } - - sq_idx++; } // If this queue is satisfactory, add to the clusters if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster) { - pcl::PointIndices r; - r.indices.resize (seed_queue.size ()); - for (std::size_t j = 0; j < seed_queue.size (); ++j) - r.indices[j] = seed_queue[j]; - std::sort (r.indices.begin (), r.indices.end ()); r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ()); - r.header = cloud.header; - clusters.push_back (r); // We could avoid a copy by working directly in the vector + // Might be better to work directly in the cluster somehow + clusters.emplace_back (std::move(r)); // Trying to avoid a copy by moving } } } From e80e16619272121164824cf0a5fed185cd234b44 Mon Sep 17 00:00:00 2001 From: Kunal Tyagi Date: Thu, 9 Jan 2020 16:13:08 +0900 Subject: [PATCH 7/8] esf.hpp: Code improvement via auto and modern for loops --- features/include/pcl/features/impl/esf.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/features/include/pcl/features/impl/esf.hpp b/features/include/pcl/features/impl/esf.hpp index ac38bddd04d..93096f9aa3e 100644 --- a/features/include/pcl/features/impl/esf.hpp +++ b/features/include/pcl/features/impl/esf.hpp @@ -54,6 +54,7 @@ pcl::ESFEstimation::computeESF ( { const int binsize = 64; unsigned int sample_size = 20000; + // @TODO: Replace with c++ stdlib uniform_random_generator srand (static_cast (time (nullptr))); int maxindex = static_cast (pc.points.size ()); From ca463df6e0d62a88add0625f5d8dcd389e1747e8 Mon Sep 17 00:00:00 2001 From: Kunal Tyagi Date: Thu, 9 Jan 2020 16:13:08 +0900 Subject: [PATCH 8/8] from_meshes.h: Code improvement via auto and modern for loops --- features/include/pcl/features/from_meshes.h | 37 ++++++++++----------- 1 file changed, 18 insertions(+), 19 deletions(-) diff --git a/features/include/pcl/features/from_meshes.h b/features/include/pcl/features/from_meshes.h index 2d65b0f3e37..e4c25578731 100644 --- a/features/include/pcl/features/from_meshes.h +++ b/features/include/pcl/features/from_meshes.h @@ -16,34 +16,32 @@ namespace pcl template inline void computeApproximateNormals(const pcl::PointCloud& cloud, const std::vector& polygons, pcl::PointCloud& normals) { - int nr_points = static_cast(cloud.points.size()); - int nr_polygons = static_cast(polygons.size()); + const auto nr_points = cloud.points.size(); normals.header = cloud.header; normals.width = cloud.width; normals.height = cloud.height; normals.points.resize(nr_points); - for ( int i = 0; i < nr_points; ++i ) - normals.points[i].getNormalVector3fMap() = Eigen::Vector3f::Zero(); + for (auto& point: normals.points) + point.getNormalVector3fMap() = Eigen::Vector3f::Zero(); // NOTE: for efficiency the weight is computed implicitly by using the // cross product, this causes inaccurate normals for meshes containing // non-triangle polygons (quads or other types) - for ( int i = 0; i < nr_polygons; ++i ) + for (const auto& polygon: polygons) { - const int nr_points_polygon = (int)polygons[i].vertices.size(); - if (nr_points_polygon < 3) continue; + if (polygon.vertices.size() < 3) continue; // compute normal for triangle - Eigen::Vector3f vec_a_b = cloud.points[polygons[i].vertices[0]].getVector3fMap() - cloud.points[polygons[i].vertices[1]].getVector3fMap(); - Eigen::Vector3f vec_a_c = cloud.points[polygons[i].vertices[0]].getVector3fMap() - cloud.points[polygons[i].vertices[2]].getVector3fMap(); + Eigen::Vector3f vec_a_b = cloud.points[polygon.vertices[0]].getVector3fMap() - cloud.points[polygon.vertices[1]].getVector3fMap(); + Eigen::Vector3f vec_a_c = cloud.points[polygon.vertices[0]].getVector3fMap() - cloud.points[polygon.vertices[2]].getVector3fMap(); Eigen::Vector3f normal = vec_a_b.cross(vec_a_c); - pcl::flipNormalTowardsViewpoint(cloud.points[polygons[i].vertices[0]], 0.0f, 0.0f, 0.0f, normal(0), normal(1), normal(2)); + pcl::flipNormalTowardsViewpoint(cloud.points[polygon.vertices[0]], 0.0f, 0.0f, 0.0f, normal(0), normal(1), normal(2)); // add normal to all points in polygon - for ( int j = 0; j < nr_points_polygon; ++j ) - normals.points[polygons[i].vertices[j]].getNormalVector3fMap() += normal; + for (const auto& vertex: polygon.vertices) + normals.points[vertex].getNormalVector3fMap() += normal; } for (std::size_t i = 0; i < nr_points; ++i) @@ -70,12 +68,13 @@ namespace pcl assert(cloud.points.size() == normals.points.size()); const auto nr_points = cloud.points.size(); - covariances.resize(nr_points); - for (std::size_t i = 0; i < nr_points; ++i) + covariances.clear (); + covariances.reserve (nr_points); + for (const auto& point: normals.points) { - Eigen::Vector3d normal(normals.points[i].normal_x, - normals.points[i].normal_y, - normals.points[i].normal_z); + Eigen::Vector3d normal (normals.points[i].normal_x, + normals.points[i].normal_y, + normals.points[i].normal_z); // compute rotation matrix Eigen::Matrix3d rot; @@ -86,13 +85,13 @@ namespace pcl y.normalize(); rot.row(1) = y; rot.row(0) = normal.cross(rot.row(1)); - + // comnpute approximate covariance Eigen::Matrix3d cov; cov << 1, 0, 0, 0, 1, 0, 0, 0, epsilon; - covariances[i] = rot.transpose()*cov*rot; + covariances.emplace_back (rot.transpose()*cov*rot); } }