From 5f833b5e6e72243a4dc15b7fdd4b9beeb732d855 Mon Sep 17 00:00:00 2001 From: Robert Huitl Date: Fri, 15 Mar 2013 15:35:24 +0100 Subject: [PATCH 01/32] Fix bug 961: SACSegmentation crashes when setSamplesMaxDist() is used and a search object with indices is used --- .../include/pcl/sample_consensus/sac_model.h | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/sample_consensus/include/pcl/sample_consensus/sac_model.h b/sample_consensus/include/pcl/sample_consensus/sac_model.h index a0ee3cb156a..932a3ced1da 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model.h @@ -466,14 +466,19 @@ namespace pcl std::vector indices; std::vector sqr_dists; - samples_radius_search_->radiusSearch (shuffled_indices_[0], samples_radius_, - indices, sqr_dists ); + // If indices have been set when the search object was constructed, + // radiusSearch() expects an index into the indices vector as its + // first parameter. This can't be determined efficiently, so we use + // the point instead of the index. + // Returned indices are converted automatically. + samples_radius_search_->radiusSearch (input_->at(shuffled_indices_[0]), + samples_radius_, indices, sqr_dists ); if (indices.size () < sample_size - 1) { // radius search failed, make an invalid model for(unsigned int i = 1; i < sample_size; ++i) - shuffled_indices_[i] = shuffled_indices_[0]; + shuffled_indices_[i] = shuffled_indices_[0]; } else { From 13c4acede87a8ea33d8175398aad8b40b75a30e1 Mon Sep 17 00:00:00 2001 From: Robert Huitl Date: Fri, 15 Mar 2013 16:34:50 +0100 Subject: [PATCH 02/32] Implementation of an OpenMP-parallelized version of Moving Least Squares --- surface/include/pcl/surface/impl/mls.hpp | 110 ++++++++++++++++++--- surface/include/pcl/surface/mls.h | 83 ++++++++++++++-- surface/src/mls.cpp | 3 + test/surface/test_moving_least_squares.cpp | 4 +- 4 files changed, 178 insertions(+), 22 deletions(-) diff --git a/surface/include/pcl/surface/impl/mls.hpp b/surface/include/pcl/surface/impl/mls.hpp index b66614892c3..cef084cb296 100644 --- a/surface/include/pcl/surface/impl/mls.hpp +++ b/surface/include/pcl/surface/impl/mls.hpp @@ -47,12 +47,16 @@ #include #include +#ifdef _OPENMP +#include +#endif + ////////////////////////////////////////////////////////////////////////////////////////////// template void pcl::MovingLeastSquares::process (PointCloudOut &output) { // Reset or initialize the collection of indices - corresponding_input_indices_.reset(new PointIndices); + corresponding_input_indices_.reset (new PointIndices); // Check if normals have to be computed/saved if (compute_normals_) @@ -156,8 +160,13 @@ pcl::MovingLeastSquares::computeMLSPointNormal (int index, const std::vector &nn_indices, std::vector &nn_sqr_dists, PointCloudOut &projected_points, - NormalCloud &projected_points_normals) + NormalCloud &projected_points_normals, + PointIndices &corresponding_input_indices, + MLSResult &mls_result) const { + // Note: this method is const because it needs to be thread-safe + // (MovingLeastSquaresOMP calls it from multiple threads) + // Compute the plane coefficients EIGEN_ALIGN16 Eigen::Matrix3d covariance_matrix; Eigen::Vector4d xyz_centroid; @@ -285,7 +294,7 @@ pcl::MovingLeastSquares::computeMLSPointNormal (int index, aux_normal.normal_z = static_cast (normal[2]); aux_normal.curvature = curvature; projected_points_normals.push_back (aux_normal); - corresponding_input_indices_->indices.push_back (index); + corresponding_input_indices.indices.push_back (index); } break; @@ -306,7 +315,7 @@ pcl::MovingLeastSquares::computeMLSPointNormal (int index, projected_point, projected_normal); projected_points.push_back (projected_point); - corresponding_input_indices_->indices.push_back (index); + corresponding_input_indices.indices.push_back (index); if (compute_normals_) projected_points_normals.push_back (projected_normal); } @@ -336,7 +345,7 @@ pcl::MovingLeastSquares::computeMLSPointNormal (int index, aux.y = static_cast (point[1]); aux.z = static_cast (point[2]); projected_points.push_back (aux); - corresponding_input_indices_->indices.push_back (index); + corresponding_input_indices.indices.push_back (index); if (compute_normals_) { @@ -368,7 +377,7 @@ pcl::MovingLeastSquares::computeMLSPointNormal (int index, projected_point, projected_normal); projected_points.push_back (projected_point); - corresponding_input_indices_->indices.push_back (index); + corresponding_input_indices.indices.push_back (index); if (compute_normals_) projected_points_normals.push_back (projected_normal); @@ -383,8 +392,7 @@ pcl::MovingLeastSquares::computeMLSPointNormal (int index, { // Take all point pairs and sample space between them in a grid-fashion // \note consider only point pairs with increasing indices - MLSResult result (point, plane_normal, u_axis, v_axis, c_vec, static_cast (nn_indices.size ()), curvature); - mls_results_[index] = result; + mls_result = MLSResult (point, plane_normal, u_axis, v_axis, c_vec, static_cast (nn_indices.size ()), curvature); break; } } @@ -400,7 +408,7 @@ pcl::MovingLeastSquares::projectPointToMLSSurface (float &u Eigen::VectorXd &c_vec, int num_neighbors, PointOutT &result_point, - pcl::Normal &result_normal) + pcl::Normal &result_normal) const { double n_disp = 0.0f; double d_u = 0.0f, d_v = 0.0f; @@ -447,7 +455,6 @@ pcl::MovingLeastSquares::projectPointToMLSSurface (float &u result_normal.curvature = curvature; } - ////////////////////////////////////////////////////////////////////////////////////////////// template void pcl::MovingLeastSquares::performProcessing (PointCloudOut &output) @@ -477,7 +484,8 @@ pcl::MovingLeastSquares::performProcessing (PointCloudOut & PointCloudOut projected_points; NormalCloud projected_points_normals; // Get a plane approximating the local surface's tangent and project point onto it - computeMLSPointNormal ((*indices_)[cp], nn_indices, nn_sqr_dists, projected_points, projected_points_normals); + int index = (*indices_)[cp]; + computeMLSPointNormal (index, nn_indices, nn_sqr_dists, projected_points, projected_points_normals, *corresponding_input_indices_, mls_results_[index]); // Copy all information from the input cloud to the output points (not doing any interpolation) @@ -491,7 +499,81 @@ pcl::MovingLeastSquares::performProcessing (PointCloudOut & normals_->insert (normals_->end (), projected_points_normals.begin (), projected_points_normals.end ()); } + // Perform the distinct-cloud or voxel-grid upsampling + performUpsampling (output); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +#ifdef _OPENMP +template void +pcl::MovingLeastSquaresOMP::performProcessing (PointCloudOut &output) +{ + // Compute the number of coefficients + nr_coeff_ = (order_ + 1) * (order_ + 2) / 2; + + // (Maximum) number of threads + unsigned int threads = threads_ == 0 ? 1 : threads_; + + // Create temporaries for each thread in order to avoid synchronization + std::vector projected_points (threads); + std::vector projected_points_normals (threads); + std::vector corresponding_input_indices (threads); + + // For all points +#pragma omp parallel for schedule (dynamic,1000) num_threads (threads) + for (size_t cp = 0; cp < indices_->size (); ++cp) + { + // Allocate enough space to hold the results of nearest neighbor searches + // \note resize is irrelevant for a radiusSearch (). + std::vector nn_indices; + std::vector nn_sqr_dists; + + // Get the initial estimates of point positions and their neighborhoods + if (!searchForNeighbors ((*indices_)[cp], nn_indices, nn_sqr_dists)) + continue; + + + // Check the number of nearest neighbors for normal estimation (and later + // for polynomial fit as well) + if (nn_indices.size () < 3) + continue; + + + // This thread's ID (range 0 to threads-1) + int tn = omp_get_thread_num (); + + // Size of projected points before computeMLSPointNormal () adds points + size_t pp_size = projected_points[tn].size (); + + // Get a plane approximating the local surface's tangent and project point onto it + int index = (*indices_)[cp]; + computeMLSPointNormal (index, nn_indices, nn_sqr_dists, projected_points[tn], projected_points_normals[tn], corresponding_input_indices[tn], this->mls_results_[index]); + + // Copy all information from the input cloud to the output points (not doing any interpolation) + for (size_t pp = pp_size; pp < projected_points[tn].size (); ++pp) + copyMissingFields (input_->points[(*indices_)[cp]], projected_points[tn][pp]); + } + + // Combine all threads' results into the output vectors + for (unsigned int tn = 0; tn < threads; ++tn) + { + output.insert (output.end (), projected_points[tn].begin (), projected_points[tn].end ()); + corresponding_input_indices_->indices.insert (corresponding_input_indices_->indices.end (), + corresponding_input_indices[tn].indices.begin (), corresponding_input_indices[tn].indices.end ()); + if (compute_normals_) + normals_->insert (normals_->end (), projected_points_normals[tn].begin (), projected_points_normals[tn].end ()); + } + + // Perform the distinct-cloud or voxel-grid upsampling + performUpsampling (output); +} +#endif + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::MovingLeastSquares::performUpsampling (PointCloudOut &output) +{ if (upsample_method_ == DISTINCT_CLOUD) { for (size_t dp_i = 0; dp_i < distinct_cloud_->size (); ++dp_i) // dp_i = distinct_point_i @@ -588,7 +670,7 @@ pcl::MovingLeastSquares::performProcessing (PointCloudOut & copyMissingFields (input_->points[input_index], result_point); // Store the id of the original point - corresponding_input_indices_->indices.push_back(input_index); + corresponding_input_indices_->indices.push_back (input_index); output.push_back (result_point); @@ -687,4 +769,8 @@ pcl::MovingLeastSquares::copyMissingFields (const PointInT #define PCL_INSTANTIATE_MovingLeastSquares(T,OutT) template class PCL_EXPORTS pcl::MovingLeastSquares; +#ifdef _OPENMP +#define PCL_INSTANTIATE_MovingLeastSquaresOMP(T,OutT) template class PCL_EXPORTS pcl::MovingLeastSquaresOMP; +#endif + #endif // PCL_SURFACE_IMPL_MLS_H_ diff --git a/surface/include/pcl/surface/mls.h b/surface/include/pcl/surface/mls.h index 013f19401f1..9de582c2ced 100644 --- a/surface/include/pcl/surface/mls.h +++ b/surface/include/pcl/surface/mls.h @@ -342,7 +342,7 @@ namespace pcl /** \brief Data structure used to store the results of the MLS fitting - * \note Used only in the case of VOXEL_GRID_DILATION upsampling + * \note Used only in the case of VOXEL_GRID_DILATION or DISTINCT_CLOUD upsampling */ struct MLSResult { @@ -364,7 +364,7 @@ namespace pcl }; /** \brief Stores the MLS result for each point in the input cloud - * \note Used only in the case of VOXEL_GRID_DILATION upsampling + * \note Used only in the case of VOXEL_GRID_DILATION or DISTINCT_CLOUD upsampling */ std::vector mls_results_; @@ -434,13 +434,16 @@ namespace pcl /** \brief Number of coefficients, to be computed from the requested order.*/ int nr_coeff_; + /** \brief Collects for each point in output the corrseponding point in the input. */ + PointIndicesPtr corresponding_input_indices_; + /** \brief Search for the closest nearest neighbors of a given point using a radius search * \param[in] index the index of the query point * \param[out] indices the resultant vector of indices representing the k-nearest neighbors * \param[out] sqr_distances the resultant squared distances from the query point to the k-nearest neighbors */ inline int - searchForNeighbors (int index, std::vector &indices, std::vector &sqr_distances) + searchForNeighbors (int index, std::vector &indices, std::vector &sqr_distances) const { return (search_method_ (index, search_radius_, indices, sqr_distances)); } @@ -453,13 +456,18 @@ namespace pcl * (in the case of upsampling method NONE, only the query point projected to its own fitted surface will be returned, * in the case of the other upsampling methods, multiple points will be returned) * \param[out] projected_points_normals the normals corresponding to the projected points + * \param[out] corresponding_input_indices the set of indices with each point in output having the corresponding point in input + * \param[out] mls_result stores the MLS result for each point in the input cloud + * (used only in the case of VOXEL_GRID_DILATION or DISTINCT_CLOUD upsampling) */ void computeMLSPointNormal (int index, const std::vector &nn_indices, std::vector &nn_sqr_dists, PointCloudOut &projected_points, - NormalCloud &projected_points_normals); + NormalCloud &projected_points_normals, + PointIndices &corresponding_input_indices, + MLSResult &mls_result) const; /** \brief Fits a point (sample point) given in the local plane coordinates of an input point (query point) to * the MLS surface of the input point @@ -484,21 +492,23 @@ namespace pcl Eigen::VectorXd &c_vec, int num_neighbors, PointOutT &result_point, - pcl::Normal &result_normal); + pcl::Normal &result_normal) const; void copyMissingFields (const PointInT &point_in, PointOutT &point_out) const; - private: /** \brief Abstract surface reconstruction method. * \param[out] output the result of the reconstruction */ virtual void performProcessing (PointCloudOut &output); - /** \brief Collects for each point in output the corrseponding point in the input. */ - PointIndicesPtr corresponding_input_indices_; + /** \brief Perform upsampling for the distinct-cloud and voxel-grid methods + * \param[out] output the result of the reconstruction + */ + void performUpsampling (PointCloudOut &output); + private: /** \brief Boost-based random number generator algorithm. */ boost::mt19937 rng_alg_; @@ -515,6 +525,63 @@ namespace pcl public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; + +#ifdef _OPENMP + /** \brief MovingLeastSquaresOMP is a parallelized version of MovingLeastSquares, using the OpenMP standard. + * \note Compared to MovingLeastSquares, an overhead is incurred in terms of runtime and memory usage. + * \note The upsampling methods DISTINCT_CLOUD and VOXEL_GRID_DILATION are not parallelized completely, i.e. parts of the algorithm run on a single thread only. + * \author Robert Huitl + * \ingroup surface + */ + template + class MovingLeastSquaresOMP: public MovingLeastSquares + { + public: + typedef boost::shared_ptr > Ptr; + typedef boost::shared_ptr > ConstPtr; + + using PCLBase::input_; + using PCLBase::indices_; + using MovingLeastSquares::normals_; + using MovingLeastSquares::corresponding_input_indices_; + using MovingLeastSquares::nr_coeff_; + using MovingLeastSquares::order_; + using MovingLeastSquares::compute_normals_; + + typedef pcl::PointCloud NormalCloud; + typedef pcl::PointCloud::Ptr NormalCloudPtr; + + typedef pcl::PointCloud PointCloudOut; + typedef typename PointCloudOut::Ptr PointCloudOutPtr; + typedef typename PointCloudOut::ConstPtr PointCloudOutConstPtr; + + /** \brief Constructor for parallelized Moving Least Squares + * \param threads the maximum number of hardware threads to use (0 sets the value to 1) + */ + MovingLeastSquaresOMP (unsigned int threads = 0) : threads_ (threads) + { + + } + + /** \brief Set the maximum number of threads to use + * \param threads the maximum number of hardware threads to use (0 sets the value to 1) + */ + inline void + setNumberOfThreads (unsigned int threads = 0) + { + threads_ = threads; + } + + protected: + /** \brief Abstract surface reconstruction method. + * \param[out] output the result of the reconstruction + */ + virtual void performProcessing (PointCloudOut &output); + + /** \brief The maximum number of threads the scheduler should use. */ + unsigned int threads_; + }; +#endif } #ifdef PCL_NO_PRECOMPILE diff --git a/surface/src/mls.cpp b/surface/src/mls.cpp index 4f7aab993b2..65c5340c91b 100644 --- a/surface/src/mls.cpp +++ b/surface/src/mls.cpp @@ -46,5 +46,8 @@ PCL_INSTANTIATE_PRODUCT(MovingLeastSquares, ((pcl::PointXYZ)(pcl::PointXYZRGB)(pcl::PointXYZRGBA)) ((pcl::PointXYZ)(pcl::PointXYZRGB)(pcl::PointXYZRGBA)(pcl::PointXYZRGBNormal)(pcl::PointNormal))) +PCL_INSTANTIATE_PRODUCT(MovingLeastSquaresOMP, ((pcl::PointXYZ)(pcl::PointXYZRGB)(pcl::PointXYZRGBA)) + ((pcl::PointXYZ)(pcl::PointXYZRGB)(pcl::PointXYZRGBA)(pcl::PointXYZRGBNormal)(pcl::PointNormal))) + /// Ideally, we should instantiate like below, but it takes large amounts of main memory for compilation //PCL_INSTANTIATE_PRODUCT(MovingLeastSquares, (PCL_XYZ_POINT_TYPES)(PCL_XYZ_POINT_TYPES)) diff --git a/test/surface/test_moving_least_squares.cpp b/test/surface/test_moving_least_squares.cpp index 2a887ef80df..71d7e8e0a83 100644 --- a/test/surface/test_moving_least_squares.cpp +++ b/test/surface/test_moving_least_squares.cpp @@ -89,13 +89,13 @@ TEST (PCL, MovingLeastSquares) // Testing OpenMP version - MovingLeastSquares mls_omp; + MovingLeastSquaresOMP mls_omp; mls_omp.setInputCloud (cloud); mls_omp.setComputeNormals (true); mls_omp.setPolynomialFit (true); mls_omp.setSearchMethod (tree); mls_omp.setSearchRadius (0.03); - //mls_omp.setNumberOfThreads (4); + mls_omp.setNumberOfThreads (4); // Reconstruct mls_normals->clear (); From 7caaa57c9b5bb1a018c966ee8f7e2200aef7b33b Mon Sep 17 00:00:00 2001 From: Robert Huitl Date: Fri, 15 Mar 2013 16:15:15 +0100 Subject: [PATCH 03/32] Fix compilation with Boost 1.46.1: boost::mt19937 instead of boost::random::mt19937 --- filters/include/pcl/filters/random_sample.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/filters/include/pcl/filters/random_sample.h b/filters/include/pcl/filters/random_sample.h index 1e126e1ce43..d04ea62dbab 100644 --- a/filters/include/pcl/filters/random_sample.h +++ b/filters/include/pcl/filters/random_sample.h @@ -134,7 +134,7 @@ namespace pcl private: /** \brief Boost-based random number generator algorithm. */ - boost::random::mt19937 rng_alg_; + boost::mt19937 rng_alg_; /** \brief Boost-based random number generator distribution. */ boost::shared_ptr > rng_; @@ -218,7 +218,7 @@ namespace pcl private: /** \brief Boost-based random number generator algorithm. */ - boost::random::mt19937 rng_alg_; + boost::mt19937 rng_alg_; /** \brief Boost-based random number generator distribution. */ boost::shared_ptr > rng_; From 11cd99b5b946d0e8b44558925249f7a6bb8c7b83 Mon Sep 17 00:00:00 2001 From: "Radu B. Rusu" Date: Sun, 17 Mar 2013 14:46:25 -0700 Subject: [PATCH 04/32] added -neg command line option for @pcl_sac_segmentation_plane@ to return the largest cluster minus the plane (if activated) --- tools/CMakeLists.txt | 2 +- tools/sac_segmentation_plane.cpp | 58 ++++++++++++++++++++++++++------ 2 files changed, 49 insertions(+), 11 deletions(-) diff --git a/tools/CMakeLists.txt b/tools/CMakeLists.txt index eb23bfd640b..9d6ffba2057 100644 --- a/tools/CMakeLists.txt +++ b/tools/CMakeLists.txt @@ -10,7 +10,7 @@ PCL_SUBSYS_DEPEND (build ${SUBSYS_NAME} DEPS ${SUBSYS_DEPS}) if (build) PCL_ADD_EXECUTABLE(pcl_sac_segmentation_plane ${SUBSYS_NAME} sac_segmentation_plane.cpp) - target_link_libraries(pcl_sac_segmentation_plane pcl_common pcl_io pcl_sample_consensus) + target_link_libraries(pcl_sac_segmentation_plane pcl_common pcl_io pcl_sample_consensus pcl_segmentation) PCL_ADD_EXECUTABLE (pcl_plane_projection ${SUBSYS_NAME} plane_projection.cpp) target_link_libraries (pcl_plane_projection pcl_common pcl_io pcl_sample_consensus) diff --git a/tools/sac_segmentation_plane.cpp b/tools/sac_segmentation_plane.cpp index 1dc90ae0985..ea2b717016d 100644 --- a/tools/sac_segmentation_plane.cpp +++ b/tools/sac_segmentation_plane.cpp @@ -39,6 +39,7 @@ #include #include #include +#include #include #include #include @@ -50,6 +51,7 @@ using namespace pcl::console; int default_max_iterations = 1000; double default_threshold = 0.05; +bool default_negative = false; Eigen::Vector4f translation; Eigen::Quaternionf orientation; @@ -63,6 +65,8 @@ printHelp (int, char **argv) print_value ("%g", default_threshold); print_info (")\n"); print_info (" -max_it X = set the maximum number of RANSAC iterations to X (default: "); print_value ("%d", default_max_iterations); print_info (")\n"); + print_info (" -neg 0/1 = if true (1), instead of the plane, it returns the largest cluster on top of the plane (default: "); + print_value ("%s", default_negative ? "true" : "false"); print_info (")\n"); print_info ("\nOptional arguments are:\n"); print_info (" -input_dir X = batch process all PCD files found in input_dir\n"); print_info (" -output_dir X = save the processed files from input_dir in this directory\n"); @@ -85,7 +89,7 @@ loadCloud (const string &filename, sensor_msgs::PointCloud2 &cloud) void compute (const sensor_msgs::PointCloud2::ConstPtr &input, sensor_msgs::PointCloud2 &output, - int max_iterations = 1000, double threshold = 0.05) + int max_iterations = 1000, double threshold = 0.05, bool negative = false) { // Convert data to PointCloud PointCloud::Ptr xyz (new PointCloud); @@ -123,9 +127,38 @@ compute (const sensor_msgs::PointCloud2::ConstPtr &input, sensor_msgs::PointClou print_info ("Model coefficients: ["); print_value ("%g %g %g %g", coefficients[0], coefficients[1], coefficients[2], coefficients[3]); print_info ("]\n"); - // Convert data back - PointCloud output_inliers; - copyPointCloud (*input, inliers, output); + // Instead of returning the planar model as a set of inliers, return the outliers, but perform a cluster segmentation first + if (negative) + { + // Remove the plane indices from the data + PointIndices::Ptr everything_but_the_plane (new PointIndices); + std::vector indices_fullset (xyz->size ()); + for (int p_it = 0; p_it < static_cast (indices_fullset.size ()); ++p_it) + indices_fullset[p_it] = p_it; + + std::sort (inliers.begin (), inliers.end ()); + set_difference (indices_fullset.begin (), indices_fullset.end (), + inliers.begin (), inliers.end (), + inserter (everything_but_the_plane->indices, everything_but_the_plane->indices.begin ())); + + // Extract largest cluster minus the plane + vector cluster_indices; + EuclideanClusterExtraction ec; + ec.setClusterTolerance (0.02); // 2cm + ec.setMinClusterSize (100); + ec.setInputCloud (xyz); + ec.setIndices (everything_but_the_plane); + ec.extract (cluster_indices); + + // Convert data back + copyPointCloud (*input, cluster_indices[0].indices, output); + } + else + { + // Convert data back + PointCloud output_inliers; + copyPointCloud (*input, inliers, output); + } } void @@ -143,7 +176,7 @@ saveCloud (const string &filename, const sensor_msgs::PointCloud2 &output) } int -batchProcess (const vector &pcd_files, string &output_dir, int max_it, double thresh) +batchProcess (const vector &pcd_files, string &output_dir, int max_it, double thresh, bool negative) { vector st; for (size_t i = 0; i < pcd_files.size (); ++i) @@ -155,7 +188,7 @@ batchProcess (const vector &pcd_files, string &output_dir, int max_it, d // Perform the feature estimation sensor_msgs::PointCloud2 output; - compute (cloud, output, max_it, thresh); + compute (cloud, output, max_it, thresh, negative); // Prepare output file name string filename = pcd_files[i]; @@ -183,11 +216,11 @@ main (int argc, char** argv) } bool debug = false; - pcl::console::parse_argument (argc, argv, "-debug", debug); + console::parse_argument (argc, argv, "-debug", debug); if (debug) { print_highlight ("Enabling debug mode.\n"); - pcl::console::setVerbosityLevel (pcl::console::L_DEBUG); + console::setVerbosityLevel (console::L_DEBUG); if (!isVerbosityLevelEnabled (L_DEBUG)) PCL_ERROR ("Error enabling debug mode.\n"); } @@ -197,8 +230,10 @@ main (int argc, char** argv) // Command line parsing int max_it = default_max_iterations; double thresh = default_threshold; + bool negative = default_negative; parse_argument (argc, argv, "-max_it", max_it); parse_argument (argc, argv, "-thresh", thresh); + parse_argument (argc, argv, "-neg", negative); string input_dir, output_dir; if (parse_argument (argc, argv, "-input_dir", input_dir) != -1) { @@ -227,6 +262,9 @@ main (int argc, char** argv) print_info ("Estimating planes with a threshold of: "); print_value ("%g\n", thresh); + print_info ("Planar model segmentation: "); + print_value ("%s\n", negative ? "false" : "true"); + // Load the first file sensor_msgs::PointCloud2::Ptr cloud (new sensor_msgs::PointCloud2); if (!loadCloud (argv[p_file_indices[0]], *cloud)) @@ -234,7 +272,7 @@ main (int argc, char** argv) // Perform the feature estimation sensor_msgs::PointCloud2 output; - compute (cloud, output, max_it, thresh); + compute (cloud, output, max_it, thresh, negative); // Save into the second file saveCloud (argv[p_file_indices[1]], output); @@ -254,7 +292,7 @@ main (int argc, char** argv) PCL_INFO ("[Batch processing mode] Added %s for processing.\n", itr->path ().string ().c_str ()); } } - batchProcess (pcd_files, output_dir, max_it, thresh); + batchProcess (pcd_files, output_dir, max_it, thresh, negative); } else { From 4a8f9ad44e785f83c6a9c7b477f59584420dc357 Mon Sep 17 00:00:00 2001 From: Anatoly Baksheev Date: Mon, 18 Mar 2013 14:38:52 +0400 Subject: [PATCH 05/32] compilation for VS2008 --- io/include/pcl/io/openni_camera/openni_device.h | 9 +++++---- io/src/openni_camera/openni_device.cpp | 10 +++++----- 2 files changed, 10 insertions(+), 9 deletions(-) diff --git a/io/include/pcl/io/openni_camera/openni_device.h b/io/include/pcl/io/openni_camera/openni_device.h index 54aaafea0fa..6821e958dd2 100644 --- a/io/include/pcl/io/openni_camera/openni_device.h +++ b/io/include/pcl/io/openni_camera/openni_device.h @@ -49,6 +49,7 @@ #include #include + /// @todo Get rid of all exception-specifications, these are useless and soon to be deprecated #ifndef _WIN32 @@ -418,12 +419,12 @@ namespace openni_wrapper /** \brief Convert shift to depth value. */ - uint16_t - shiftToDepth (uint16_t shift_value) const + pcl::uint16_t + shiftToDepth (pcl::uint16_t shift_value) const { assert (shift_conversion_parameters_.init_); - uint16_t ret = 0; + pcl::uint16_t ret = 0; // lookup depth value in shift lookup table if (shift_value shift_to_depth_table_; + std::vector shift_to_depth_table_; // holds the callback functions together with custom data // since same callback function can be registered multiple times with e.g. different custom data diff --git a/io/src/openni_camera/openni_device.cpp b/io/src/openni_camera/openni_device.cpp index 100893fb68f..38014b3754f 100644 --- a/io/src/openni_camera/openni_device.cpp +++ b/io/src/openni_camera/openni_device.cpp @@ -418,15 +418,15 @@ void openni_wrapper::OpenNIDevice::InitShiftToDepthConversion () { // Calculate shift conversion table - int32_t nIndex = 0; - int32_t nShiftValue = 0; + pcl::int32_t nIndex = 0; + pcl::int32_t nShiftValue = 0; double dFixedRefX = 0; double dMetric = 0; double dDepth = 0; double dPlanePixelSize = shift_conversion_parameters_.zero_plane_pixel_size_; double dPlaneDsr = shift_conversion_parameters_.zero_plane_distance_; double dPlaneDcl = shift_conversion_parameters_.emitter_dcmos_distace_; - int32_t nConstShift = shift_conversion_parameters_.param_coeff_ * + pcl::int32_t nConstShift = shift_conversion_parameters_.param_coeff_ * shift_conversion_parameters_.const_shift_; dPlanePixelSize *= shift_conversion_parameters_.pixel_size_factor_; @@ -436,7 +436,7 @@ void openni_wrapper::OpenNIDevice::InitShiftToDepthConversion () for (nIndex = 1; nIndex < shift_conversion_parameters_.device_max_shift_; nIndex++) { - nShiftValue = (int32_t)nIndex; + nShiftValue = (pcl::int32_t)nIndex; dFixedRefX = (double) (nShiftValue - nConstShift) / (double) shift_conversion_parameters_.param_coeff_; @@ -449,7 +449,7 @@ void openni_wrapper::OpenNIDevice::InitShiftToDepthConversion () if ((dDepth > shift_conversion_parameters_.min_depth_) && (dDepth < shift_conversion_parameters_.max_depth_)) { - shift_to_depth_table_[nIndex] = (uint16_t)(dDepth); + shift_to_depth_table_[nIndex] = (pcl::uint16_t)(dDepth); } } From 9b1d1b09da840d41546b1a6ce7a62b3e24d0fc8f Mon Sep 17 00:00:00 2001 From: Koen_Buys Date: Mon, 18 Mar 2013 13:47:40 +0100 Subject: [PATCH 06/32] added PCD video player --- apps/CMakeLists.txt | 6 + .../src/pcd_video_player/pcd_video_player.cpp | 322 ++++++++++++++++++ apps/src/pcd_video_player/pcd_video_player.ui | 155 +++++++++ 3 files changed, 483 insertions(+) create mode 100644 apps/src/pcd_video_player/pcd_video_player.cpp create mode 100644 apps/src/pcd_video_player/pcd_video_player.ui diff --git a/apps/CMakeLists.txt b/apps/CMakeLists.txt index 5e70e4c8423..5f231b2fb96 100644 --- a/apps/CMakeLists.txt +++ b/apps/CMakeLists.txt @@ -162,6 +162,12 @@ if(build) PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_manual_registration ${SUBSYS_NAME} ${manual_registration_ui} ${manual_registration_moc} src/manual_registration/manual_registration.cpp) target_link_libraries(pcl_manual_registration pcl_common pcl_io pcl_visualization pcl_segmentation pcl_features pcl_surface QVTK ${QT_LIBRARIES}) + # Manual registration demo + QT4_WRAP_UI(pcd_video_player_ui src/pcd_video_player/pcd_video_player.ui) + QT4_WRAP_CPP(pcd_video_player_moc include/pcl/apps/pcd_video_player.h OPTIONS -DBOOST_TT_HAS_OPERATOR_HPP_INCLUDED) + PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_pcd_video_player ${SUBSYS_NAME} ${pcd_video_player_ui} ${pcd_video_player_moc} src/pcd_video_player/pcd_video_player.cpp) + target_link_libraries(pcl_pcd_video_player pcl_common pcl_io pcl_visualization pcl_segmentation pcl_features pcl_surface QVTK ${QT_LIBRARIES}) + # Database processing (integration) demo # QT4_WRAP_UI(db_proc_ui src/db_proc/db_proc.ui) # QT4_WRAP_CPP(db_proc_moc include/pcl/apps/db_proc.h OPTIONS -DBOOST_TT_HAS_OPERATOR_HPP_INCLUDED) diff --git a/apps/src/pcd_video_player/pcd_video_player.cpp b/apps/src/pcd_video_player/pcd_video_player.cpp new file mode 100644 index 00000000000..f1ba023c3d0 --- /dev/null +++ b/apps/src/pcd_video_player/pcd_video_player.cpp @@ -0,0 +1,322 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: $ + * + * @author: Koen Buys - KU Leuven + */ + +//PCL +#include +#include +#include + +//STD +#include +#include + +//BOOST +#include +#include +#include +#include + +//QT4 +#include +#include +#include +#include +#include +#include +#include +#include + +// VTK +#include +#include +#include + +using namespace pcl; +using namespace std; + +////////////////////////////////////////////////////////////////////////////////////////////////////////// +PCDVideoPlayer::PCDVideoPlayer () +{ + cloud_present_ = false; + cloud_modified_ = false; + play_mode_ = false; + speed_counter_ = 0; + speed_value_ = 5; + + //Create a timer + vis_timer_ = new QTimer (this); + vis_timer_->start (5);//5ms + + connect (vis_timer_, SIGNAL (timeout ()), this, SLOT (timeoutSlot())); + + ui_ = new Ui::MainWindow; + ui_->setupUi (this); + + this->setWindowTitle ("PCL PCD Video Player"); + + // Setup the cloud pointer + cloud_.reset (new pcl::PointCloud); + + // Set up the qvtk window + vis_.reset (new pcl::visualization::PCLVisualizer ("", false)); + ui_->qvtkWidget->SetRenderWindow (vis_->getRenderWindow ()); + vis_->setupInteractor (ui_->qvtkWidget->GetInteractor (), ui_->qvtkWidget->GetRenderWindow ()); + vis_->getInteractorStyle ()->setKeyboardModifier (pcl::visualization::INTERACTOR_KB_MOD_SHIFT); + ui_->qvtkWidget->update (); + + // Connect all buttons + connect (ui_->playButton, SIGNAL(clicked()), this, SLOT(playButtonPressed())); + connect (ui_->stopButton, SIGNAL(clicked()), this, SLOT(stopButtonPressed())); + connect (ui_->backButton, SIGNAL(clicked()), this, SLOT(backButtonPressed())); + connect (ui_->nextButton, SIGNAL(clicked()), this, SLOT(nextButtonPressed())); + + connect (ui_->selectFolderButton, SIGNAL(clicked()), this, SLOT(selectFolderButtonPressed())); + connect (ui_->selectFilesButton, SIGNAL(clicked()), this, SLOT(selectFilesButtonPressed())); + + connect (ui_->indexSlider, SIGNAL(valueChanged(int)), this, SLOT(indexSliderValueChanged(int))); +} + +void +PCDVideoPlayer::playButtonPressed() +{ + play_mode_ = true; +} + +void +PCDVideoPlayer::stopButtonPressed() +{ + play_mode_= false; +} + +void +PCDVideoPlayer::backButtonPressed() +{ + if(current_frame_ == 0) // Allready in the beginning + { + PCL_DEBUG ("[PCDVideoPlayer::nextButtonPressed] : reached the end\n"); + current_frame_ = nr_of_frames_-1; // reset to end + } + else + { + current_frame_--; + cloud_modified_ = true; + ui_->indexSlider->setSliderPosition(current_frame_); // Update the slider position + } +} + +void +PCDVideoPlayer::nextButtonPressed() +{ + if(current_frame_ == (nr_of_frames_-1)) // Reached the end + { + PCL_DEBUG ("[PCDVideoPlayer::nextButtonPressed] : reached the end\n"); + current_frame_ = 0; // reset to beginning + } + else + { + current_frame_++; + cloud_modified_ = true; + ui_->indexSlider->setSliderPosition(current_frame_); // Update the slider position + } +} + +void +PCDVideoPlayer::selectFolderButtonPressed() +{ + pcd_files_.clear(); // Clear the std::vector + pcd_paths_.clear(); // Clear the boost filesystem paths + + dir_ = QFileDialog::getExistingDirectory(this, tr("Open Directory"), "/home", QFileDialog::ShowDirsOnly | QFileDialog::DontResolveSymlinks); + + boost::filesystem::directory_iterator end_itr; + + if(boost::filesystem::is_directory(dir_.toStdString())) + { + for (boost::filesystem::directory_iterator itr(dir_.toStdString()); itr != end_itr; ++itr) + { + std::string ext = itr->path().extension().string(); + if(ext.compare(".pcd") == 0) + { + pcd_files_.push_back (itr->path ().string ()); + pcd_paths_.push_back (itr->path ()); + } + else + { + // Found non pcd file + PCL_DEBUG ("[PCDVideoPlayer::selectFolderButtonPressed] : found a different file\n"); + } + } + } + else + { + PCL_ERROR("Path is not a directory\n"); + exit(-1); + } + nr_of_frames_ = pcd_files_.size(); + PCL_DEBUG ("[PCDVideoPlayer::selectFolderButtonPressed] : found %d files\n", nr_of_frames_ ); + + if(nr_of_frames_ == 0) + { + PCL_ERROR("Please select valid pcd folder\n"); + cloud_present_ = false; + return; + } + else + { + // Reset the Slider + ui_->indexSlider->setValue(0); // set cursor back in the beginning + ui_->indexSlider->setRange(0,nr_of_frames_-1); // rescale the slider + + current_frame_ = 0; + + cloud_present_ = true; + cloud_modified_ = true; + } +} + +void +PCDVideoPlayer::selectFilesButtonPressed() +{ + pcd_files_.clear(); // Clear the std::vector + pcd_paths_.clear(); // Clear the boost filesystem paths + + QStringList qt_pcd_files = QFileDialog::getOpenFileNames(this, "Select one or more PCD files to open", "/home", "PointClouds (*.pcd)"); + nr_of_frames_ = qt_pcd_files.size(); + std::cout << "[PCDVideoPlayer::selectFilesButtonPressed] : selected " << nr_of_frames_ << " files" << std::endl; + + if(nr_of_frames_ == 0) + { + PCL_ERROR("Please select valid pcd files\n"); + cloud_present_ = false; + return; + } + else + { + frame_semantics_.resize (nr_of_frames_); + for(int i = 0; i < qt_pcd_files.size(); i++) + { + pcd_files_.push_back(qt_pcd_files.at(i).toStdString()); + } + + current_frame_ = 0; + + // Reset the Slider + ui_->indexSlider->setValue(0); // set cursor back in the beginning + ui_->indexSlider->setRange(0,nr_of_frames_-1); // rescale the slider + + cloud_present_ = true; + cloud_modified_ = true; + } +} + +void +PCDVideoPlayer::timeoutSlot () +{ + if(play_mode_) + { + if(speed_counter_ == speed_value_) + { + if(current_frame_ == (nr_of_frames_-1)) // Reached the end + { + current_frame_ = 0; // reset to beginning + } + else + { + current_frame_++; + cloud_modified_ = true; + ui_->indexSlider->setSliderPosition(current_frame_); // Update the slider position + } + } + else + { + speed_counter_++; + } + } + + if(cloud_present_ && cloud_modified_) + { + if (pcl::io::loadPCDFile (pcd_files_[current_frame_], *cloud_) == -1) //* load the file + { + PCL_ERROR ("[PCDVideoPlayer::timeoutSlot] : Couldn't read file %s\n"); + } + + if(!vis_->updatePointCloud(cloud_, "cloud_")) + { + vis_->addPointCloud (cloud_, "cloud_"); + vis_->resetCameraViewpoint("cloud_"); + } + cloud_modified_ = false; + } + ui_->qvtkWidget->update(); +} + +void +PCDVideoPlayer::indexSliderValueChanged(int value) +{ + PCL_DEBUG ("[PCDVideoPlayer::indexSliderValueChanged] : (I) : value %d\n", value); + current_frame_ = value; + cloud_modified_ = true; +} + +void +print_usage () +{ + PCL_INFO ("PCDVideoPlayer V0.1\n"); + PCL_INFO ("-------------------\n"); + PCL_INFO ("\tThe slider accepts focus on Tab and provides both a mouse wheel and a keyboard interface. The keyboard interface is the following:\n"); + PCL_INFO ("\t Left/Right move a horizontal slider by one single step.\n"); + PCL_INFO ("\t Up/Down move a vertical slider by one single step.\n"); + PCL_INFO ("\t PageUp moves up one page.\n"); + PCL_INFO ("\t PageDown moves down one page.\n"); + PCL_INFO ("\t Home moves to the start (mininum).\n"); + PCL_INFO ("\t End moves to the end (maximum).\n"); +} + +int +main (int argc, char** argv) +{ + QApplication app(argc, argv); + + PCDVideoPlayer VideoPlayer; + + VideoPlayer.show(); + + return (app.exec()); +} diff --git a/apps/src/pcd_video_player/pcd_video_player.ui b/apps/src/pcd_video_player/pcd_video_player.ui new file mode 100644 index 00000000000..c80ed5d4941 --- /dev/null +++ b/apps/src/pcd_video_player/pcd_video_player.ui @@ -0,0 +1,155 @@ + + + MainWindow + + + + 0 + 0 + 800 + 600 + + + + MainWindow + + + + + + 339 + 10 + 451 + 531 + + + + + + + + + + Qt::Horizontal + + + + + + + + + 9 + 9 + 321 + 531 + + + + + + + Select PCD Files + + + + + + + Select Folder + + + + + + + Qt::Horizontal + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + Qt::Horizontal + + + + + + + + + ... + + + Qt::LeftArrow + + + + + + + Stop + + + + + + + Play + + + Qt::NoArrow + + + + + + + ... + + + Qt::RightArrow + + + + + + + + + + + + 0 + 0 + 800 + 25 + + + + + + + + QVTKWidget + QWidget +
QVTKWidget.h
+ + + + + From 0c01ef7085fd00cb52679d6f5671110f46f68d5c Mon Sep 17 00:00:00 2001 From: Koen_Buys Date: Mon, 18 Mar 2013 14:53:16 +0100 Subject: [PATCH 07/32] Forgot the header file --- apps/include/pcl/apps/pcd_video_player.h | 155 +++++++++++++++++++++++ 1 file changed, 155 insertions(+) create mode 100644 apps/include/pcl/apps/pcd_video_player.h diff --git a/apps/include/pcl/apps/pcd_video_player.h b/apps/include/pcl/apps/pcd_video_player.h new file mode 100644 index 00000000000..221da9da42f --- /dev/null +++ b/apps/include/pcl/apps/pcd_video_player.h @@ -0,0 +1,155 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include + +#include +#include + +// QT4 +#include +#include +#include + +// Boost +#include +#include + +// PCL +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include + +#include + +typedef pcl::PointXYZRGBA PointT; + +#define CURRENT_VERSION 0.2 + +// Useful macros +#define FPS_CALC(_WHAT_) \ +do \ +{ \ + static unsigned count = 0;\ + static double last = pcl::getTime ();\ + double now = pcl::getTime (); \ + ++count; \ + if (now - last >= 1.0) \ + { \ + std::cout << "Average framerate("<< _WHAT_ << "): " << double(count)/double(now - last) << " Hz" << std::endl; \ + count = 0; \ + last = now; \ + } \ +}while(false) + +namespace Ui +{ + class MainWindow; +} + +class PCDVideoPlayer : public QMainWindow +{ + Q_OBJECT + public: + typedef pcl::PointCloud Cloud; + typedef Cloud::Ptr CloudPtr; + typedef Cloud::ConstPtr CloudConstPtr; + + PCDVideoPlayer (); + + ~PCDVideoPlayer () {} + + protected: + boost::shared_ptr vis_; + pcl::PointCloud::Ptr cloud_; + + QMutex mtx_; + QMutex vis_mtx_; + Ui::MainWindow *ui_; + QTimer *vis_timer_; + + QString dir_; + + std::vector pcd_files_; + std::vector pcd_paths_; + + //QStringList pcd_files_; + + unsigned int current_frame_; // The current displayed frame + unsigned int nr_of_frames_; // Store the number of loaded frames + + bool cloud_present_; // Indicate that pointclouds were loaded + bool cloud_modified_; // Indicate that the timeoutSlot needs to reload the pointcloud + + bool play_mode_; // Indicate that files should play continiously + unsigned int speed_counter_; // In play mode only update if speed_counter_ == speed_value + unsigned int speed_value_; // Fixes the speed in steps of 5ms, default 5, gives 5+1 * 5ms = 30ms = 33,3 Hz playback speed + + public slots: + void + playButtonPressed(); + void + stopButtonPressed(); + void + backButtonPressed(); + void + nextButtonPressed(); + void + selectFolderButtonPressed(); + void + selectFilesButtonPressed(); + + void + indexSliderValueChanged(int value); + + private slots: + void + timeoutSlot(); + +}; From 17099edb8735ec1ea7f6b57a4a2db259c1e0d338 Mon Sep 17 00:00:00 2001 From: Koen_Buys Date: Mon, 18 Mar 2013 15:20:13 +0100 Subject: [PATCH 08/32] forgot to remove a line from MHMC --- apps/src/pcd_video_player/pcd_video_player.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/apps/src/pcd_video_player/pcd_video_player.cpp b/apps/src/pcd_video_player/pcd_video_player.cpp index f1ba023c3d0..4f6da488627 100644 --- a/apps/src/pcd_video_player/pcd_video_player.cpp +++ b/apps/src/pcd_video_player/pcd_video_player.cpp @@ -229,7 +229,6 @@ PCDVideoPlayer::selectFilesButtonPressed() } else { - frame_semantics_.resize (nr_of_frames_); for(int i = 0; i < qt_pcd_files.size(); i++) { pcd_files_.push_back(qt_pcd_files.at(i).toStdString()); From efb83ac2bcc10db61e8328e4d97e3f75e0365906 Mon Sep 17 00:00:00 2001 From: Alexandru Eugen Ichim Date: Mon, 18 Mar 2013 22:39:43 -0700 Subject: [PATCH 09/32] made some apps mac-friendly --- apps/CMakeLists.txt | 2 +- .../sources/ground_based_rgbd_people_detection/CMakeLists.txt | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/apps/CMakeLists.txt b/apps/CMakeLists.txt index 5e70e4c8423..a0b525442a8 100644 --- a/apps/CMakeLists.txt +++ b/apps/CMakeLists.txt @@ -78,7 +78,7 @@ if(build) PCL_ADD_EXECUTABLE(pcl_face_trainer ${SUBSYS_NAME} src/face_detection/face_trainer.cpp) target_link_libraries(pcl_face_trainer pcl_features pcl_recognition pcl_common pcl_io pcl_filters pcl_visualization pcl_segmentation pcl_sample_consensus pcl_surface pcl_keypoints pcl_ml pcl_search pcl_kdtree vtkRendering vtkIO) - PCL_ADD_EXECUTABLE(pcl_fs_face_detector ${SUBSYS_NAME} src/face_detection//filesystem_face_detection.cpp) + PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_fs_face_detector ${SUBSYS_NAME} src/face_detection//filesystem_face_detection.cpp) target_link_libraries(pcl_fs_face_detector pcl_features pcl_recognition pcl_common pcl_io pcl_filters pcl_visualization pcl_segmentation pcl_sample_consensus pcl_surface pcl_keypoints pcl_ml pcl_search pcl_kdtree vtkRendering vtkIO) PCL_ADD_EXECUTABLE(pcl_stereo_ground_segmentation ${SUBSYS_NAME} src/stereo_ground_segmentation.cpp) diff --git a/doc/tutorials/content/sources/ground_based_rgbd_people_detection/CMakeLists.txt b/doc/tutorials/content/sources/ground_based_rgbd_people_detection/CMakeLists.txt index f2789d40db4..175f6452e32 100644 --- a/doc/tutorials/content/sources/ground_based_rgbd_people_detection/CMakeLists.txt +++ b/doc/tutorials/content/sources/ground_based_rgbd_people_detection/CMakeLists.txt @@ -6,5 +6,5 @@ include_directories(${PCL_INCLUDE_DIRS}) link_directories(${PCL_LIBRARY_DIRS}) add_definitions(${PCL_DEFINITIONS}) -add_executable (ground_based_rgbd_people_detector src/main_ground_based_people_detection.cpp) +add_executable (ground_based_rgbd_people_detector MACOSX_BUNDLE src/main_ground_based_people_detection.cpp) target_link_libraries (ground_based_rgbd_people_detector ${PCL_LIBRARIES}) From 0ae4bdce736bd4d44fdd14f097b53d443f3ec210 Mon Sep 17 00:00:00 2001 From: Alexandru Eugen Ichim Date: Mon, 18 Mar 2013 23:19:21 -0700 Subject: [PATCH 10/32] beautification --- apps/CMakeLists.txt | 1 - apps/include/pcl/apps/pcd_video_player.h | 71 +++++----- .../src/pcd_video_player/pcd_video_player.cpp | 121 ++++++++---------- 3 files changed, 97 insertions(+), 96 deletions(-) diff --git a/apps/CMakeLists.txt b/apps/CMakeLists.txt index 812a19f5f9f..e4e612d5703 100644 --- a/apps/CMakeLists.txt +++ b/apps/CMakeLists.txt @@ -162,7 +162,6 @@ if(build) PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_manual_registration ${SUBSYS_NAME} ${manual_registration_ui} ${manual_registration_moc} src/manual_registration/manual_registration.cpp) target_link_libraries(pcl_manual_registration pcl_common pcl_io pcl_visualization pcl_segmentation pcl_features pcl_surface QVTK ${QT_LIBRARIES}) - # Manual registration demo QT4_WRAP_UI(pcd_video_player_ui src/pcd_video_player/pcd_video_player.ui) QT4_WRAP_CPP(pcd_video_player_moc include/pcl/apps/pcd_video_player.h OPTIONS -DBOOST_TT_HAS_OPERATOR_HPP_INCLUDED) PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_pcd_video_player ${SUBSYS_NAME} ${pcd_video_player_ui} ${pcd_video_player_moc} src/pcd_video_player/pcd_video_player.cpp) diff --git a/apps/include/pcl/apps/pcd_video_player.h b/apps/include/pcl/apps/pcd_video_player.h index 221da9da42f..36e118eeff8 100644 --- a/apps/include/pcl/apps/pcd_video_player.h +++ b/apps/include/pcl/apps/pcd_video_player.h @@ -68,8 +68,6 @@ #include -typedef pcl::PointXYZRGBA PointT; - #define CURRENT_VERSION 0.2 // Useful macros @@ -97,7 +95,7 @@ class PCDVideoPlayer : public QMainWindow { Q_OBJECT public: - typedef pcl::PointCloud Cloud; + typedef pcl::PointCloud Cloud; typedef Cloud::Ptr CloudPtr; typedef Cloud::ConstPtr CloudConstPtr; @@ -106,50 +104,63 @@ class PCDVideoPlayer : public QMainWindow ~PCDVideoPlayer () {} protected: - boost::shared_ptr vis_; - pcl::PointCloud::Ptr cloud_; + boost::shared_ptr vis_; + pcl::PointCloud::Ptr cloud_; - QMutex mtx_; - QMutex vis_mtx_; - Ui::MainWindow *ui_; - QTimer *vis_timer_; + QMutex mtx_; + QMutex vis_mtx_; + Ui::MainWindow *ui_; + QTimer *vis_timer_; - QString dir_; + QString dir_; - std::vector pcd_files_; - std::vector pcd_paths_; + std::vector pcd_files_; + std::vector pcd_paths_; - //QStringList pcd_files_; - unsigned int current_frame_; // The current displayed frame - unsigned int nr_of_frames_; // Store the number of loaded frames + /** \brief The current displayed frame */ + unsigned int current_frame_; + /** \brief Store the number of loaded frames */ + unsigned int nr_of_frames_; - bool cloud_present_; // Indicate that pointclouds were loaded - bool cloud_modified_; // Indicate that the timeoutSlot needs to reload the pointcloud + /** \brief Indicate that pointclouds were loaded */ + bool cloud_present_; + /** \brief Indicate that the timeoutSlot needs to reload the pointcloud */ + bool cloud_modified_; - bool play_mode_; // Indicate that files should play continiously - unsigned int speed_counter_; // In play mode only update if speed_counter_ == speed_value - unsigned int speed_value_; // Fixes the speed in steps of 5ms, default 5, gives 5+1 * 5ms = 30ms = 33,3 Hz playback speed + /** \brief Indicate that files should play continiously */ + bool play_mode_; + /** \brief In play mode only update if speed_counter_ == speed_value */ + unsigned int speed_counter_; + /** \brief Fixes the speed in steps of 5ms, default 5, gives 5+1 * 5ms = 30ms = 33,3 Hz playback speed */ + unsigned int speed_value_; public slots: void - playButtonPressed(); + playButtonPressed () + { play_mode_ = true; } + void - stopButtonPressed(); + stopButtonPressed() + { play_mode_ = false; } + void - backButtonPressed(); - void - nextButtonPressed(); - void - selectFolderButtonPressed(); + backButtonPressed (); + + void + nextButtonPressed (); + + void + selectFolderButtonPressed (); + void - selectFilesButtonPressed(); + selectFilesButtonPressed (); void - indexSliderValueChanged(int value); + indexSliderValueChanged (int value); private slots: void - timeoutSlot(); + timeoutSlot (); }; diff --git a/apps/src/pcd_video_player/pcd_video_player.cpp b/apps/src/pcd_video_player/pcd_video_player.cpp index 4f6da488627..e990461f8cf 100644 --- a/apps/src/pcd_video_player/pcd_video_player.cpp +++ b/apps/src/pcd_video_player/pcd_video_player.cpp @@ -33,9 +33,6 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * - * $Id: $ - * - * @author: Koen Buys - KU Leuven */ //PCL @@ -84,7 +81,7 @@ PCDVideoPlayer::PCDVideoPlayer () vis_timer_ = new QTimer (this); vis_timer_->start (5);//5ms - connect (vis_timer_, SIGNAL (timeout ()), this, SLOT (timeoutSlot())); + connect (vis_timer_, SIGNAL (timeout ()), this, SLOT (timeoutSlot ())); ui_ = new Ui::MainWindow; ui_->setupUi (this); @@ -102,49 +99,37 @@ PCDVideoPlayer::PCDVideoPlayer () ui_->qvtkWidget->update (); // Connect all buttons - connect (ui_->playButton, SIGNAL(clicked()), this, SLOT(playButtonPressed())); - connect (ui_->stopButton, SIGNAL(clicked()), this, SLOT(stopButtonPressed())); - connect (ui_->backButton, SIGNAL(clicked()), this, SLOT(backButtonPressed())); - connect (ui_->nextButton, SIGNAL(clicked()), this, SLOT(nextButtonPressed())); + connect (ui_->playButton, SIGNAL (clicked ()), this, SLOT (playButtonPressed ())); + connect (ui_->stopButton, SIGNAL (clicked ()), this, SLOT (stopButtonPressed ())); + connect (ui_->backButton, SIGNAL (clicked ()), this, SLOT (backButtonPressed ())); + connect (ui_->nextButton, SIGNAL (clicked ()), this, SLOT (nextButtonPressed ())); - connect (ui_->selectFolderButton, SIGNAL(clicked()), this, SLOT(selectFolderButtonPressed())); - connect (ui_->selectFilesButton, SIGNAL(clicked()), this, SLOT(selectFilesButtonPressed())); + connect (ui_->selectFolderButton, SIGNAL (clicked ()), this, SLOT (selectFolderButtonPressed ())); + connect (ui_->selectFilesButton, SIGNAL (clicked ()), this, SLOT (selectFilesButtonPressed ())); - connect (ui_->indexSlider, SIGNAL(valueChanged(int)), this, SLOT(indexSliderValueChanged(int))); -} - -void -PCDVideoPlayer::playButtonPressed() -{ - play_mode_ = true; + connect (ui_->indexSlider, SIGNAL (valueChanged (int)), this, SLOT (indexSliderValueChanged (int))); } void -PCDVideoPlayer::stopButtonPressed() -{ - play_mode_= false; -} - -void -PCDVideoPlayer::backButtonPressed() +PCDVideoPlayer::backButtonPressed () { if(current_frame_ == 0) // Allready in the beginning { PCL_DEBUG ("[PCDVideoPlayer::nextButtonPressed] : reached the end\n"); - current_frame_ = nr_of_frames_-1; // reset to end + current_frame_ = nr_of_frames_ - 1; // reset to end } else { current_frame_--; cloud_modified_ = true; - ui_->indexSlider->setSliderPosition(current_frame_); // Update the slider position + ui_->indexSlider->setSliderPosition (current_frame_); // Update the slider position } } void -PCDVideoPlayer::nextButtonPressed() +PCDVideoPlayer::nextButtonPressed () { - if(current_frame_ == (nr_of_frames_-1)) // Reached the end + if (current_frame_ == (nr_of_frames_ - 1)) // Reached the end { PCL_DEBUG ("[PCDVideoPlayer::nextButtonPressed] : reached the end\n"); current_frame_ = 0; // reset to beginning @@ -153,26 +138,29 @@ PCDVideoPlayer::nextButtonPressed() { current_frame_++; cloud_modified_ = true; - ui_->indexSlider->setSliderPosition(current_frame_); // Update the slider position + ui_->indexSlider->setSliderPosition (current_frame_); // Update the slider position } } void -PCDVideoPlayer::selectFolderButtonPressed() +PCDVideoPlayer::selectFolderButtonPressed () { - pcd_files_.clear(); // Clear the std::vector - pcd_paths_.clear(); // Clear the boost filesystem paths + pcd_files_.clear (); // Clear the std::vector + pcd_paths_.clear (); // Clear the boost filesystem paths - dir_ = QFileDialog::getExistingDirectory(this, tr("Open Directory"), "/home", QFileDialog::ShowDirsOnly | QFileDialog::DontResolveSymlinks); + dir_ = QFileDialog::getExistingDirectory (this, + tr("Open Directory"), + "/home", + QFileDialog::ShowDirsOnly | QFileDialog::DontResolveSymlinks); boost::filesystem::directory_iterator end_itr; - if(boost::filesystem::is_directory(dir_.toStdString())) + if (boost::filesystem::is_directory (dir_.toStdString ())) { - for (boost::filesystem::directory_iterator itr(dir_.toStdString()); itr != end_itr; ++itr) + for (boost::filesystem::directory_iterator itr (dir_.toStdString ()); itr != end_itr; ++itr) { - std::string ext = itr->path().extension().string(); - if(ext.compare(".pcd") == 0) + std::string ext = itr->path ().extension ().string (); + if (ext.compare (".pcd") == 0) { pcd_files_.push_back (itr->path ().string ()); pcd_paths_.push_back (itr->path ()); @@ -189,20 +177,20 @@ PCDVideoPlayer::selectFolderButtonPressed() PCL_ERROR("Path is not a directory\n"); exit(-1); } - nr_of_frames_ = pcd_files_.size(); + nr_of_frames_ = pcd_files_.size (); PCL_DEBUG ("[PCDVideoPlayer::selectFolderButtonPressed] : found %d files\n", nr_of_frames_ ); - if(nr_of_frames_ == 0) + if (nr_of_frames_ == 0) { - PCL_ERROR("Please select valid pcd folder\n"); + PCL_ERROR ("Please select valid pcd folder\n"); cloud_present_ = false; return; } else { // Reset the Slider - ui_->indexSlider->setValue(0); // set cursor back in the beginning - ui_->indexSlider->setRange(0,nr_of_frames_-1); // rescale the slider + ui_->indexSlider->setValue (0); // set cursor back in the beginning + ui_->indexSlider->setRange (0, nr_of_frames_ - 1); // rescale the slider current_frame_ = 0; @@ -212,33 +200,36 @@ PCDVideoPlayer::selectFolderButtonPressed() } void -PCDVideoPlayer::selectFilesButtonPressed() +PCDVideoPlayer::selectFilesButtonPressed () { - pcd_files_.clear(); // Clear the std::vector - pcd_paths_.clear(); // Clear the boost filesystem paths + pcd_files_.clear (); // Clear the std::vector + pcd_paths_.clear (); // Clear the boost filesystem paths - QStringList qt_pcd_files = QFileDialog::getOpenFileNames(this, "Select one or more PCD files to open", "/home", "PointClouds (*.pcd)"); - nr_of_frames_ = qt_pcd_files.size(); - std::cout << "[PCDVideoPlayer::selectFilesButtonPressed] : selected " << nr_of_frames_ << " files" << std::endl; + QStringList qt_pcd_files = QFileDialog::getOpenFileNames (this, + "Select one or more PCD files to open", + "/home", + "PointClouds (*.pcd)"); + nr_of_frames_ = qt_pcd_files.size (); + PCL_INFO ("[PCDVideoPlayer::selectFilesButtonPressed] : selected %ld files\n", nr_of_frames_); - if(nr_of_frames_ == 0) + if (nr_of_frames_ == 0) { - PCL_ERROR("Please select valid pcd files\n"); + PCL_ERROR ("Please select valid pcd files\n"); cloud_present_ = false; return; } else { - for(int i = 0; i < qt_pcd_files.size(); i++) + for(int i = 0; i < qt_pcd_files.size (); i++) { - pcd_files_.push_back(qt_pcd_files.at(i).toStdString()); + pcd_files_.push_back (qt_pcd_files.at (i).toStdString ()); } current_frame_ = 0; // Reset the Slider - ui_->indexSlider->setValue(0); // set cursor back in the beginning - ui_->indexSlider->setRange(0,nr_of_frames_-1); // rescale the slider + ui_->indexSlider->setValue (0); // set cursor back in the beginning + ui_->indexSlider->setRange (0, nr_of_frames_ - 1); // rescale the slider cloud_present_ = true; cloud_modified_ = true; @@ -248,11 +239,11 @@ PCDVideoPlayer::selectFilesButtonPressed() void PCDVideoPlayer::timeoutSlot () { - if(play_mode_) + if (play_mode_) { - if(speed_counter_ == speed_value_) + if (speed_counter_ == speed_value_) { - if(current_frame_ == (nr_of_frames_-1)) // Reached the end + if (current_frame_ == (nr_of_frames_-1)) // Reached the end { current_frame_ = 0; // reset to beginning } @@ -260,7 +251,7 @@ PCDVideoPlayer::timeoutSlot () { current_frame_++; cloud_modified_ = true; - ui_->indexSlider->setSliderPosition(current_frame_); // Update the slider position + ui_->indexSlider->setSliderPosition (current_frame_); // Update the slider position } } else @@ -269,25 +260,25 @@ PCDVideoPlayer::timeoutSlot () } } - if(cloud_present_ && cloud_modified_) + if (cloud_present_ && cloud_modified_) { if (pcl::io::loadPCDFile (pcd_files_[current_frame_], *cloud_) == -1) //* load the file { PCL_ERROR ("[PCDVideoPlayer::timeoutSlot] : Couldn't read file %s\n"); } - if(!vis_->updatePointCloud(cloud_, "cloud_")) + if (!vis_->updatePointCloud(cloud_, "cloud_")) { vis_->addPointCloud (cloud_, "cloud_"); vis_->resetCameraViewpoint("cloud_"); } cloud_modified_ = false; } - ui_->qvtkWidget->update(); + ui_->qvtkWidget->update (); } void -PCDVideoPlayer::indexSliderValueChanged(int value) +PCDVideoPlayer::indexSliderValueChanged (int value) { PCL_DEBUG ("[PCDVideoPlayer::indexSliderValueChanged] : (I) : value %d\n", value); current_frame_ = value; @@ -311,11 +302,11 @@ print_usage () int main (int argc, char** argv) { - QApplication app(argc, argv); + QApplication app (argc, argv); PCDVideoPlayer VideoPlayer; - VideoPlayer.show(); + VideoPlayer.show (); - return (app.exec()); + return (app.exec ()); } From 492ee60266945bda5c46b835afea0952220c675b Mon Sep 17 00:00:00 2001 From: Michael Korn Date: Wed, 20 Mar 2013 13:29:02 +0100 Subject: [PATCH 11/32] small fix in pcl_find_ros.cmake see www.pcl-users.org/Migration-from-svn-to-git-tt4026826.html --- cmake/pcl_find_ros.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/pcl_find_ros.cmake b/cmake/pcl_find_ros.cmake index 20117296c2a..cf2e1e37f8e 100644 --- a/cmake/pcl_find_ros.cmake +++ b/cmake/pcl_find_ros.cmake @@ -51,7 +51,7 @@ if(ROS_DISTRO) link_directories(${_path}/lib) endforeach(_path) endif() - elseif() + else() find_package(ROS COMPONENTS catkin roscpp_serialization std_msgs sensor_msgs rostime) if (ROS_FOUND) # if find_package(ROS ...) found the required components, add their include directories From ef6cdb1aa50cd9bdd0d2c42e971543ae6404a33f Mon Sep 17 00:00:00 2001 From: Mourad Boufarguine Date: Wed, 20 Mar 2013 23:25:11 +0100 Subject: [PATCH 12/32] fix Boost modules on Windows --- cmake/pcl_find_boost.cmake | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/cmake/pcl_find_boost.cmake b/cmake/pcl_find_boost.cmake index 5ed268e7d14..212d2e0e1a4 100644 --- a/cmake/pcl_find_boost.cmake +++ b/cmake/pcl_find_boost.cmake @@ -24,15 +24,16 @@ if(Boost_SERIALIZATION_FOUND) endif(Boost_SERIALIZATION_FOUND) # Required boost modules -find_package(Boost 1.40.0 REQUIRED COMPONENTS system filesystem thread date_time iostreams) - +set(BOOST_REQUIRED_MODULES system filesystem thread date_time iostreams) # Starting with Boost 1.50, boost_thread depends on chrono. As this is not # taken care of automatically on Windows, we add an explicit dependency as a # workaround. if(WIN32 AND Boost_VERSION VERSION_GREATER "104900") - find_package(Boost 1.40.0 REQUIRED COMPONENTS chrono) + set(BOOST_REQUIRED_MODULES ${BOOST_REQUIRED_MODULES} chrono) endif(WIN32 AND Boost_VERSION VERSION_GREATER "104900") +find_package(Boost 1.40.0 REQUIRED COMPONENTS ${BOOST_REQUIRED_MODULES}) + if(Boost_FOUND) set(BOOST_FOUND TRUE) # Obtain diagnostic information about Boost's automatic linking outputted From 25c95203ef14eb6c9d29f07adbf6e4eba019abf9 Mon Sep 17 00:00:00 2001 From: Keven Ring Date: Fri, 22 Mar 2013 10:17:52 -0400 Subject: [PATCH 13/32] Added ability for user to define minimum and maximum return thresholds. Values returned outside these thresholds will be discarded. Fixed an issue where the rotation angle returned from the HDL might be 36000 (instead of 0). This causes a buffer access overflow. --- io/include/pcl/io/hdl_grabber.h | 27 ++++++++++++++++-- io/src/hdl_grabber.cpp | 49 +++++++++++++++++++++++++++++---- 2 files changed, 69 insertions(+), 7 deletions(-) diff --git a/io/include/pcl/io/hdl_grabber.h b/io/include/pcl/io/hdl_grabber.h index eca1b4b89d6..d86eda29b2c 100644 --- a/io/include/pcl/io/hdl_grabber.h +++ b/io/include/pcl/io/hdl_grabber.h @@ -144,9 +144,30 @@ namespace pcl */ void setLaserColorRGB (const pcl::RGB& color, unsigned int laserNumber); + /** \brief Any returns from the HDL with a distance less than this are discarded. + * This value is in meters + * Default: 0.0 + */ + void setMinimumDistanceThreshold(float & minThreshold); + + /** \brief Any returns from the HDL with a distance greater than this are discarded. + * This value is in meters + * Default: 10000.0 + */ + void setMaximumDistanceThreshold(float & maxThreshold); + + /** \brief Returns the current minimum distance threshold, in meters + */ + + float getMinimumDistanceThreshold(); + + /** \brief Returns the current maximum distance threshold, in meters + */ + float getMaximumDistanceThreshold(); + protected: static const int HDL_DATA_PORT = 2368; - static const int HDL_NUM_ROT_ANGLES = 36000; + static const int HDL_NUM_ROT_ANGLES = 36001; static const int HDL_LASER_PER_FIRING = 32; static const int HDL_MAX_NUM_LASERS = 64; static const int HDL_FIRING_PER_PKT = 12; @@ -200,7 +221,7 @@ namespace pcl boost::asio::ip::udp::endpoint udp_listener_endpoint_; boost::asio::ip::address source_address_filter_; unsigned short source_port_filter_; - boost::asio::io_service hdl_read_socket_service_; + boost::asio::io_service hdl_read_socket_service_; boost::asio::ip::udp::socket *hdl_read_socket_; std::string pcap_file_name_; boost::thread *queue_consumer_thread_; @@ -221,6 +242,8 @@ namespace pcl boost::signals2::signal* scan_xyzrgb_signal_; boost::signals2::signal* scan_xyzi_signal_; pcl::RGB laser_rgb_mapping_[HDL_MAX_NUM_LASERS]; + float min_distance_threshold_; + float max_distance_threshold_; void processVelodynePackets (); void enqueueHDLPacket (const unsigned char *data, diff --git a/io/src/hdl_grabber.cpp b/io/src/hdl_grabber.cpp index a8c7acd87cd..a6183886e6b 100644 --- a/io/src/hdl_grabber.cpp +++ b/io/src/hdl_grabber.cpp @@ -80,6 +80,8 @@ pcl::HDLGrabber::HDLGrabber (const std::string& correctionsFile, , scan_xyz_signal_ () , scan_xyzrgb_signal_ () , scan_xyzi_signal_ () + , min_distance_threshold_(0.0) + , max_distance_threshold_(10000.0) { initialize (correctionsFile); } @@ -110,6 +112,8 @@ pcl::HDLGrabber::HDLGrabber (const boost::asio::ip::address& ipAddress, , scan_xyz_signal_ () , scan_xyzrgb_signal_ () , scan_xyzi_signal_ () + , min_distance_threshold_(0.0) + , max_distance_threshold_(10000.0) { initialize (correctionsFile); } @@ -373,6 +377,7 @@ pcl::HDLGrabber::toPointClouds (HDLDataPacket *dataPacket) PointXYZ xyz; PointXYZI xyzi; PointXYZRGBA xyzrgb; + computeXYZI (xyzi, firingData.rotationalPosition, firingData.laserReturns[j], laser_corrections_[j + offset]); xyz.x = xyzrgb.x = xyzi.x; @@ -409,6 +414,15 @@ pcl::HDLGrabber::computeXYZI (pcl::PointXYZI& point, int azimuth, HDLLaserReturn laserReturn, HDLLaserCorrection correction) { double cosAzimuth, sinAzimuth; + + double distanceM = laserReturn.distance * 0.002; + + if (distanceM < min_distance_threshold_ || distanceM > max_distance_threshold_) { + point.x = point.y = point.z = std::numeric_limits::quiet_NaN(); + point.intensity = static_cast (laserReturn.intensity); + return; + } + if (correction.azimuthCorrection == 0) { cosAzimuth = cos_lookup_table_[azimuth]; @@ -420,7 +434,8 @@ pcl::HDLGrabber::computeXYZI (pcl::PointXYZI& point, int azimuth, cosAzimuth = std::cos (azimuthInRadians); sinAzimuth = std::sin (azimuthInRadians); } - double distanceM = laserReturn.distance * 0.002 + correction.distanceCorrection; + + distanceM += correction.distanceCorrection; double xyDistance = distanceM * correction.cosVertCorrection - correction.sinVertOffsetCorrection; @@ -587,18 +602,42 @@ pcl::HDLGrabber::setLaserColorRGB (const pcl::RGB& color, ///////////////////////////////////////////////////////////////////////////// bool -pcl::HDLGrabber::isAddressUnspecified (const boost::asio::ip::address& ip_address) +pcl::HDLGrabber::isAddressUnspecified (const boost::asio::ip::address& ipAddress) { #if BOOST_VERSION>=104700 - return (ip_address.is_unspecified ()); + return (ipAddress.is_unspecified ()); #else - if (ip_address.is_v4 ()) - return (ip_address.to_v4 ().to_ulong() == 0); + if (ipAddress.is_v4 ()) + return (ipAddress.to_v4 ().to_ulong() == 0); return (false); #endif } +///////////////////////////////////////////////////////////////////////////// +void +pcl::HDLGrabber::setMaximumDistanceThreshold(float &maxThreshold) { + max_distance_threshold_ = maxThreshold; +} + +///////////////////////////////////////////////////////////////////////////// +void +pcl::HDLGrabber::setMinimumDistanceThreshold(float &minThreshold) { + min_distance_threshold_ = minThreshold; +} + +///////////////////////////////////////////////////////////////////////////// +float +pcl::HDLGrabber::getMaximumDistanceThreshold() { + return(max_distance_threshold_); +} + +///////////////////////////////////////////////////////////////////////////// +float +pcl::HDLGrabber::getMinimumDistanceThreshold() { + return(min_distance_threshold_); +} + ///////////////////////////////////////////////////////////////////////////// void pcl::HDLGrabber::readPacketsFromSocket () From 858823b1f8ef21b02871a8793b40971d4a12bc9a Mon Sep 17 00:00:00 2001 From: Mourad Boufarguine Date: Sun, 24 Mar 2013 00:06:35 +0100 Subject: [PATCH 14/32] fix the build with MSVC - no continue statements inside OpenMP for loops - signed index variable for OpenMP for loops --- surface/include/pcl/surface/impl/mls.hpp | 40 +++++++++++------------- 1 file changed, 19 insertions(+), 21 deletions(-) diff --git a/surface/include/pcl/surface/impl/mls.hpp b/surface/include/pcl/surface/impl/mls.hpp index cef084cb296..1262b2ab4c7 100644 --- a/surface/include/pcl/surface/impl/mls.hpp +++ b/surface/include/pcl/surface/impl/mls.hpp @@ -521,7 +521,7 @@ pcl::MovingLeastSquaresOMP::performProcessing (PointCloudOu // For all points #pragma omp parallel for schedule (dynamic,1000) num_threads (threads) - for (size_t cp = 0; cp < indices_->size (); ++cp) + for (int cp = 0; cp < static_cast (indices_->size ()); ++cp) { // Allocate enough space to hold the results of nearest neighbor searches // \note resize is irrelevant for a radiusSearch (). @@ -529,29 +529,27 @@ pcl::MovingLeastSquaresOMP::performProcessing (PointCloudOu std::vector nn_sqr_dists; // Get the initial estimates of point positions and their neighborhoods - if (!searchForNeighbors ((*indices_)[cp], nn_indices, nn_sqr_dists)) - continue; - - - // Check the number of nearest neighbors for normal estimation (and later - // for polynomial fit as well) - if (nn_indices.size () < 3) - continue; - - - // This thread's ID (range 0 to threads-1) - int tn = omp_get_thread_num (); + if (searchForNeighbors ((*indices_)[cp], nn_indices, nn_sqr_dists)) + { + // Check the number of nearest neighbors for normal estimation (and later + // for polynomial fit as well) + if (nn_indices.size () >= 3) + { + // This thread's ID (range 0 to threads-1) + int tn = omp_get_thread_num (); - // Size of projected points before computeMLSPointNormal () adds points - size_t pp_size = projected_points[tn].size (); + // Size of projected points before computeMLSPointNormal () adds points + size_t pp_size = projected_points[tn].size (); - // Get a plane approximating the local surface's tangent and project point onto it - int index = (*indices_)[cp]; - computeMLSPointNormal (index, nn_indices, nn_sqr_dists, projected_points[tn], projected_points_normals[tn], corresponding_input_indices[tn], this->mls_results_[index]); + // Get a plane approximating the local surface's tangent and project point onto it + int index = (*indices_)[cp]; + computeMLSPointNormal (index, nn_indices, nn_sqr_dists, projected_points[tn], projected_points_normals[tn], corresponding_input_indices[tn], this->mls_results_[index]); - // Copy all information from the input cloud to the output points (not doing any interpolation) - for (size_t pp = pp_size; pp < projected_points[tn].size (); ++pp) - copyMissingFields (input_->points[(*indices_)[cp]], projected_points[tn][pp]); + // Copy all information from the input cloud to the output points (not doing any interpolation) + for (size_t pp = pp_size; pp < projected_points[tn].size (); ++pp) + copyMissingFields (input_->points[(*indices_)[cp]], projected_points[tn][pp]); + } + } } From bdbab9f12d8c3cd0cd72cf2d78e4a96f50d53d64 Mon Sep 17 00:00:00 2001 From: Mourad Boufarguine Date: Wed, 20 Mar 2013 23:51:42 +0100 Subject: [PATCH 15/32] fix the build of the recognition module on Windows --- recognition/src/ransac_based/obj_rec_ransac.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/recognition/src/ransac_based/obj_rec_ransac.cpp b/recognition/src/ransac_based/obj_rec_ransac.cpp index 576af74a09f..8494ba8ecf0 100644 --- a/recognition/src/ransac_based/obj_rec_ransac.cpp +++ b/recognition/src/ransac_based/obj_rec_ransac.cpp @@ -537,7 +537,7 @@ pcl::recognition::ObjRecRANSAC::buildGraphOfConflictingHypotheses (const BVHH& b } // Make sure that we do not compute the same set intersection twice - pair::iterator, bool> res = ordered_hypotheses_ids.insert (id_pair); + pair::iterator, bool> res = ordered_hypotheses_ids.insert (id_pair); if ( res.second == false ) continue; // We've already computed that set intersection -> check the next pair From b05a05b7b1d27c5400d27f09d78ad7e7565d6ad9 Mon Sep 17 00:00:00 2001 From: "Radu B. Rusu" Date: Sun, 24 Mar 2013 12:22:21 -0700 Subject: [PATCH 16/32] fixed bizarre compilation problem of MLS on gcc 4.7.2 --- surface/include/pcl/surface/impl/mls.hpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/surface/include/pcl/surface/impl/mls.hpp b/surface/include/pcl/surface/impl/mls.hpp index 1262b2ab4c7..ca9513e92cb 100644 --- a/surface/include/pcl/surface/impl/mls.hpp +++ b/surface/include/pcl/surface/impl/mls.hpp @@ -529,7 +529,7 @@ pcl::MovingLeastSquaresOMP::performProcessing (PointCloudOu std::vector nn_sqr_dists; // Get the initial estimates of point positions and their neighborhoods - if (searchForNeighbors ((*indices_)[cp], nn_indices, nn_sqr_dists)) + if (this->searchForNeighbors ((*indices_)[cp], nn_indices, nn_sqr_dists)) { // Check the number of nearest neighbors for normal estimation (and later // for polynomial fit as well) @@ -543,13 +543,13 @@ pcl::MovingLeastSquaresOMP::performProcessing (PointCloudOu // Get a plane approximating the local surface's tangent and project point onto it int index = (*indices_)[cp]; - computeMLSPointNormal (index, nn_indices, nn_sqr_dists, projected_points[tn], projected_points_normals[tn], corresponding_input_indices[tn], this->mls_results_[index]); + this->computeMLSPointNormal (index, nn_indices, nn_sqr_dists, projected_points[tn], projected_points_normals[tn], corresponding_input_indices[tn], this->mls_results_[index]); // Copy all information from the input cloud to the output points (not doing any interpolation) for (size_t pp = pp_size; pp < projected_points[tn].size (); ++pp) - copyMissingFields (input_->points[(*indices_)[cp]], projected_points[tn][pp]); + this->copyMissingFields (input_->points[(*indices_)[cp]], projected_points[tn][pp]); + } } - } } @@ -564,7 +564,7 @@ pcl::MovingLeastSquaresOMP::performProcessing (PointCloudOu } // Perform the distinct-cloud or voxel-grid upsampling - performUpsampling (output); + this->performUpsampling (output); } #endif From a81fa7a8bb8c0825b0d149735d07059e81eabff6 Mon Sep 17 00:00:00 2001 From: Mourad Boufarguine Date: Sun, 24 Mar 2013 20:46:05 +0100 Subject: [PATCH 17/32] fix win32 build --- surface/include/pcl/surface/impl/mls.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/surface/include/pcl/surface/impl/mls.hpp b/surface/include/pcl/surface/impl/mls.hpp index 1262b2ab4c7..b7df7557d35 100644 --- a/surface/include/pcl/surface/impl/mls.hpp +++ b/surface/include/pcl/surface/impl/mls.hpp @@ -515,8 +515,8 @@ pcl::MovingLeastSquaresOMP::performProcessing (PointCloudOu unsigned int threads = threads_ == 0 ? 1 : threads_; // Create temporaries for each thread in order to avoid synchronization - std::vector projected_points (threads); - std::vector projected_points_normals (threads); + PointCloudOut::CloudVectorType projected_points (threads); + NormalCloud::CloudVectorType projected_points_normals (threads); std::vector corresponding_input_indices (threads); // For all points From 6a6e6991336da1ef3b58b6fecc9331b5e93b81ef Mon Sep 17 00:00:00 2001 From: "Radu B. Rusu" Date: Sun, 24 Mar 2013 13:00:12 -0700 Subject: [PATCH 18/32] added an extra typename for GCC/POSIX compilers for Mourad's win32 MLS fix --- surface/include/pcl/surface/impl/mls.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/surface/include/pcl/surface/impl/mls.hpp b/surface/include/pcl/surface/impl/mls.hpp index 2ffdc4686d2..6b3a459e0bd 100644 --- a/surface/include/pcl/surface/impl/mls.hpp +++ b/surface/include/pcl/surface/impl/mls.hpp @@ -515,8 +515,8 @@ pcl::MovingLeastSquaresOMP::performProcessing (PointCloudOu unsigned int threads = threads_ == 0 ? 1 : threads_; // Create temporaries for each thread in order to avoid synchronization - PointCloudOut::CloudVectorType projected_points (threads); - NormalCloud::CloudVectorType projected_points_normals (threads); + typename PointCloudOut::CloudVectorType projected_points (threads); + typename NormalCloud::CloudVectorType projected_points_normals (threads); std::vector corresponding_input_indices (threads); // For all points From cfeb0d25f3ed66b85581a0765d986c21107c32b1 Mon Sep 17 00:00:00 2001 From: "Radu B. Rusu" Date: Sun, 24 Mar 2013 13:47:57 -0700 Subject: [PATCH 19/32] if we have an #ifdef for OPENMP when PCL_INSTANTIATE_MovingLeastSquaresOMP is defined then we need to add the same when it's instantiated --- surface/src/mls.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/surface/src/mls.cpp b/surface/src/mls.cpp index 65c5340c91b..37d617299dd 100644 --- a/surface/src/mls.cpp +++ b/surface/src/mls.cpp @@ -46,8 +46,9 @@ PCL_INSTANTIATE_PRODUCT(MovingLeastSquares, ((pcl::PointXYZ)(pcl::PointXYZRGB)(pcl::PointXYZRGBA)) ((pcl::PointXYZ)(pcl::PointXYZRGB)(pcl::PointXYZRGBA)(pcl::PointXYZRGBNormal)(pcl::PointNormal))) +#ifdef _OPENMP PCL_INSTANTIATE_PRODUCT(MovingLeastSquaresOMP, ((pcl::PointXYZ)(pcl::PointXYZRGB)(pcl::PointXYZRGBA)) ((pcl::PointXYZ)(pcl::PointXYZRGB)(pcl::PointXYZRGBA)(pcl::PointXYZRGBNormal)(pcl::PointNormal))) - +#endif /// Ideally, we should instantiate like below, but it takes large amounts of main memory for compilation //PCL_INSTANTIATE_PRODUCT(MovingLeastSquares, (PCL_XYZ_POINT_TYPES)(PCL_XYZ_POINT_TYPES)) From a7f5810c63537057b13135d59abd76530388e413 Mon Sep 17 00:00:00 2001 From: "Radu B. Rusu" Date: Sun, 24 Mar 2013 13:53:02 -0700 Subject: [PATCH 20/32] added extra _OPENMP guards for the surface tests --- test/surface/test_moving_least_squares.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/test/surface/test_moving_least_squares.cpp b/test/surface/test_moving_least_squares.cpp index 71d7e8e0a83..37f24279b83 100644 --- a/test/surface/test_moving_least_squares.cpp +++ b/test/surface/test_moving_least_squares.cpp @@ -87,7 +87,7 @@ TEST (PCL, MovingLeastSquares) EXPECT_NEAR (fabs (mls_normals->points[0].normal[2]), 0.795969, 1e-3); EXPECT_NEAR (mls_normals->points[0].curvature, 0.012019, 1e-3); - +#ifdef _OPENMP // Testing OpenMP version MovingLeastSquaresOMP mls_omp; mls_omp.setInputCloud (cloud); @@ -104,7 +104,7 @@ TEST (PCL, MovingLeastSquares) int count = 0; for (size_t i = 0; i < mls_normals->size (); ++i) { - if (fabs (mls_normals->points[i].x - 0.005417) < 1e-3 && + if (fabs (mls_normals->points[i].x - 0.005417) < 1e-3 && fabs (mls_normals->points[i].y - 0.113463) < 1e-3 && fabs (mls_normals->points[i].z - 0.040715) < 1e-3 && fabs (fabs (mls_normals->points[i].normal[0]) - 0.111894) < 1e-3 && @@ -116,7 +116,7 @@ TEST (PCL, MovingLeastSquares) EXPECT_EQ (count, 1); - +#endif // Testing upsampling MovingLeastSquares mls_upsampling; From a223cc9d867bbcbf23b13ce23e60f17c4abab99e Mon Sep 17 00:00:00 2001 From: "Radu B. Rusu" Date: Sun, 24 Mar 2013 15:37:49 -0700 Subject: [PATCH 21/32] reverted random_sample code to 03/08/13, as we're not sure yet what the right patch for it is, and this version breaks other higher level code --- .../pcl/filters/impl/random_sample.hpp | 61 +++++----- filters/include/pcl/filters/random_sample.h | 90 ++++++++------- filters/src/random_sample.cpp | 108 +++++++----------- 3 files changed, 119 insertions(+), 140 deletions(-) diff --git a/filters/include/pcl/filters/impl/random_sample.hpp b/filters/include/pcl/filters/impl/random_sample.hpp index 8f1f2584881..e71396b64fb 100644 --- a/filters/include/pcl/filters/impl/random_sample.hpp +++ b/filters/include/pcl/filters/impl/random_sample.hpp @@ -72,7 +72,9 @@ pcl::RandomSample::applyFilter (PointCloud &output) { uint8_t* pt_data = reinterpret_cast (&output[(*removed_indices_)[rii]]); for (size_t i = 0; i < offsets.size (); ++i) + { memcpy (pt_data + offsets[i], &user_filter_value, sizeof (float)); + } if (!pcl_isfinite (user_filter_value_)) output.is_dense = false; } @@ -90,12 +92,11 @@ template void pcl::RandomSample::applyFilter (std::vector &indices) { - rng_->base ().seed (seed_); - - size_t N = indices_->size (); + unsigned N = static_cast (indices_->size ()); + float one_over_N = 1.0f / float (N); - int sample_size = negative_ ? N - sample_ : sample_; - // If sample size is 0 or if the sample size is greater than input cloud size + unsigned int sample_size = negative_ ? N - sample_ : sample_; + // If sample size is 0 or if the sample size is greater then input cloud size // then return all indices if (sample_size >= N) { @@ -109,58 +110,50 @@ pcl::RandomSample::applyFilter (std::vector &indices) if (extract_removed_indices_) removed_indices_->resize (static_cast (N - sample_size)); + // Set random seed so derived indices are the same each time the filter runs + std::srand (seed_); + // Algorithm A - float one_over_N = 0.f; - float top = 0.f; - size_t index = 0; + unsigned top = N - sample_size; + unsigned i = 0; + unsigned index = 0; std::vector added; if (extract_removed_indices_) added.resize (indices_->size (), false); - size_t i = 0; - - for (size_t n = sample_; n > 1; n--) + for (size_t n = sample_size; n >= 2; n--) { - top = N - n; // N are the remaining number of elements, n the remaining number of wanted samples - one_over_N = 1.f / static_cast (N); //we need to re-calculate N^{-1} - - float V = unifRand (); - size_t S = 0; - float quot = top * one_over_N; - + unsigned int V = static_cast( unifRand () ); + unsigned S = 0; + float quot = float (top) * one_over_N; while (quot > V) { - S ++; - N --; - quot = quot * (top * one_over_N); + S++; + top--; + N--; + quot = quot * float (top) * one_over_N; } - - N--; // this together with N-- above is the same than N - S - 1 (paper Vit84) index += S; - - if (extract_removed_indices_) added[index] = true; - indices[i] = (*indices_)[index]; - - i ++; - index ++; + indices[i++] = (*indices_)[index++]; + N--; } - - index += static_cast (N * unifRand ()); + index += N * static_cast (unifRand ()); if (extract_removed_indices_) added[index] = true; - indices[i] = (*indices_)[index]; - + indices[i++] = (*indices_)[index++]; // Now populate removed_indices_ appropriately if (extract_removed_indices_) { - size_t ri = 0; + unsigned ri = 0; for (size_t i = 0; i < added.size (); i++) { if (!added[i]) + { (*removed_indices_)[ri++] = (*indices_)[i]; + } } } } diff --git a/filters/include/pcl/filters/random_sample.h b/filters/include/pcl/filters/random_sample.h index d04ea62dbab..34ab878ed1d 100644 --- a/filters/include/pcl/filters/random_sample.h +++ b/filters/include/pcl/filters/random_sample.h @@ -75,41 +75,50 @@ namespace pcl typedef boost::shared_ptr< const RandomSample > ConstPtr; /** \brief Empty constructor. */ - RandomSample (bool extract_removed_indices = false) - : FilterIndices (extract_removed_indices) - , sample_ (std::numeric_limits::max ()) - , rng_alg_ () - , rng_ (new boost::uniform_01 (rng_alg_)) - , seed_ (static_cast (time (NULL))) - { filter_name_ = "RandomSample"; } + RandomSample (bool extract_removed_indices = false) : + FilterIndices (extract_removed_indices), + sample_ (UINT_MAX), + seed_ (static_cast (time (NULL))) + { + filter_name_ = "RandomSample"; + } /** \brief Set number of indices to be sampled. * \param sample */ inline void setSample (unsigned int sample) - { sample_ = sample; } + { + sample_ = sample; + } /** \brief Get the value of the internal \a sample parameter. */ inline unsigned int getSample () - { return (sample_); } + { + return (sample_); + } /** \brief Set seed of random function. * \param seed */ inline void setSeed (unsigned int seed) - { seed_ = seed; } + { + seed_ = seed; + } /** \brief Get the value of the internal \a seed parameter. */ inline unsigned int getSeed () - { return (seed_); } + { + return (seed_); + } protected: + /** \brief Number of indices that will be returned. */ unsigned int sample_; /** \brief Random number seed. */ @@ -127,17 +136,15 @@ namespace pcl void applyFilter (std::vector &indices); - /** \brief Return a random number. */ + /** \brief Return a random number fast using a LCG (Linear Congruential Generator) algorithm. + * See http://software.intel.com/en-us/articles/fast-random-number-generator-on-the-intel-pentiumr-4-processor/ for more information. + */ inline float unifRand () - { return ((*rng_) ()); } - - private: - /** \brief Boost-based random number generator algorithm. */ - boost::mt19937 rng_alg_; - - /** \brief Boost-based random number generator distribution. */ - boost::shared_ptr > rng_; + { + return (static_cast(rand () / double (RAND_MAX))); + //return (((214013 * seed_ + 2531011) >> 16) & 0x7FFF); + } }; /** \brief @b RandomSample applies a random sampling with uniform probability. @@ -160,40 +167,47 @@ namespace pcl typedef boost::shared_ptr > ConstPtr; /** \brief Empty constructor. */ - RandomSample () - : sample_ (std::numeric_limits::max ()) - , rng_alg_ () - , rng_ (new boost::uniform_01 (rng_alg_)) - , seed_ (static_cast (time (NULL))) - { filter_name_ = "RandomSample"; } + RandomSample () : sample_ (UINT_MAX), seed_ (static_cast (time (NULL))) + { + filter_name_ = "RandomSample"; + } /** \brief Set number of indices to be sampled. * \param sample */ inline void setSample (unsigned int sample) - { sample_ = sample; } + { + sample_ = sample; + } /** \brief Get the value of the internal \a sample parameter. */ inline unsigned int getSample () - { return (sample_); } + { + return (sample_); + } /** \brief Set seed of random function. * \param seed */ inline void setSeed (unsigned int seed) - { seed_ = seed; } + { + seed_ = seed; + } /** \brief Get the value of the internal \a seed parameter. */ inline unsigned int getSeed () - { return (seed_); } + { + return (seed_); + } protected: + /** \brief Number of indices that will be returned. */ unsigned int sample_; /** \brief Random number seed. */ @@ -211,17 +225,15 @@ namespace pcl void applyFilter (std::vector &indices); - /** \brief Return a random number. */ + /** \brief Return a random number fast using a LCG (Linear Congruential Generator) algorithm. + * See http://software.intel.com/en-us/articles/fast-random-number-generator-on-the-intel-pentiumr-4-processor/ for more information. + */ inline float unifRand () - { return ((*rng_) ()); } - - private: - /** \brief Boost-based random number generator algorithm. */ - boost::mt19937 rng_alg_; - - /** \brief Boost-based random number generator distribution. */ - boost::shared_ptr > rng_; + { + return (static_cast (rand () / double (RAND_MAX))); + //return (((214013 * seed_ + 2531011) >> 16) & 0x7FFF); + } }; } diff --git a/filters/src/random_sample.cpp b/filters/src/random_sample.cpp index 2af856da545..901080e18a2 100644 --- a/filters/src/random_sample.cpp +++ b/filters/src/random_sample.cpp @@ -44,8 +44,6 @@ void pcl::RandomSample::applyFilter (PointCloud2 &output) { - rng_->base ().seed (seed_); - unsigned N = input_->width * input_->height; // If sample size is 0 or if the sample size is greater then input cloud size // then return entire copy of cloud @@ -65,54 +63,43 @@ pcl::RandomSample::applyFilter (PointCloud2 &output) output.point_step = input_->point_step; output.height = 1; + // Set random seed so derived indices are the same each time the filter runs + std::srand (seed_); + + unsigned top = N - sample_; + unsigned i = 0; + unsigned index = 0; + // Algorithm A - float one_over_N = 0.f; - float top = 0.f; - size_t index = 0; - std::vector added; - if (extract_removed_indices_) - added.resize (indices_->size (), false); - size_t i = 0; - - for (size_t n = sample_; n > 1; n--) + for (size_t n = sample_; n >= 2; n--) { - top = N - n; // N are the remaining number of elements, n the remaining number of wanted samples - one_over_N = 1.f / static_cast (N); //we need to re-calculate N^{-1} - float V = unifRand (); - size_t S = 0; - float quot = top * one_over_N; - + unsigned S = 0; + float quot = float (top) / float (N); while (quot > V) { - S ++; - N --; - quot = quot * (top * one_over_N); + S++; + top--; + N--; + quot = quot * float (top) / float (N); } - - N--; // this together with N-- above is the same than N - S - 1 (paper Vit84) index += S; - - memcpy (&output.data[i * output.point_step], &input_->data[index * output.point_step], output.point_step); - - i ++; - index ++; + memcpy (&output.data[i++ * output.point_step], &input_->data[index++ * output.point_step], output.point_step); + N--; } - index += static_cast (N * unifRand ()); - memcpy (&output.data[i * output.point_step], &input_->data[index * output.point_step], output.point_step); - } + index += N * static_cast (unifRand ()); + memcpy (&output.data[i++ * output.point_step], &input_->data[index++ * output.point_step], output.point_step); - output.width = sample_; - output.row_step = output.point_step * output.width; + output.width = sample_; + output.row_step = output.point_step * output.width; + } } /////////////////////////////////////////////////////////////////////////////// void pcl::RandomSample::applyFilter (std::vector &indices) { - rng_->base ().seed (seed_); - unsigned N = input_->width * input_->height; // If sample size is 0 or if the sample size is greater then input cloud size // then return all indices @@ -125,46 +112,33 @@ pcl::RandomSample::applyFilter (std::vector &indi // Resize output indices to sample size indices.resize (sample_); + // Set random seed so derived indices are the same each time the filter runs + std::srand (seed_); + + unsigned top = N - sample_; + unsigned i = 0; + unsigned index = 0; + // Algorithm A - float one_over_N = 0.f; - float top = 0.f; - size_t index = 0; - std::vector added; - if (extract_removed_indices_) - added.resize (indices_->size (), false); - size_t i = 0; - - for (size_t n = sample_; n > 1; n--) + for (size_t n = sample_; n >= 2; n--) { - top = N - n; // N are the remaining number of elements, n the remaining number of wanted samples - one_over_N = 1.f / static_cast (N); //we need to re-calculate N^{-1} - float V = unifRand (); - size_t S = 0; - float quot = top * one_over_N; - + unsigned S = 0; + float quot = float (top) / float (N); while (quot > V) { - S ++; - N --; - quot = quot * (top * one_over_N); + S++; + top--; + N--; + quot = quot * float (top) / float (N); } - - N--; // this together with N-- above is the same than N - S - 1 (paper Vit84) index += S; - - if (extract_removed_indices_) - added[index] = true; - indices[i] = (*indices_)[index]; - - i ++; - index ++; + indices[i++] = (*indices_)[index++]; + N--; } - index += static_cast (N * unifRand ()); - if (extract_removed_indices_) - added[index] = true; - indices[i] = (*indices_)[index]; + index += N * static_cast (unifRand ()); + indices[i++] = (*indices_)[index++]; } } @@ -172,7 +146,7 @@ pcl::RandomSample::applyFilter (std::vector &indi #include #include - PCL_INSTANTIATE(RandomSample, PCL_POINT_TYPES) +PCL_INSTANTIATE(RandomSample, PCL_POINT_TYPES) - #endif // PCL_NO_PRECOMPILE +#endif // PCL_NO_PRECOMPILE From 58c4b65ada301ef7e94f4b6948cb7f5db7c7c52c Mon Sep 17 00:00:00 2001 From: "Radu B. Rusu" Date: Sun, 24 Mar 2013 15:52:53 -0700 Subject: [PATCH 22/32] fixed a compile problem on older versions of GCC (e.g., 4.2.1 for MacOS) --- .../recognition/impl/ransac_based/simple_octree.hpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/recognition/include/pcl/recognition/impl/ransac_based/simple_octree.hpp b/recognition/include/pcl/recognition/impl/ransac_based/simple_octree.hpp index 2068a6fc24b..9aa87e8be04 100644 --- a/recognition/include/pcl/recognition/impl/ransac_based/simple_octree.hpp +++ b/recognition/include/pcl/recognition/impl/ransac_based/simple_octree.hpp @@ -255,11 +255,11 @@ pcl::recognition::SimpleOctree::build (const bounds_[5] = center[2] + half_root_side; // Create and initialize the root - root_ = new SimpleOctree::Node(); - root_->setCenter(center); - root_->setBounds(bounds_); - root_->setParent(NULL); - root_->computeRadius(); + root_ = new Node (); + root_->setCenter (center); + root_->setBounds (bounds_); + root_->setParent (NULL); + root_->computeRadius (); } //=============================================================================================================================== From 05b05c14f7599be19def77e1da4d6e27d1ee4398 Mon Sep 17 00:00:00 2001 From: Oliver Posniak Date: Thu, 21 Mar 2013 11:14:13 +0100 Subject: [PATCH 23/32] Compilation for VS2008 --- apps/src/openni_organized_edge_detection.cpp | 2 +- apps/src/openni_tracking.cpp | 4 ++-- apps/src/stereo_ground_segmentation.cpp | 8 ++++---- common/include/pcl/common/impl/random.hpp | 13 ++++++------ common/include/pcl/common/random.h | 21 ++++++++++---------- io/tools/hdl_grabber_example.cpp | 4 ++-- tools/mesh_sampling.cpp | 2 +- visualization/tools/image_grabber_viewer.cpp | 2 +- 8 files changed, 29 insertions(+), 27 deletions(-) diff --git a/apps/src/openni_organized_edge_detection.cpp b/apps/src/openni_organized_edge_detection.cpp index 635228a1bc1..d7caa8c20ed 100644 --- a/apps/src/openni_organized_edge_detection.cpp +++ b/apps/src/openni_organized_edge_detection.cpp @@ -203,7 +203,7 @@ class OpenNIOrganizedEdgeDetection // Make gray point cloud for (size_t idx = 0; idx < cloud_.points.size (); idx++) { - uint8_t gray = uint8_t((cloud_.points[idx].r + cloud_.points[idx].g + cloud_.points[idx].b)/3); + pcl::uint8_t gray = pcl::uint8_t((cloud_.points[idx].r + cloud_.points[idx].g + cloud_.points[idx].b)/3); cloud_.points[idx].r = cloud_.points[idx].g = cloud_.points[idx].b = gray; } diff --git a/apps/src/openni_tracking.cpp b/apps/src/openni_tracking.cpp index 49491bbc76b..4fd0d13839b 100644 --- a/apps/src/openni_tracking.cpp +++ b/apps/src/openni_tracking.cpp @@ -503,7 +503,7 @@ class OpenNISegmentTracking result.points.push_back(point); } - result.width = static_cast (result.points.size ()); + result.width = static_cast (result.points.size ()); result.height = 1; result.is_dense = true; } @@ -519,7 +519,7 @@ class OpenNISegmentTracking PointType point = cloud->points[segmented_indices.indices[i]]; result.points.push_back (point); } - result.width = uint32_t (result.points.size ()); + result.width = pcl::uint32_t (result.points.size ()); result.height = 1; result.is_dense = true; } diff --git a/apps/src/stereo_ground_segmentation.cpp b/apps/src/stereo_ground_segmentation.cpp index 30ce9e9a77b..8423900678c 100755 --- a/apps/src/stereo_ground_segmentation.cpp +++ b/apps/src/stereo_ground_segmentation.cpp @@ -277,7 +277,7 @@ class HRCSSegmentation cloud->points[region_indices[i].indices[j]].y, cloud->points[region_indices[i].indices[j]].z); ground_cloud->points.push_back (ground_pt); - ground_image->points[region_indices[i].indices[j]].g = static_cast ((cloud->points[region_indices[i].indices[j]].g + 255) / 2); + ground_image->points[region_indices[i].indices[j]].g = static_cast ((cloud->points[region_indices[i].indices[j]].g + 255) / 2); label_image->points[region_indices[i].indices[j]].r = 0; label_image->points[region_indices[i].indices[j]].g = 255; label_image->points[region_indices[i].indices[j]].b = 0; @@ -367,8 +367,8 @@ class HRCSSegmentation cloud->points[region_indices[i].indices[j]].y, cloud->points[region_indices[i].indices[j]].z); ground_cloud->points.push_back (ground_pt); - ground_image->points[region_indices[i].indices[j]].r = static_cast ((cloud->points[region_indices[i].indices[j]].r + 255) / 2); - ground_image->points[region_indices[i].indices[j]].g = static_cast ((cloud->points[region_indices[i].indices[j]].g + 255) / 2); + ground_image->points[region_indices[i].indices[j]].r = static_cast ((cloud->points[region_indices[i].indices[j]].r + 255) / 2); + ground_image->points[region_indices[i].indices[j]].g = static_cast ((cloud->points[region_indices[i].indices[j]].g + 255) / 2); label_image->points[region_indices[i].indices[j]].r = 128; label_image->points[region_indices[i].indices[j]].g = 128; label_image->points[region_indices[i].indices[j]].b = 0; @@ -455,7 +455,7 @@ class HRCSSegmentation { if (!pcl::isFinite (cloud->points[i])) { - ground_image->points[i].b = static_cast((cloud->points[i].b + 255) / 2); + ground_image->points[i].b = static_cast((cloud->points[i].b + 255) / 2); label_image->points[i].r = 0; label_image->points[i].g = 0; label_image->points[i].b = 255; diff --git a/common/include/pcl/common/impl/random.hpp b/common/include/pcl/common/impl/random.hpp index e05de8dc2a3..9a99e7c437e 100644 --- a/common/include/pcl/common/impl/random.hpp +++ b/common/include/pcl/common/impl/random.hpp @@ -41,10 +41,11 @@ #define PCL_COMMON_RANDOM_HPP_ #include +#include ///////////////////////////////////////////////////////////////////////////////////////////////////////// template -pcl::common::UniformGenerator::UniformGenerator(T min, T max, uint32_t seed) +pcl::common::UniformGenerator::UniformGenerator(T min, T max, pcl::uint32_t seed) : distribution_ (min, max) , generator_ (rng_, distribution_) { @@ -67,7 +68,7 @@ pcl::common::UniformGenerator::UniformGenerator(const Parameters& parameters) ///////////////////////////////////////////////////////////////////////////////////////////////////////// template void -pcl::common::UniformGenerator::setSeed (uint32_t seed) +pcl::common::UniformGenerator::setSeed (pcl::uint32_t seed) { if (seed != -1) { @@ -78,7 +79,7 @@ pcl::common::UniformGenerator::setSeed (uint32_t seed) ///////////////////////////////////////////////////////////////////////////////////////////////////////// template void -pcl::common::UniformGenerator::setParameters (T min, T max, uint32_t seed) +pcl::common::UniformGenerator::setParameters (T min, T max, pcl::uint32_t seed) { parameters_.min = min; parameters_.max = max; @@ -117,7 +118,7 @@ pcl::common::UniformGenerator::setParameters (const Parameters& parameters) ///////////////////////////////////////////////////////////////////////////////////////////////////////// template -pcl::common::NormalGenerator::NormalGenerator(T mean, T sigma, uint32_t seed) +pcl::common::NormalGenerator::NormalGenerator(T mean, T sigma, pcl::uint32_t seed) : distribution_ (mean, sigma) , generator_ (rng_, distribution_) { @@ -140,7 +141,7 @@ pcl::common::NormalGenerator::NormalGenerator(const Parameters& parameters) ///////////////////////////////////////////////////////////////////////////////////////////////////////// template void -pcl::common::NormalGenerator::setSeed (uint32_t seed) +pcl::common::NormalGenerator::setSeed (pcl::uint32_t seed) { if (seed != -1) { @@ -151,7 +152,7 @@ pcl::common::NormalGenerator::setSeed (uint32_t seed) ///////////////////////////////////////////////////////////////////////////////////////////////////////// template void -pcl::common::NormalGenerator::setParameters (T mean, T sigma, uint32_t seed) +pcl::common::NormalGenerator::setParameters (T mean, T sigma, pcl::uint32_t seed) { parameters_.mean = mean; parameters_.sigma = sigma; diff --git a/common/include/pcl/common/random.h b/common/include/pcl/common/random.h index ed08008a859..696018d1529 100644 --- a/common/include/pcl/common/random.h +++ b/common/include/pcl/common/random.h @@ -49,6 +49,7 @@ #include #include #include +#include namespace pcl { @@ -87,7 +88,7 @@ namespace pcl public: struct Parameters { - Parameters (T _min = 0, T _max = 1, uint32_t _seed = 1) + Parameters (T _min = 0, T _max = 1, pcl::uint32_t _seed = 1) : min (_min) , max (_max) , seed (_seed) @@ -95,7 +96,7 @@ namespace pcl T min; T max; - uint32_t seed; + pcl::uint32_t seed; }; /** Constructor @@ -103,7 +104,7 @@ namespace pcl * \param max: included higher bound * \param seed: seeding value */ - UniformGenerator(T min = 0, T max = 1, uint32_t seed = -1); + UniformGenerator(T min = 0, T max = 1, pcl::uint32_t seed = -1); /** Constructor * \param parameters uniform distribution parameters and generator seed @@ -114,7 +115,7 @@ namespace pcl * \param[in] seed new generator seed value */ void - setSeed (uint32_t seed); + setSeed (pcl::uint32_t seed); /** Set the uniform number generator parameters * \param[in] min minimum allowed value @@ -122,7 +123,7 @@ namespace pcl * \param[in] seed random number generator seed (applied if != -1) */ void - setParameters (T min, T max, uint32_t seed = -1); + setParameters (T min, T max, pcl::uint32_t seed = -1); /** Set generator parameters * \param parameters uniform distribution parameters and generator seed @@ -162,7 +163,7 @@ namespace pcl public: struct Parameters { - Parameters (T _mean = 0, T _sigma = 1, uint32_t _seed = 1) + Parameters (T _mean = 0, T _sigma = 1, pcl::uint32_t _seed = 1) : mean (_mean) , sigma (_sigma) , seed (_seed) @@ -170,7 +171,7 @@ namespace pcl T mean; T sigma; - uint32_t seed; + pcl::uint32_t seed; }; /** Constructor @@ -178,7 +179,7 @@ namespace pcl * \param[in] sigma normal variation * \param[in] seed seeding value */ - NormalGenerator(T mean = 0, T sigma = 1, uint32_t seed = -1); + NormalGenerator(T mean = 0, T sigma = 1, pcl::uint32_t seed = -1); /** Constructor * \param parameters normal distribution parameters and seed @@ -189,7 +190,7 @@ namespace pcl * \param[in] seed new seed value */ void - setSeed (uint32_t seed); + setSeed (pcl::uint32_t seed); /** Set the normal number generator parameters * \param[in] mean mean of the normal distribution @@ -197,7 +198,7 @@ namespace pcl * \param[in] seed random number generator seed (applied if != -1) */ void - setParameters (T mean, T sigma, uint32_t seed = -1); + setParameters (T mean, T sigma, pcl::uint32_t seed = -1); /** Set generator parameters * \param parameters normal distribution parameters and seed diff --git a/io/tools/hdl_grabber_example.cpp b/io/tools/hdl_grabber_example.cpp index b8cde5676e1..5b2f55b064b 100644 --- a/io/tools/hdl_grabber_example.cpp +++ b/io/tools/hdl_grabber_example.cpp @@ -49,14 +49,14 @@ class SimpleHDLGrabber static double last = pcl::getTime(); if (sweep->header.seq == 0) { - uint64_t stamp; + pcl::uint64_t stamp; #ifdef USE_ROS stamp = sweep->header.stamp.toNSec () / 1000; #else //USE_ROS stamp = sweep->header.stamp; #endif //USE_ROS time_t systemTime = static_cast(((stamp & 0xffffffff00000000l) >> 32) & 0x00000000ffffffff); - uint32_t usec = static_cast(stamp & 0x00000000ffffffff); + pcl::uint32_t usec = static_cast(stamp & 0x00000000ffffffff); std::cout << std::hex << stamp << " " << ctime(&systemTime) << " usec: " << usec << std::endl; } diff --git a/tools/mesh_sampling.cpp b/tools/mesh_sampling.cpp index 73bfd713ae3..d25760e55c9 100644 --- a/tools/mesh_sampling.cpp +++ b/tools/mesh_sampling.cpp @@ -118,7 +118,7 @@ uniform_sampling (vtkSmartPointer polydata, size_t n_samples, pcl:: } cloud_out.points.resize (n_samples); - cloud_out.width = static_cast (n_samples); + cloud_out.width = static_cast (n_samples); cloud_out.height = 1; for (i = 0; i < n_samples; i++) diff --git a/visualization/tools/image_grabber_viewer.cpp b/visualization/tools/image_grabber_viewer.cpp index 32708566302..32dd1e8e62e 100644 --- a/visualization/tools/image_grabber_viewer.cpp +++ b/visualization/tools/image_grabber_viewer.cpp @@ -95,7 +95,7 @@ struct EventHelper void cloud_cb (const pcl::PointCloud::ConstPtr & cloud) { - uint64_t timestamp; + pcl::uint64_t timestamp; #ifdef USE_ROS timestamp = cloud->header.stamp.toNSec() / 1000; //Microseconds #else From c6e9b265e050a9a96768bce21536fff9682489a3 Mon Sep 17 00:00:00 2001 From: Alexandru Ichim Date: Mon, 25 Mar 2013 14:48:54 -0700 Subject: [PATCH 24/32] demean cloud tool --- tools/CMakeLists.txt | 4 ++ tools/demean_cloud.cpp | 123 +++++++++++++++++++++++++++++++++++++++++ 2 files changed, 127 insertions(+) create mode 100644 tools/demean_cloud.cpp diff --git a/tools/CMakeLists.txt b/tools/CMakeLists.txt index 9d6ffba2057..cbd4cacf9dd 100644 --- a/tools/CMakeLists.txt +++ b/tools/CMakeLists.txt @@ -130,6 +130,10 @@ if (build) PCL_ADD_EXECUTABLE(pcl_fast_bilateral_filter ${SUBSYS_NAME} fast_bilateral_filter.cpp) target_link_libraries(pcl_fast_bilateral_filter pcl_common pcl_io pcl_filters) + PCL_ADD_EXECUTABLE(pcl_demean_cloud ${SUBSYS_NAME} demean_cloud.cpp) + target_link_libraries(pcl_demean_cloud pcl_common pcl_io) + + if(BUILD_OPENNI AND OPENNI_FOUND) PCL_ADD_EXECUTABLE(pcl_oni2pcd ${SUBSYS_NAME} oni2pcd.cpp) target_link_libraries(pcl_oni2pcd pcl_common pcl_io) diff --git a/tools/demean_cloud.cpp b/tools/demean_cloud.cpp new file mode 100644 index 00000000000..51a6443e3a1 --- /dev/null +++ b/tools/demean_cloud.cpp @@ -0,0 +1,123 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2013, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include +#include +#include +#include +#include + +using namespace pcl; +using namespace pcl::io; +using namespace pcl::console; + +void +printHelp (int, char **argv) +{ + print_error ("Syntax is: %s input.pcd output.pcd\n", argv[0]); +} + +bool +loadCloud (const std::string &filename, sensor_msgs::PointCloud2 &cloud) +{ + TicToc tt; + print_highlight ("Loading "); print_value ("%s ", filename.c_str ()); + + tt.tic (); + if (loadPCDFile (filename, cloud) < 0) + return (false); + print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", cloud.width * cloud.height); print_info (" points]\n"); + print_info ("Available dimensions: "); print_value ("%s\n", pcl::getFieldsList (cloud).c_str ()); + + return (true); +} + +void +saveCloud (const std::string &filename, const sensor_msgs::PointCloud2 &cloud) +{ + TicToc tt; + tt.tic (); + + print_highlight ("Saving "); print_value ("%s ", filename.c_str ()); + + pcl::PCDWriter writer; + writer.writeBinaryCompressed (filename, cloud); + print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", cloud.width * cloud.height); print_info (" points]\n"); +} + +/* ---[ */ +int +main (int argc, char** argv) +{ + print_info ("Convert a PCD file to PLY format. For more information, use: %s -h\n", argv[0]); + + if (argc < 3) + { + printHelp (argc, argv); + return (-1); + } + + // Parse the command line arguments for .pcd files + std::vector pcd_file_indices = parse_file_extension_argument (argc, argv, ".pcd"); + if (pcd_file_indices.size () != 2) + { + print_error ("Need one input and one output PCD file.\n"); + return (-1); + } + + // Load the first file + sensor_msgs::PointCloud2 cloud; + if (!loadCloud (argv[pcd_file_indices[0]], cloud)) + return (-1); + + PointCloud cloud_xyz, cloud_xyz_demeaned; + fromROSMsg (cloud, cloud_xyz); + Eigen::Vector4f centroid; + compute3DCentroid (cloud_xyz, centroid); + demeanPointCloud (cloud_xyz, centroid, cloud_xyz_demeaned); + + toROSMsg (cloud_xyz_demeaned, cloud); + sensor_msgs::PointCloud2 cloud_out; + copyPointCloud (cloud, cloud_out); + + + // Convert to PLY and save + saveCloud (argv[pcd_file_indices[1]], cloud_out); + + return (0); +} + From cdffb4d650a48393159df0d18c8f35b5a804da05 Mon Sep 17 00:00:00 2001 From: Mourad Boufarguine Date: Tue, 26 Mar 2013 00:05:11 +0100 Subject: [PATCH 25/32] fix pcl::copyPointCloud --- common/include/pcl/common/impl/io.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/common/include/pcl/common/impl/io.hpp b/common/include/pcl/common/impl/io.hpp index 005c3bbfdfe..4504c1cfc33 100644 --- a/common/include/pcl/common/impl/io.hpp +++ b/common/include/pcl/common/impl/io.hpp @@ -368,7 +368,7 @@ pcl::copyPointCloud (const pcl::PointCloud &cloud_in, // Copy the rest pcl::for_each_type (pcl::NdConcatenateFunctor (cloud_in.points[indices[i]], cloud_out.points[i])); // Copy RGB<->RGBA - memcpy (reinterpret_cast (&cloud_out.points[indices[i]]) + fields_out[rgb_idx_out].offset, reinterpret_cast (&cloud_in.points[i]) + fields_in[rgb_idx_in].offset, field_size_in); + memcpy (reinterpret_cast (&cloud_out.points[i]) + fields_out[rgb_idx_out].offset, reinterpret_cast (&cloud_in.points[indices[i]]) + fields_in[rgb_idx_in].offset, field_size_in); } return; } From 96f925a315530ea16b62cacb940bf06ef16db798 Mon Sep 17 00:00:00 2001 From: Alexandru Ichim Date: Mon, 25 Mar 2013 16:09:35 -0700 Subject: [PATCH 26/32] fix --- tools/demean_cloud.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/tools/demean_cloud.cpp b/tools/demean_cloud.cpp index 51a6443e3a1..cfce76be707 100644 --- a/tools/demean_cloud.cpp +++ b/tools/demean_cloud.cpp @@ -110,12 +110,12 @@ main (int argc, char** argv) compute3DCentroid (cloud_xyz, centroid); demeanPointCloud (cloud_xyz, centroid, cloud_xyz_demeaned); - toROSMsg (cloud_xyz_demeaned, cloud); + sensor_msgs::PointCloud2 cloud2_xyz_demeaned; + toROSMsg (cloud_xyz_demeaned, cloud2_xyz_demeaned); sensor_msgs::PointCloud2 cloud_out; - copyPointCloud (cloud, cloud_out); + concatenateFields (cloud, cloud2_xyz_demeaned, cloud_out); - - // Convert to PLY and save + // Save cloud saveCloud (argv[pcd_file_indices[1]], cloud_out); return (0); From 9e7eb3b97efbeaa93519a23600527eaf15b465d1 Mon Sep 17 00:00:00 2001 From: Julius Kammerl Date: Mon, 25 Mar 2013 22:02:31 -0700 Subject: [PATCH 27/32] fixing several doxygen errors & adjusting octree to pcl programming style guide --- .../impl/octree_pointcloud_compression.hpp | 450 +++++----- .../octree_pointcloud_compression.h | 136 +-- .../pcl/octree/impl/octree2buf_base.hpp | 517 ++++++------ .../include/pcl/octree/impl/octree_base.hpp | 773 +++++++++--------- .../pcl/octree/impl/octree_iterator.hpp | 110 +-- .../pcl/octree/impl/octree_pointcloud.hpp | 416 +++++----- .../impl/octree_pointcloud_supervoxel.hpp | 2 +- .../impl/octree_pointcloud_voxelcentroid.hpp | 12 +- .../include/pcl/octree/impl/octree_search.hpp | 588 +++++++------ octree/include/pcl/octree/octree2buf_base.h | 363 ++++---- octree/include/pcl/octree/octree_base.h | 294 +++---- octree/include/pcl/octree/octree_container.h | 23 +- octree/include/pcl/octree/octree_iterator.h | 122 +-- octree/include/pcl/octree/octree_key.h | 21 +- octree/include/pcl/octree/octree_node_pool.h | 2 +- octree/include/pcl/octree/octree_nodes.h | 46 +- octree/include/pcl/octree/octree_pointcloud.h | 161 ++-- .../pcl/octree/octree_pointcloud_density.h | 20 +- .../pcl/octree/octree_pointcloud_supervoxel.h | 2 +- .../octree/octree_pointcloud_voxelcentroid.h | 3 +- octree/include/pcl/octree/octree_search.h | 303 ++++--- .../include/pcl/outofcore/octree_base_node.h | 4 +- registration/include/pcl/registration/gicp.h | 6 +- .../include/pcl/surface/impl/concave_hull.hpp | 3 + 24 files changed, 2200 insertions(+), 2177 deletions(-) diff --git a/io/include/pcl/compression/impl/octree_pointcloud_compression.hpp b/io/include/pcl/compression/impl/octree_pointcloud_compression.hpp index 3d4c9ae174f..c3e75bf6b6c 100644 --- a/io/include/pcl/compression/impl/octree_pointcloud_compression.hpp +++ b/io/include/pcl/compression/impl/octree_pointcloud_compression.hpp @@ -58,9 +58,9 @@ namespace pcl template void OctreePointCloudCompression< PointT, LeafT, BranchT, OctreeT>::encodePointCloud ( const PointCloudConstPtr &cloud_arg, - std::ostream& compressedTreeDataOut_arg) + std::ostream& compressed_tree_data_out_arg) { - unsigned char recentTreeDepth = + unsigned char recent_tree_depth = static_cast (this->getTreeDepth ()); // initialize octree @@ -70,11 +70,11 @@ namespace pcl this->addPointsFromInputCloud (); // make sure cloud contains points - if (this->leafCount_>0) { + if (this->leaf_count_>0) { // color field analysis - cloudWithColor_ = false; + cloud_with_color_ = false; std::vector fields; int rgba_index = -1; rgba_index = pcl::getFieldIndex (*this->input_, "rgb", fields); @@ -84,112 +84,112 @@ namespace pcl } if (rgba_index >= 0) { - pointColorOffset_ = static_cast (fields[rgba_index].offset); - cloudWithColor_ = true; + point_color_offset_ = static_cast (fields[rgba_index].offset); + cloud_with_color_ = true; } // apply encoding configuration - cloudWithColor_ &= doColorEncoding_; + cloud_with_color_ &= do_color_encoding_; // if octree depth changed, we enforce I-frame encoding - iFrame_ |= (recentTreeDepth != this->getTreeDepth ());// | !(iFrameCounter%10); + i_frame_ |= (recent_tree_depth != this->getTreeDepth ());// | !(iFrameCounter%10); // enable I-frame rate - if (iFrameCounter_++==iFrameRate_) + if (i_frame_counter_++==i_frame_rate_) { - iFrameCounter_ =0; - iFrame_ = true; + i_frame_counter_ =0; + i_frame_ = true; } // increase frameID - frameID_++; + frame_ID_++; // do octree encoding - if (!doVoxelGridEnDecoding_) + if (!do_voxel_grid_enDecoding_) { - pointCountDataVector_.clear (); - pointCountDataVector_.reserve (cloud_arg->points.size ()); + point_count_data_vector_.clear (); + point_count_data_vector_.reserve (cloud_arg->points.size ()); } // initialize color encoding - colorCoder_.initializeEncoding (); - colorCoder_.setPointCount (static_cast (cloud_arg->points.size ())); - colorCoder_.setVoxelCount (static_cast (this->leafCount_)); + color_coder_.initializeEncoding (); + color_coder_.setPointCount (static_cast (cloud_arg->points.size ())); + color_coder_.setVoxelCount (static_cast (this->leaf_count_)); // initialize point encoding - pointCoder_.initializeEncoding (); - pointCoder_.setPointCount (static_cast (cloud_arg->points.size ())); + point_coder_.initializeEncoding (); + point_coder_.setPointCount (static_cast (cloud_arg->points.size ())); // serialize octree - if (iFrame_) + if (i_frame_) // i-frame encoding - encode tree structure without referencing previous buffer - this->serializeTree (binaryTreeDataVector_, false); + this->serializeTree (binary_tree_data_vector_, false); else // p-frame encoding - XOR encoded tree structure - this->serializeTree (binaryTreeDataVector_, true); + this->serializeTree (binary_tree_data_vector_, true); // write frame header information to stream - this->writeFrameHeader (compressedTreeDataOut_arg); + this->writeFrameHeader (compressed_tree_data_out_arg); // apply entropy coding to the content of all data vectors and send data to output stream - this->entropyEncoding (compressedTreeDataOut_arg); + this->entropyEncoding (compressed_tree_data_out_arg); // prepare for next frame this->switchBuffers (); - iFrame_ = false; + i_frame_ = false; // reset object count - objectCount_ = 0; + object_count_ = 0; - if (bShowStatistics) + if (b_show_statistics_) { - float bytesPerXYZ = static_cast (compressedPointDataLen_) / static_cast (pointCount_); - float bytesPerColor = static_cast (compressedColorDataLen_) / static_cast (pointCount_); + float bytes_per_XYZ = static_cast (compressed_point_data_len_) / static_cast (point_count_); + float bytes_per_color = static_cast (compressed_color_data_len_) / static_cast (point_count_); PCL_INFO ("*** POINTCLOUD ENCODING ***\n"); - PCL_INFO ("Frame ID: %d\n", frameID_); - if (iFrame_) + PCL_INFO ("Frame ID: %d\n", frame_ID_); + if (i_frame_) PCL_INFO ("Encoding Frame: Intra frame\n"); else PCL_INFO ("Encoding Frame: Prediction frame\n"); - PCL_INFO ("Number of encoded points: %ld\n", pointCount_); - PCL_INFO ("XYZ compression percentage: %f%%\n", bytesPerXYZ / (3.0f * sizeof(float)) * 100.0f); - PCL_INFO ("XYZ bytes per point: %f bytes\n", bytesPerXYZ); - PCL_INFO ("Color compression percentage: %f%%\n", bytesPerColor / (sizeof (int)) * 100.0f); - PCL_INFO ("Color bytes per point: %f bytes\n", bytesPerColor); - PCL_INFO ("Size of uncompressed point cloud: %f kBytes\n", static_cast (pointCount_) * (sizeof (int) + 3.0f * sizeof (float)) / 1024); - PCL_INFO ("Size of compressed point cloud: %d kBytes\n", (compressedPointDataLen_ + compressedColorDataLen_) / (1024)); - PCL_INFO ("Total bytes per point: %f\n", bytesPerXYZ + bytesPerColor); - PCL_INFO ("Total compression percentage: %f\n", (bytesPerXYZ + bytesPerColor) / (sizeof (int) + 3.0f * sizeof(float)) * 100.0f); - PCL_INFO ("Compression ratio: %f\n\n", static_cast (sizeof (int) + 3.0f * sizeof (float)) / static_cast (bytesPerXYZ + bytesPerColor)); + PCL_INFO ("Number of encoded points: %ld\n", point_count_); + PCL_INFO ("XYZ compression percentage: %f%%\n", bytes_per_XYZ / (3.0f * sizeof(float)) * 100.0f); + PCL_INFO ("XYZ bytes per point: %f bytes\n", bytes_per_XYZ); + PCL_INFO ("Color compression percentage: %f%%\n", bytes_per_color / (sizeof (int)) * 100.0f); + PCL_INFO ("Color bytes per point: %f bytes\n", bytes_per_color); + PCL_INFO ("Size of uncompressed point cloud: %f kBytes\n", static_cast (point_count_) * (sizeof (int) + 3.0f * sizeof (float)) / 1024); + PCL_INFO ("Size of compressed point cloud: %d kBytes\n", (compressed_point_data_len_ + compressed_color_data_len_) / (1024)); + PCL_INFO ("Total bytes per point: %f\n", bytes_per_XYZ + bytes_per_color); + PCL_INFO ("Total compression percentage: %f\n", (bytes_per_XYZ + bytes_per_color) / (sizeof (int) + 3.0f * sizeof(float)) * 100.0f); + PCL_INFO ("Compression ratio: %f\n\n", static_cast (sizeof (int) + 3.0f * sizeof (float)) / static_cast (bytes_per_XYZ + bytes_per_color)); } } else { - if (bShowStatistics) + if (b_show_statistics_) PCL_INFO ("Info: Dropping empty point cloud\n"); this->deleteTree(); - iFrameCounter_ = 0; - iFrame_ = true; + i_frame_counter_ = 0; + i_frame_ = true; } } ////////////////////////////////////////////////////////////////////////////////////////////// template void OctreePointCloudCompression::decodePointCloud ( - std::istream& compressedTreeDataIn_arg, + std::istream& compressed_tree_data_in_arg, PointCloudPtr &cloud_arg) { // synchronize to frame header - syncToHeader(compressedTreeDataIn_arg); + syncToHeader(compressed_tree_data_in_arg); // initialize octree this->switchBuffers (); this->setOutputCloud (cloud_arg); // color field analysis - cloudWithColor_ = false; + cloud_with_color_ = false; std::vector fields; int rgba_index = -1; rgba_index = pcl::getFieldIndex (*output_, "rgb", fields); @@ -197,172 +197,172 @@ namespace pcl rgba_index = pcl::getFieldIndex (*output_, "rgba", fields); if (rgba_index >= 0) { - pointColorOffset_ = static_cast (fields[rgba_index].offset); - cloudWithColor_ = true; + point_color_offset_ = static_cast (fields[rgba_index].offset); + cloud_with_color_ = true; } // read header from input stream - this->readFrameHeader (compressedTreeDataIn_arg); + this->readFrameHeader (compressed_tree_data_in_arg); // decode data vectors from stream - this->entropyDecoding (compressedTreeDataIn_arg); + this->entropyDecoding (compressed_tree_data_in_arg); // initialize color and point encoding - colorCoder_.initializeDecoding (); - pointCoder_.initializeDecoding (); + color_coder_.initializeDecoding (); + point_coder_.initializeDecoding (); // initialize output cloud output_->points.clear (); - output_->points.reserve (static_cast (pointCount_)); + output_->points.reserve (static_cast (point_count_)); - if (iFrame_) + if (i_frame_) // i-frame decoding - decode tree structure without referencing previous buffer - this->deserializeTree (binaryTreeDataVector_, false); + this->deserializeTree (binary_tree_data_vector_, false); else // p-frame decoding - decode XOR encoded tree structure - this->deserializeTree (binaryTreeDataVector_, true); + this->deserializeTree (binary_tree_data_vector_, true); // assign point cloud properties output_->height = 1; output_->width = static_cast (cloud_arg->points.size ()); output_->is_dense = false; - if (bShowStatistics) + if (b_show_statistics_) { - float bytesPerXYZ = static_cast (compressedPointDataLen_) / static_cast (pointCount_); - float bytesPerColor = static_cast (compressedColorDataLen_) / static_cast (pointCount_); + float bytes_per_XYZ = static_cast (compressed_point_data_len_) / static_cast (point_count_); + float bytes_per_color = static_cast (compressed_color_data_len_) / static_cast (point_count_); PCL_INFO ("*** POINTCLOUD DECODING ***\n"); - PCL_INFO ("Frame ID: %d\n", frameID_); - if (iFrame_) + PCL_INFO ("Frame ID: %d\n", frame_ID_); + if (i_frame_) PCL_INFO ("Encoding Frame: Intra frame\n"); else PCL_INFO ("Encoding Frame: Prediction frame\n"); - PCL_INFO ("Number of encoded points: %ld\n", pointCount_); - PCL_INFO ("XYZ compression percentage: %f%%\n", bytesPerXYZ / (3.0f * sizeof (float)) * 100.0f); - PCL_INFO ("XYZ bytes per point: %f bytes\n", bytesPerXYZ); - PCL_INFO ("Color compression percentage: %f%%\n", bytesPerColor / (sizeof (int)) * 100.0f); - PCL_INFO ("Color bytes per point: %f bytes\n", bytesPerColor); - PCL_INFO ("Size of uncompressed point cloud: %f kBytes\n", static_cast (pointCount_) * (sizeof (int) + 3.0f * sizeof (float)) / 1024.0f); - PCL_INFO ("Size of compressed point cloud: %f kBytes\n", static_cast (compressedPointDataLen_ + compressedColorDataLen_) / 1024.0f); - PCL_INFO ("Total bytes per point: %d bytes\n", static_cast (bytesPerXYZ + bytesPerColor)); - PCL_INFO ("Total compression percentage: %f%%\n", (bytesPerXYZ + bytesPerColor) / (sizeof (int) + 3.0f * sizeof (float)) * 100.0f); - PCL_INFO ("Compression ratio: %f\n\n", static_cast (sizeof (int) + 3.0f * sizeof (float)) / static_cast (bytesPerXYZ + bytesPerColor)); + PCL_INFO ("Number of encoded points: %ld\n", point_count_); + PCL_INFO ("XYZ compression percentage: %f%%\n", bytes_per_XYZ / (3.0f * sizeof (float)) * 100.0f); + PCL_INFO ("XYZ bytes per point: %f bytes\n", bytes_per_XYZ); + PCL_INFO ("Color compression percentage: %f%%\n", bytes_per_color / (sizeof (int)) * 100.0f); + PCL_INFO ("Color bytes per point: %f bytes\n", bytes_per_color); + PCL_INFO ("Size of uncompressed point cloud: %f kBytes\n", static_cast (point_count_) * (sizeof (int) + 3.0f * sizeof (float)) / 1024.0f); + PCL_INFO ("Size of compressed point cloud: %f kBytes\n", static_cast (compressed_point_data_len_ + compressed_color_data_len_) / 1024.0f); + PCL_INFO ("Total bytes per point: %d bytes\n", static_cast (bytes_per_XYZ + bytes_per_color)); + PCL_INFO ("Total compression percentage: %f%%\n", (bytes_per_XYZ + bytes_per_color) / (sizeof (int) + 3.0f * sizeof (float)) * 100.0f); + PCL_INFO ("Compression ratio: %f\n\n", static_cast (sizeof (int) + 3.0f * sizeof (float)) / static_cast (bytes_per_XYZ + bytes_per_color)); } } ////////////////////////////////////////////////////////////////////////////////////////////// template void - OctreePointCloudCompression::entropyEncoding (std::ostream& compressedTreeDataOut_arg) + OctreePointCloudCompression::entropyEncoding (std::ostream& compressed_tree_data_out_arg) { - uint64_t binaryTreeDataVector_size; - uint64_t pointAvgColorDataVector_size; + uint64_t binary_tree_data_vector_size; + uint64_t point_avg_color_data_vector_size; - compressedPointDataLen_ = 0; - compressedColorDataLen_ = 0; + compressed_point_data_len_ = 0; + compressed_color_data_len_ = 0; // encode binary octree structure - binaryTreeDataVector_size = binaryTreeDataVector_.size (); - compressedTreeDataOut_arg.write (reinterpret_cast (&binaryTreeDataVector_size), sizeof (binaryTreeDataVector_size)); - compressedPointDataLen_ += entropyCoder_.encodeCharVectorToStream (binaryTreeDataVector_, - compressedTreeDataOut_arg); + binary_tree_data_vector_size = binary_tree_data_vector_.size (); + compressed_tree_data_out_arg.write (reinterpret_cast (&binary_tree_data_vector_size), sizeof (binary_tree_data_vector_size)); + compressed_point_data_len_ += entropy_coder_.encodeCharVectorToStream (binary_tree_data_vector_, + compressed_tree_data_out_arg); - if (cloudWithColor_) + if (cloud_with_color_) { // encode averaged voxel color information - std::vector& pointAvgColorDataVector = colorCoder_.getAverageDataVector (); - pointAvgColorDataVector_size = pointAvgColorDataVector.size (); - compressedTreeDataOut_arg.write (reinterpret_cast (&pointAvgColorDataVector_size), - sizeof (pointAvgColorDataVector_size)); - compressedColorDataLen_ += entropyCoder_.encodeCharVectorToStream (pointAvgColorDataVector, - compressedTreeDataOut_arg); + std::vector& pointAvgColorDataVector = color_coder_.getAverageDataVector (); + point_avg_color_data_vector_size = pointAvgColorDataVector.size (); + compressed_tree_data_out_arg.write (reinterpret_cast (&point_avg_color_data_vector_size), + sizeof (point_avg_color_data_vector_size)); + compressed_color_data_len_ += entropy_coder_.encodeCharVectorToStream (pointAvgColorDataVector, + compressed_tree_data_out_arg); } - if (!doVoxelGridEnDecoding_) + if (!do_voxel_grid_enDecoding_) { uint64_t pointCountDataVector_size; - uint64_t pointDiffDataVector_size; - uint64_t pointDiffColorDataVector_size; + uint64_t point_diff_data_vector_size; + uint64_t point_diff_color_data_vector_size; // encode amount of points per voxel - pointCountDataVector_size = pointCountDataVector_.size (); - compressedTreeDataOut_arg.write (reinterpret_cast (&pointCountDataVector_size), sizeof (pointCountDataVector_size)); - compressedPointDataLen_ += entropyCoder_.encodeIntVectorToStream (pointCountDataVector_, - compressedTreeDataOut_arg); + pointCountDataVector_size = point_count_data_vector_.size (); + compressed_tree_data_out_arg.write (reinterpret_cast (&pointCountDataVector_size), sizeof (pointCountDataVector_size)); + compressed_point_data_len_ += entropy_coder_.encodeIntVectorToStream (point_count_data_vector_, + compressed_tree_data_out_arg); // encode differential point information - std::vector& pointDiffDataVector = pointCoder_.getDifferentialDataVector (); - pointDiffDataVector_size = pointDiffDataVector.size (); - compressedTreeDataOut_arg.write (reinterpret_cast (&pointDiffDataVector_size), sizeof (pointDiffDataVector_size)); - compressedPointDataLen_ += entropyCoder_.encodeCharVectorToStream (pointDiffDataVector, - compressedTreeDataOut_arg); - if (cloudWithColor_) + std::vector& point_diff_data_vector = point_coder_.getDifferentialDataVector (); + point_diff_data_vector_size = point_diff_data_vector.size (); + compressed_tree_data_out_arg.write (reinterpret_cast (&point_diff_data_vector_size), sizeof (point_diff_data_vector_size)); + compressed_point_data_len_ += entropy_coder_.encodeCharVectorToStream (point_diff_data_vector, + compressed_tree_data_out_arg); + if (cloud_with_color_) { // encode differential color information - std::vector& pointDiffColorDataVector = colorCoder_.getDifferentialDataVector (); - pointDiffColorDataVector_size = pointDiffColorDataVector.size (); - compressedTreeDataOut_arg.write (reinterpret_cast (&pointDiffColorDataVector_size), - sizeof (pointDiffColorDataVector_size)); - compressedColorDataLen_ += entropyCoder_.encodeCharVectorToStream (pointDiffColorDataVector, - compressedTreeDataOut_arg); + std::vector& point_diff_color_data_vector = color_coder_.getDifferentialDataVector (); + point_diff_color_data_vector_size = point_diff_color_data_vector.size (); + compressed_tree_data_out_arg.write (reinterpret_cast (&point_diff_color_data_vector_size), + sizeof (point_diff_color_data_vector_size)); + compressed_color_data_len_ += entropy_coder_.encodeCharVectorToStream (point_diff_color_data_vector, + compressed_tree_data_out_arg); } } // flush output stream - compressedTreeDataOut_arg.flush (); + compressed_tree_data_out_arg.flush (); } ////////////////////////////////////////////////////////////////////////////////////////////// template void - OctreePointCloudCompression::entropyDecoding (std::istream& compressedTreeDataIn_arg) + OctreePointCloudCompression::entropyDecoding (std::istream& compressed_tree_data_in_arg) { - uint64_t binaryTreeDataVector_size; - uint64_t pointAvgColorDataVector_size; + uint64_t binary_tree_data_vector_size; + uint64_t point_avg_color_data_vector_size; - compressedPointDataLen_ = 0; - compressedColorDataLen_ = 0; + compressed_point_data_len_ = 0; + compressed_color_data_len_ = 0; // decode binary octree structure - compressedTreeDataIn_arg.read (reinterpret_cast (&binaryTreeDataVector_size), sizeof (binaryTreeDataVector_size)); - binaryTreeDataVector_.resize (static_cast (binaryTreeDataVector_size)); - compressedPointDataLen_ += entropyCoder_.decodeStreamToCharVector (compressedTreeDataIn_arg, - binaryTreeDataVector_); + compressed_tree_data_in_arg.read (reinterpret_cast (&binary_tree_data_vector_size), sizeof (binary_tree_data_vector_size)); + binary_tree_data_vector_.resize (static_cast (binary_tree_data_vector_size)); + compressed_point_data_len_ += entropy_coder_.decodeStreamToCharVector (compressed_tree_data_in_arg, + binary_tree_data_vector_); - if (dataWithColor_) + if (data_with_color_) { // decode averaged voxel color information - std::vector& pointAvgColorDataVector = colorCoder_.getAverageDataVector (); - compressedTreeDataIn_arg.read (reinterpret_cast (&pointAvgColorDataVector_size), sizeof (pointAvgColorDataVector_size)); - pointAvgColorDataVector.resize (static_cast (pointAvgColorDataVector_size)); - compressedColorDataLen_ += entropyCoder_.decodeStreamToCharVector (compressedTreeDataIn_arg, - pointAvgColorDataVector); + std::vector& point_avg_color_data_vector = color_coder_.getAverageDataVector (); + compressed_tree_data_in_arg.read (reinterpret_cast (&point_avg_color_data_vector_size), sizeof (point_avg_color_data_vector_size)); + point_avg_color_data_vector.resize (static_cast (point_avg_color_data_vector_size)); + compressed_color_data_len_ += entropy_coder_.decodeStreamToCharVector (compressed_tree_data_in_arg, + point_avg_color_data_vector); } - if (!doVoxelGridEnDecoding_) + if (!do_voxel_grid_enDecoding_) { - uint64_t pointCountDataVector_size; - uint64_t pointDiffDataVector_size; - uint64_t pointDiffColorDataVector_size; + uint64_t point_count_data_vector_size; + uint64_t point_diff_data_vector_size; + uint64_t point_diff_color_data_vector_size; // decode amount of points per voxel - compressedTreeDataIn_arg.read (reinterpret_cast (&pointCountDataVector_size), sizeof (pointCountDataVector_size)); - pointCountDataVector_.resize (static_cast (pointCountDataVector_size)); - compressedPointDataLen_ += entropyCoder_.decodeStreamToIntVector (compressedTreeDataIn_arg, pointCountDataVector_); - pointCountDataVectorIterator_ = pointCountDataVector_.begin (); + compressed_tree_data_in_arg.read (reinterpret_cast (&point_count_data_vector_size), sizeof (point_count_data_vector_size)); + point_count_data_vector_.resize (static_cast (point_count_data_vector_size)); + compressed_point_data_len_ += entropy_coder_.decodeStreamToIntVector (compressed_tree_data_in_arg, point_count_data_vector_); + point_count_data_vector_iterator_ = point_count_data_vector_.begin (); // decode differential point information - std::vector& pointDiffDataVector = pointCoder_.getDifferentialDataVector (); - compressedTreeDataIn_arg.read (reinterpret_cast (&pointDiffDataVector_size), sizeof (pointDiffDataVector_size)); - pointDiffDataVector.resize (static_cast (pointDiffDataVector_size)); - compressedPointDataLen_ += entropyCoder_.decodeStreamToCharVector (compressedTreeDataIn_arg, + std::vector& pointDiffDataVector = point_coder_.getDifferentialDataVector (); + compressed_tree_data_in_arg.read (reinterpret_cast (&point_diff_data_vector_size), sizeof (point_diff_data_vector_size)); + pointDiffDataVector.resize (static_cast (point_diff_data_vector_size)); + compressed_point_data_len_ += entropy_coder_.decodeStreamToCharVector (compressed_tree_data_in_arg, pointDiffDataVector); - if (dataWithColor_) + if (data_with_color_) { // decode differential color information - std::vector& pointDiffColorDataVector = colorCoder_.getDifferentialDataVector (); - compressedTreeDataIn_arg.read (reinterpret_cast (&pointDiffColorDataVector_size), sizeof (pointDiffColorDataVector_size)); - pointDiffColorDataVector.resize (static_cast (pointDiffColorDataVector_size)); - compressedColorDataLen_ += entropyCoder_.decodeStreamToCharVector (compressedTreeDataIn_arg, + std::vector& pointDiffColorDataVector = color_coder_.getDifferentialDataVector (); + compressed_tree_data_in_arg.read (reinterpret_cast (&point_diff_color_data_vector_size), sizeof (point_diff_color_data_vector_size)); + pointDiffColorDataVector.resize (static_cast (point_diff_color_data_vector_size)); + compressed_color_data_len_ += entropy_coder_.decodeStreamToCharVector (compressed_tree_data_in_arg, pointDiffColorDataVector); } } @@ -370,104 +370,104 @@ namespace pcl ////////////////////////////////////////////////////////////////////////////////////////////// template void - OctreePointCloudCompression::writeFrameHeader (std::ostream& compressedTreeDataOut_arg) + OctreePointCloudCompression::writeFrameHeader (std::ostream& compressed_tree_data_out_arg) { // encode header identifier - compressedTreeDataOut_arg.write (reinterpret_cast (frameHeaderIdentifier_), strlen (frameHeaderIdentifier_)); + compressed_tree_data_out_arg.write (reinterpret_cast (frame_header_identifier_), strlen (frame_header_identifier_)); // encode point cloud header id - compressedTreeDataOut_arg.write (reinterpret_cast (&frameID_), sizeof (frameID_)); + compressed_tree_data_out_arg.write (reinterpret_cast (&frame_ID_), sizeof (frame_ID_)); // encode frame type (I/P-frame) - compressedTreeDataOut_arg.write (reinterpret_cast (&iFrame_), sizeof (iFrame_)); - if (iFrame_) + compressed_tree_data_out_arg.write (reinterpret_cast (&i_frame_), sizeof (i_frame_)); + if (i_frame_) { - double minX, minY, minZ, maxX, maxY, maxZ; - double octreeResolution; - unsigned char colorBitDepth; - double pointResolution; + double min_x, min_y, min_z, max_x, max_y, max_z; + double octree_resolution; + unsigned char color_bit_depth; + double point_resolution; // get current configuration - octreeResolution = this->getResolution (); - colorBitDepth = colorCoder_.getBitDepth (); - pointResolution= pointCoder_.getPrecision (); - this->getBoundingBox (minX, minY, minZ, maxX, maxY, maxZ); + octree_resolution = this->getResolution (); + color_bit_depth = color_coder_.getBitDepth (); + point_resolution= point_coder_.getPrecision (); + this->getBoundingBox (min_x, min_y, min_z, max_x, max_y, max_z); // encode amount of points - if (doVoxelGridEnDecoding_) - pointCount_ = this->leafCount_; + if (do_voxel_grid_enDecoding_) + point_count_ = this->leaf_count_; else - pointCount_ = this->objectCount_; + point_count_ = this->object_count_; // encode coding configuration - compressedTreeDataOut_arg.write (reinterpret_cast (&doVoxelGridEnDecoding_), sizeof (doVoxelGridEnDecoding_)); - compressedTreeDataOut_arg.write (reinterpret_cast (&cloudWithColor_), sizeof (cloudWithColor_)); - compressedTreeDataOut_arg.write (reinterpret_cast (&pointCount_), sizeof (pointCount_)); - compressedTreeDataOut_arg.write (reinterpret_cast (&octreeResolution), sizeof (octreeResolution)); - compressedTreeDataOut_arg.write (reinterpret_cast (&colorBitDepth), sizeof (colorBitDepth)); - compressedTreeDataOut_arg.write (reinterpret_cast (&pointResolution), sizeof (pointResolution)); + compressed_tree_data_out_arg.write (reinterpret_cast (&do_voxel_grid_enDecoding_), sizeof (do_voxel_grid_enDecoding_)); + compressed_tree_data_out_arg.write (reinterpret_cast (&cloud_with_color_), sizeof (cloud_with_color_)); + compressed_tree_data_out_arg.write (reinterpret_cast (&point_count_), sizeof (point_count_)); + compressed_tree_data_out_arg.write (reinterpret_cast (&octree_resolution), sizeof (octree_resolution)); + compressed_tree_data_out_arg.write (reinterpret_cast (&color_bit_depth), sizeof (color_bit_depth)); + compressed_tree_data_out_arg.write (reinterpret_cast (&point_resolution), sizeof (point_resolution)); // encode octree bounding box - compressedTreeDataOut_arg.write (reinterpret_cast (&minX), sizeof (minX)); - compressedTreeDataOut_arg.write (reinterpret_cast (&minY), sizeof (minY)); - compressedTreeDataOut_arg.write (reinterpret_cast (&minZ), sizeof (minZ)); - compressedTreeDataOut_arg.write (reinterpret_cast (&maxX), sizeof (maxX)); - compressedTreeDataOut_arg.write (reinterpret_cast (&maxY), sizeof (maxY)); - compressedTreeDataOut_arg.write (reinterpret_cast (&maxZ), sizeof (maxZ)); + compressed_tree_data_out_arg.write (reinterpret_cast (&min_x), sizeof (min_x)); + compressed_tree_data_out_arg.write (reinterpret_cast (&min_y), sizeof (min_y)); + compressed_tree_data_out_arg.write (reinterpret_cast (&min_z), sizeof (min_z)); + compressed_tree_data_out_arg.write (reinterpret_cast (&max_x), sizeof (max_x)); + compressed_tree_data_out_arg.write (reinterpret_cast (&max_y), sizeof (max_y)); + compressed_tree_data_out_arg.write (reinterpret_cast (&max_z), sizeof (max_z)); } } ////////////////////////////////////////////////////////////////////////////////////////////// template void - OctreePointCloudCompression::syncToHeader ( std::istream& compressedTreeDataIn_arg) + OctreePointCloudCompression::syncToHeader ( std::istream& compressed_tree_data_in_arg) { // sync to frame header - unsigned int headerIdPos = 0; - while (headerIdPos < strlen (frameHeaderIdentifier_)) + unsigned int header_id_pos = 0; + while (header_id_pos < strlen (frame_header_identifier_)) { char readChar; - compressedTreeDataIn_arg.read (static_cast (&readChar), sizeof (readChar)); - if (readChar != frameHeaderIdentifier_[headerIdPos++]) - headerIdPos = (frameHeaderIdentifier_[0]==readChar)?1:0; + compressed_tree_data_in_arg.read (static_cast (&readChar), sizeof (readChar)); + if (readChar != frame_header_identifier_[header_id_pos++]) + header_id_pos = (frame_header_identifier_[0]==readChar)?1:0; } } ////////////////////////////////////////////////////////////////////////////////////////////// template void - OctreePointCloudCompression::readFrameHeader ( std::istream& compressedTreeDataIn_arg) + OctreePointCloudCompression::readFrameHeader ( std::istream& compressed_tree_data_in_arg) { // read header - compressedTreeDataIn_arg.read (reinterpret_cast (&frameID_), sizeof (frameID_)); - compressedTreeDataIn_arg.read (reinterpret_cast(&iFrame_), sizeof (iFrame_)); - if (iFrame_) + compressed_tree_data_in_arg.read (reinterpret_cast (&frame_ID_), sizeof (frame_ID_)); + compressed_tree_data_in_arg.read (reinterpret_cast(&i_frame_), sizeof (i_frame_)); + if (i_frame_) { - double minX, minY, minZ, maxX, maxY, maxZ; - double octreeResolution; - unsigned char colorBitDepth; - double pointResolution; + double min_x, min_y, min_z, max_x, max_y, max_z; + double octree_resolution; + unsigned char color_bit_depth; + double point_resolution; // read coder configuration - compressedTreeDataIn_arg.read (reinterpret_cast (&doVoxelGridEnDecoding_), sizeof (doVoxelGridEnDecoding_)); - compressedTreeDataIn_arg.read (reinterpret_cast (&dataWithColor_), sizeof (dataWithColor_)); - compressedTreeDataIn_arg.read (reinterpret_cast (&pointCount_), sizeof (pointCount_)); - compressedTreeDataIn_arg.read (reinterpret_cast (&octreeResolution), sizeof (octreeResolution)); - compressedTreeDataIn_arg.read (reinterpret_cast (&colorBitDepth), sizeof (colorBitDepth)); - compressedTreeDataIn_arg.read (reinterpret_cast (&pointResolution), sizeof (pointResolution)); + compressed_tree_data_in_arg.read (reinterpret_cast (&do_voxel_grid_enDecoding_), sizeof (do_voxel_grid_enDecoding_)); + compressed_tree_data_in_arg.read (reinterpret_cast (&data_with_color_), sizeof (data_with_color_)); + compressed_tree_data_in_arg.read (reinterpret_cast (&point_count_), sizeof (point_count_)); + compressed_tree_data_in_arg.read (reinterpret_cast (&octree_resolution), sizeof (octree_resolution)); + compressed_tree_data_in_arg.read (reinterpret_cast (&color_bit_depth), sizeof (color_bit_depth)); + compressed_tree_data_in_arg.read (reinterpret_cast (&point_resolution), sizeof (point_resolution)); // read octree bounding box - compressedTreeDataIn_arg.read (reinterpret_cast (&minX), sizeof (minX)); - compressedTreeDataIn_arg.read (reinterpret_cast (&minY), sizeof (minY)); - compressedTreeDataIn_arg.read (reinterpret_cast (&minZ), sizeof (minZ)); - compressedTreeDataIn_arg.read (reinterpret_cast (&maxX), sizeof (maxX)); - compressedTreeDataIn_arg.read (reinterpret_cast (&maxY), sizeof (maxY)); - compressedTreeDataIn_arg.read (reinterpret_cast (&maxZ), sizeof (maxZ)); + compressed_tree_data_in_arg.read (reinterpret_cast (&min_x), sizeof (min_x)); + compressed_tree_data_in_arg.read (reinterpret_cast (&min_y), sizeof (min_y)); + compressed_tree_data_in_arg.read (reinterpret_cast (&min_z), sizeof (min_z)); + compressed_tree_data_in_arg.read (reinterpret_cast (&max_x), sizeof (max_x)); + compressed_tree_data_in_arg.read (reinterpret_cast (&max_y), sizeof (max_y)); + compressed_tree_data_in_arg.read (reinterpret_cast (&max_z), sizeof (max_z)); // reset octree and assign new bounding box & resolution this->deleteTree (); - this->setResolution (octreeResolution); - this->defineBoundingBox (minX, minY, minZ, maxX, maxY, maxZ); + this->setResolution (octree_resolution); + this->defineBoundingBox (min_x, min_y, min_z, max_x, max_y, max_z); // configure color & point coding - colorCoder_.setBitDepth (colorBitDepth); - pointCoder_.setPrecision (static_cast (pointResolution)); + color_coder_.setBitDepth (color_bit_depth); + point_coder_.setPrecision (static_cast (point_resolution)); } } @@ -479,30 +479,30 @@ namespace pcl // reference to point indices vector stored within octree leaf const std::vector& leafIdx = leaf_arg.getPointIndicesVector(); - if (!doVoxelGridEnDecoding_) + if (!do_voxel_grid_enDecoding_) { double lowerVoxelCorner[3]; // encode amount of points within voxel - pointCountDataVector_.push_back (static_cast (leafIdx.size ())); + point_count_data_vector_.push_back (static_cast (leafIdx.size ())); // calculate lower voxel corner based on octree key - lowerVoxelCorner[0] = static_cast (key_arg.x) * this->resolution_ + this->minX_; - lowerVoxelCorner[1] = static_cast (key_arg.y) * this->resolution_ + this->minY_; - lowerVoxelCorner[2] = static_cast (key_arg.z) * this->resolution_ + this->minZ_; + lowerVoxelCorner[0] = static_cast (key_arg.x) * this->resolution_ + this->min_x_; + lowerVoxelCorner[1] = static_cast (key_arg.y) * this->resolution_ + this->min_y_; + lowerVoxelCorner[2] = static_cast (key_arg.z) * this->resolution_ + this->min_z_; // differentially encode points to lower voxel corner - pointCoder_.encodePoints (leafIdx, lowerVoxelCorner, this->input_); + point_coder_.encodePoints (leafIdx, lowerVoxelCorner, this->input_); - if (cloudWithColor_) + if (cloud_with_color_) // encode color of points - colorCoder_.encodePoints (leafIdx, pointColorOffset_, this->input_); + color_coder_.encodePoints (leafIdx, point_color_offset_, this->input_); } else { - if (cloudWithColor_) + if (cloud_with_color_) // encode average color of all points within voxel - colorCoder_.encodeAverageOfPoints (leafIdx, pointColorOffset_, this->input_); + color_coder_.encodeAverageOfPoints (leafIdx, point_color_offset_, this->input_); } } @@ -517,48 +517,48 @@ namespace pcl pointCount = 1; - if (!doVoxelGridEnDecoding_) + if (!do_voxel_grid_enDecoding_) { // get current cloud size cloudSize = output_->points.size (); // get amount of point to be decoded - pointCount = *pointCountDataVectorIterator_; - pointCountDataVectorIterator_++; + pointCount = *point_count_data_vector_iterator_; + point_count_data_vector_iterator_++; // increase point cloud by amount of voxel points for (i = 0; i < pointCount; i++) output_->points.push_back (newPoint); // calculcate position of lower voxel corner - lowerVoxelCorner[0] = static_cast (key_arg.x) * this->resolution_ + this->minX_; - lowerVoxelCorner[1] = static_cast (key_arg.y) * this->resolution_ + this->minY_; - lowerVoxelCorner[2] = static_cast (key_arg.z) * this->resolution_ + this->minZ_; + lowerVoxelCorner[0] = static_cast (key_arg.x) * this->resolution_ + this->min_x_; + lowerVoxelCorner[1] = static_cast (key_arg.y) * this->resolution_ + this->min_y_; + lowerVoxelCorner[2] = static_cast (key_arg.z) * this->resolution_ + this->min_z_; // decode differentially encoded points - pointCoder_.decodePoints (output_, lowerVoxelCorner, cloudSize, cloudSize + pointCount); + point_coder_.decodePoints (output_, lowerVoxelCorner, cloudSize, cloudSize + pointCount); } else { // calculate center of lower voxel corner - newPoint.x = static_cast ((static_cast (key_arg.x) + 0.5) * this->resolution_ + this->minX_); - newPoint.y = static_cast ((static_cast (key_arg.y) + 0.5) * this->resolution_ + this->minY_); - newPoint.z = static_cast ((static_cast (key_arg.z) + 0.5) * this->resolution_ + this->minZ_); + newPoint.x = static_cast ((static_cast (key_arg.x) + 0.5) * this->resolution_ + this->min_x_); + newPoint.y = static_cast ((static_cast (key_arg.y) + 0.5) * this->resolution_ + this->min_y_); + newPoint.z = static_cast ((static_cast (key_arg.z) + 0.5) * this->resolution_ + this->min_z_); // add point to point cloud output_->points.push_back (newPoint); } - if (cloudWithColor_) + if (cloud_with_color_) { - if (dataWithColor_) + if (data_with_color_) // decode color information - colorCoder_.decodePoints (output_, output_->points.size () - pointCount, - output_->points.size (), pointColorOffset_); + color_coder_.decodePoints (output_, output_->points.size () - pointCount, + output_->points.size (), point_color_offset_); else // set default color information - colorCoder_.setDefaultColor (output_, output_->points.size () - pointCount, - output_->points.size (), pointColorOffset_); + color_coder_.setDefaultColor (output_, output_->points.size () - pointCount, + output_->points.size (), point_color_offset_); } } } diff --git a/io/include/pcl/compression/octree_pointcloud_compression.h b/io/include/pcl/compression/octree_pointcloud_compression.h index 570eb6c3ca1..d6372ce86ee 100644 --- a/io/include/pcl/compression/octree_pointcloud_compression.h +++ b/io/include/pcl/compression/octree_pointcloud_compression.h @@ -107,21 +107,21 @@ namespace pcl const unsigned char colorBitResolution_arg = 6) : OctreePointCloud (octreeResolution_arg), output_ (PointCloudPtr ()), - binaryTreeDataVector_ (), - binaryColorTreeVector_ (), - pointCountDataVector_ (), - pointCountDataVectorIterator_ (), - colorCoder_ (), - pointCoder_ (), - entropyCoder_ (), - doVoxelGridEnDecoding_ (doVoxelGridDownDownSampling_arg), iFrameRate_ (iFrameRate_arg), - iFrameCounter_ (0), frameID_ (0), pointCount_ (0), iFrame_ (true), - doColorEncoding_ (doColorEncoding_arg), cloudWithColor_ (false), dataWithColor_ (false), - pointColorOffset_ (0), bShowStatistics (showStatistics_arg), - compressedPointDataLen_ (), compressedColorDataLen_ (), selectedProfile_(compressionProfile_arg), - pointResolution_(pointResolution_arg), octreeResolution_(octreeResolution_arg), - colorBitResolution_(colorBitResolution_arg), - objectCount_(0) + binary_tree_data_vector_ (), + binary_color_tree_vector_ (), + point_count_data_vector_ (), + point_count_data_vector_iterator_ (), + color_coder_ (), + point_coder_ (), + entropy_coder_ (), + do_voxel_grid_enDecoding_ (doVoxelGridDownDownSampling_arg), i_frame_rate_ (iFrameRate_arg), + i_frame_counter_ (0), frame_ID_ (0), point_count_ (0), i_frame_ (true), + do_color_encoding_ (doColorEncoding_arg), cloud_with_color_ (false), data_with_color_ (false), + point_color_offset_ (0), b_show_statistics_ (showStatistics_arg), + compressed_point_data_len_ (), compressed_color_data_len_ (), selected_profile_(compressionProfile_arg), + point_resolution_(pointResolution_arg), octree_resolution_(octreeResolution_arg), + color_bit_resolution_(colorBitResolution_arg), + object_count_(0) { initialization(); } @@ -134,32 +134,32 @@ namespace pcl /** \brief Initialize globals */ void initialization () { - if (selectedProfile_ != MANUAL_CONFIGURATION) + if (selected_profile_ != MANUAL_CONFIGURATION) { // apply selected compression profile // retrieve profile settings - const configurationProfile_t selectedProfile = compressionProfiles_[selectedProfile_]; + const configurationProfile_t selectedProfile = compressionProfiles_[selected_profile_]; // apply profile settings - iFrameRate_ = selectedProfile.iFrameRate; - doVoxelGridEnDecoding_ = selectedProfile.doVoxelGridDownSampling; + i_frame_rate_ = selectedProfile.iFrameRate; + do_voxel_grid_enDecoding_ = selectedProfile.doVoxelGridDownSampling; this->setResolution (selectedProfile.octreeResolution); - pointCoder_.setPrecision (static_cast (selectedProfile.pointResolution)); - doColorEncoding_ = selectedProfile.doColorEncoding; - colorCoder_.setBitDepth (selectedProfile.colorBitResolution); + point_coder_.setPrecision (static_cast (selectedProfile.pointResolution)); + do_color_encoding_ = selectedProfile.doColorEncoding; + color_coder_.setBitDepth (selectedProfile.colorBitResolution); } else { // configure point & color coder - pointCoder_.setPrecision (static_cast (pointResolution_)); - colorCoder_.setBitDepth (colorBitResolution_); + point_coder_.setPrecision (static_cast (point_resolution_)); + color_coder_.setBitDepth (color_bit_resolution_); } - if (pointCoder_.getPrecision () == this->getResolution ()) + if (point_coder_.getPrecision () == this->getResolution ()) //disable differential point colding - doVoxelGridEnDecoding_ = true; + do_voxel_grid_enDecoding_ = true; } @@ -169,7 +169,7 @@ namespace pcl virtual void addPointIdx (const int pointIdx_arg) { - ++objectCount_; + ++object_count_; OctreePointCloud::addPointIdx(pointIdx_arg); } @@ -196,49 +196,49 @@ namespace pcl /** \brief Encode point cloud to output stream * \param cloud_arg: point cloud to be compressed - * \param compressedTreeDataOut_arg: binary output stream containing compressed data + * \param compressed_tree_data_out_arg: binary output stream containing compressed data */ void - encodePointCloud (const PointCloudConstPtr &cloud_arg, std::ostream& compressedTreeDataOut_arg); + encodePointCloud (const PointCloudConstPtr &cloud_arg, std::ostream& compressed_tree_data_out_arg); /** \brief Decode point cloud from input stream - * \param compressedTreeDataIn_arg: binary input stream containing compressed data + * \param compressed_tree_data_in_arg: binary input stream containing compressed data * \param cloud_arg: reference to decoded point cloud */ void - decodePointCloud (std::istream& compressedTreeDataIn_arg, PointCloudPtr &cloud_arg); + decodePointCloud (std::istream& compressed_tree_data_in_arg, PointCloudPtr &cloud_arg); protected: /** \brief Write frame information to output stream - * \param compressedTreeDataOut_arg: binary output stream + * \param compressed_tree_data_out_arg: binary output stream */ void - writeFrameHeader (std::ostream& compressedTreeDataOut_arg); + writeFrameHeader (std::ostream& compressed_tree_data_out_arg); /** \brief Read frame information to output stream - * \param compressedTreeDataIn_arg: binary input stream + * \param compressed_tree_data_in_arg: binary input stream */ void - readFrameHeader (std::istream& compressedTreeDataIn_arg); + readFrameHeader (std::istream& compressed_tree_data_in_arg); /** \brief Synchronize to frame header - * \param compressedTreeDataIn_arg: binary input stream + * \param compressed_tree_data_in_arg: binary input stream */ void - syncToHeader (std::istream& compressedTreeDataIn_arg); + syncToHeader (std::istream& compressed_tree_data_in_arg); /** \brief Apply entropy encoding to encoded information and output to binary stream - * \param compressedTreeDataOut_arg: binary output stream + * \param compressed_tree_data_out_arg: binary output stream */ void - entropyEncoding (std::ostream& compressedTreeDataOut_arg); + entropyEncoding (std::ostream& compressed_tree_data_out_arg); /** \brief Entropy decoding of input binary stream and output to information vectors - * \param compressedTreeDataIn_arg: binary input stream + * \param compressed_tree_data_in_arg: binary input stream */ void - entropyDecoding (std::istream& compressedTreeDataIn_arg); + entropyDecoding (std::istream& compressed_tree_data_in_arg); /** \brief Encode leaf node information during serialization * \param leaf_arg: reference to new leaf node @@ -259,58 +259,58 @@ namespace pcl PointCloudPtr output_; /** \brief Vector for storing binary tree structure */ - std::vector binaryTreeDataVector_; + std::vector binary_tree_data_vector_; /** \brief Interator on binary tree structure vector */ - std::vector binaryColorTreeVector_; + std::vector binary_color_tree_vector_; /** \brief Vector for storing points per voxel information */ - std::vector pointCountDataVector_; + std::vector point_count_data_vector_; /** \brief Interator on points per voxel vector */ - std::vector::const_iterator pointCountDataVectorIterator_; + std::vector::const_iterator point_count_data_vector_iterator_; /** \brief Color coding instance */ - ColorCoding colorCoder_; + ColorCoding color_coder_; /** \brief Point coding instance */ - PointCoding pointCoder_; + PointCoding point_coder_; /** \brief Static range coder instance */ - StaticRangeCoder entropyCoder_; + StaticRangeCoder entropy_coder_; - bool doVoxelGridEnDecoding_; - uint32_t iFrameRate_; - uint32_t iFrameCounter_; - uint32_t frameID_; - uint64_t pointCount_; - bool iFrame_; + bool do_voxel_grid_enDecoding_; + uint32_t i_frame_rate_; + uint32_t i_frame_counter_; + uint32_t frame_ID_; + uint64_t point_count_; + bool i_frame_; - bool doColorEncoding_; - bool cloudWithColor_; - bool dataWithColor_; - unsigned char pointColorOffset_; + bool do_color_encoding_; + bool cloud_with_color_; + bool data_with_color_; + unsigned char point_color_offset_; //bool activating statistics - bool bShowStatistics; - uint64_t compressedPointDataLen_; - uint64_t compressedColorDataLen_; + bool b_show_statistics_; + uint64_t compressed_point_data_len_; + uint64_t compressed_color_data_len_; // frame header identifier - static const char* frameHeaderIdentifier_; + static const char* frame_header_identifier_; - const compression_Profiles_e selectedProfile_; - const double pointResolution_; - const double octreeResolution_; - const unsigned char colorBitResolution_; + const compression_Profiles_e selected_profile_; + const double point_resolution_; + const double octree_resolution_; + const unsigned char color_bit_resolution_; - std::size_t objectCount_; + std::size_t object_count_; }; // define frame identifier template - const char* OctreePointCloudCompression::frameHeaderIdentifier_ = ""; + const char* OctreePointCloudCompression::frame_header_identifier_ = ""; } } diff --git a/octree/include/pcl/octree/impl/octree2buf_base.hpp b/octree/include/pcl/octree/impl/octree2buf_base.hpp index 53af04febbf..b23cc58905e 100644 --- a/octree/include/pcl/octree/impl/octree2buf_base.hpp +++ b/octree/include/pcl/octree/impl/octree2buf_base.hpp @@ -46,14 +46,14 @@ namespace pcl ////////////////////////////////////////////////////////////////////////////////////////////// template Octree2BufBase::Octree2BufBase () : - leafCount_ (0), - branchCount_ (1), - rootNode_ (new BranchNode ()), - depthMask_ (0), - maxKey_ (), - bufferSelector_ (0), - treeDirtyFlag_ (false), - octreeDepth_ (0), + leaf_count_ (0), + branch_count_ (1), + root_node_ (new BranchNode ()), + depth_mask_ (0), + max_key_ (), + buffer_selector_ (0), + tree_dirty_flag_ (false), + octree_depth_ (0), dynamic_depth_enabled_(false) { } @@ -64,24 +64,24 @@ namespace pcl { // deallocate tree structure deleteTree (); - delete (rootNode_); + delete (root_node_); } ////////////////////////////////////////////////////////////////////////////////////////////// template void - Octree2BufBase::setMaxVoxelIndex (unsigned int maxVoxelIndex_arg) + Octree2BufBase::setMaxVoxelIndex (unsigned int max_voxel_index_arg) { unsigned int treeDepth; - assert (maxVoxelIndex_arg > 0); + assert (max_voxel_index_arg > 0); // tree depth == amount of bits of maxVoxels treeDepth = std::max ((std::min (static_cast (OctreeKey::maxDepth), - static_cast (std::ceil (Log2 (maxVoxelIndex_arg))))), + static_cast (std::ceil (Log2 (max_voxel_index_arg))))), static_cast (0)); // define depthMask_ by setting a single bit to 1 at bit position == tree depth - depthMask_ = (1 << (treeDepth - 1)); + depth_mask_ = (1 << (treeDepth - 1)); } ////////////////////////////////////////////////////////////////////////////////////////////// @@ -91,21 +91,21 @@ namespace pcl assert (depth_arg > 0); // set octree depth - octreeDepth_ = depth_arg; + octree_depth_ = depth_arg; // define depthMask_ by setting a single bit to 1 at bit position == tree depth - depthMask_ = (1 << (depth_arg - 1)); + depth_mask_ = (1 << (depth_arg - 1)); // define max. keys - maxKey_.x = maxKey_.y = maxKey_.z = (1 << depth_arg) - 1; + max_key_.x = max_key_.y = max_key_.z = (1 << depth_arg) - 1; } ////////////////////////////////////////////////////////////////////////////////////////////// template LeafContainerT* - Octree2BufBase::findLeaf (unsigned int idxX_arg, unsigned int idxY_arg, unsigned int idxZ_arg) + Octree2BufBase::findLeaf (unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg) { // generate key - OctreeKey key (idxX_arg, idxY_arg, idxZ_arg); + OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg); // check if key exist in octree return ( findLeaf (key)); @@ -113,10 +113,10 @@ namespace pcl ////////////////////////////////////////////////////////////////////////////////////////////// template LeafContainerT* - Octree2BufBase::createLeaf (unsigned int idxX_arg, unsigned int idxY_arg, unsigned int idxZ_arg) + Octree2BufBase::createLeaf (unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg) { // generate key - OctreeKey key (idxX_arg, idxY_arg, idxZ_arg); + OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg); // check if key exist in octree return ( createLeaf (key)); @@ -124,11 +124,10 @@ namespace pcl ////////////////////////////////////////////////////////////////////////////////////////////// template bool - Octree2BufBase::existLeaf (unsigned int idxX_arg, unsigned int idxY_arg, - unsigned int idxZ_arg) const + Octree2BufBase::existLeaf (unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg) const { // generate key - OctreeKey key (idxX_arg, idxY_arg, idxZ_arg); + OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg); // check if key exist in octree return existLeaf (key); @@ -136,11 +135,10 @@ namespace pcl ////////////////////////////////////////////////////////////////////////////////////////////// template void - Octree2BufBase::removeLeaf (unsigned int idxX_arg, unsigned int idxY_arg, - unsigned int idxZ_arg) + Octree2BufBase::removeLeaf (unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg) { // generate key - OctreeKey key (idxX_arg, idxY_arg, idxZ_arg); + OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg); // free voxel at key return (this->removeLeaf (key)); @@ -150,16 +148,16 @@ namespace pcl template void Octree2BufBase::deleteTree () { - if (rootNode_) + if (root_node_) { // reset octree - deleteBranch (*rootNode_); - leafCount_ = 0; - branchCount_ = 1; + deleteBranch (*root_node_); + leaf_count_ = 0; + branch_count_ = 1; - treeDirtyFlag_ = false; - depthMask_ = 0; - octreeDepth_ = 0; + tree_dirty_flag_ = false; + depth_mask_ = 0; + octree_depth_ = 0; } } @@ -167,201 +165,208 @@ namespace pcl template void Octree2BufBase::switchBuffers () { - if (treeDirtyFlag_) + if (tree_dirty_flag_) { // make sure that all unused branch nodes from previous buffer are deleted - treeCleanUpRecursive (rootNode_); + treeCleanUpRecursive (root_node_); } // switch butter selector - bufferSelector_ = !bufferSelector_; + buffer_selector_ = !buffer_selector_; // reset flags - treeDirtyFlag_ = true; - leafCount_ = 0; - branchCount_ = 1; + tree_dirty_flag_ = true; + leaf_count_ = 0; + branch_count_ = 1; - unsigned char childIdx; + unsigned char child_idx; // we can safely remove children references of root node - for (childIdx = 0; childIdx < 8; childIdx++) + for (child_idx = 0; child_idx < 8; child_idx++) { - rootNode_->setChildPtr(bufferSelector_, childIdx, 0); + root_node_->setChildPtr(buffer_selector_, child_idx, 0); } } ////////////////////////////////////////////////////////////////////////////////////////////// template void - Octree2BufBase::serializeTree (std::vector& binaryTreeOut_arg, bool doXOREncoding_arg) + Octree2BufBase::serializeTree (std::vector& binary_tree_out_arg, + bool do_XOR_encoding_arg) { - OctreeKey newKey; + OctreeKey new_key; // clear binary vector - binaryTreeOut_arg.clear (); - binaryTreeOut_arg.reserve (this->branchCount_); + binary_tree_out_arg.clear (); + binary_tree_out_arg.reserve (this->branch_count_); - serializeTreeRecursive (rootNode_, newKey, &binaryTreeOut_arg, 0, - doXOREncoding_arg, false); + serializeTreeRecursive (root_node_, new_key, &binary_tree_out_arg, 0, do_XOR_encoding_arg, false); // serializeTreeRecursive cleans-up unused octree nodes in previous octree - treeDirtyFlag_ = false; + tree_dirty_flag_ = false; } ////////////////////////////////////////////////////////////////////////////////////////////// template void - Octree2BufBase::serializeTree (std::vector& binaryTreeOut_arg, - std::vector& dataVector_arg, bool doXOREncoding_arg) + Octree2BufBase::serializeTree (std::vector& binary_tree_out_arg, + std::vector& leaf_container_vector_arg, + bool do_XOR_encoding_arg) { - OctreeKey newKey; + OctreeKey new_key; // clear output vectors - binaryTreeOut_arg.clear (); - dataVector_arg.clear (); + binary_tree_out_arg.clear (); + leaf_container_vector_arg.clear (); - dataVector_arg.reserve (leafCount_); - binaryTreeOut_arg.reserve (this->branchCount_); + leaf_container_vector_arg.reserve (leaf_count_); + binary_tree_out_arg.reserve (this->branch_count_); - serializeTreeRecursive (rootNode_, newKey, &binaryTreeOut_arg, &dataVector_arg, doXOREncoding_arg, false); + serializeTreeRecursive (root_node_, new_key, &binary_tree_out_arg, &leaf_container_vector_arg, do_XOR_encoding_arg, false); // serializeTreeRecursive cleans-up unused octree nodes in previous octree - treeDirtyFlag_ = false; + tree_dirty_flag_ = false; } ////////////////////////////////////////////////////////////////////////////////////////////// template void - Octree2BufBase::serializeLeafs (std::vector& dataVector_arg) + Octree2BufBase::serializeLeafs (std::vector& leaf_container_vector_arg) { - OctreeKey newKey; + OctreeKey new_key; // clear output vector - dataVector_arg.clear (); + leaf_container_vector_arg.clear (); - dataVector_arg.reserve (leafCount_); + leaf_container_vector_arg.reserve (leaf_count_); - serializeTreeRecursive (rootNode_, newKey, 0, &dataVector_arg, false, false); + serializeTreeRecursive (root_node_, new_key, 0, &leaf_container_vector_arg, false, false); // serializeLeafsRecursive cleans-up unused octree nodes in previous octree - treeDirtyFlag_ = false; + tree_dirty_flag_ = false; } ////////////////////////////////////////////////////////////////////////////////////////////// template void - Octree2BufBase::deserializeTree (std::vector& binaryTreeIn_arg, bool doXORDecoding_arg) + Octree2BufBase::deserializeTree (std::vector& binary_tree_in_arg, + bool do_XOR_decoding_arg) { - OctreeKey newKey; + OctreeKey new_key; // we will rebuild an octree -> reset leafCount - leafCount_ = 0; + leaf_count_ = 0; // iterator for binary tree structure vector - std::vector::const_iterator binaryTreeVectorIterator = - binaryTreeIn_arg.begin (); - std::vector::const_iterator binaryTreeVectorIteratorEnd = - binaryTreeIn_arg.end (); + std::vector::const_iterator binary_tree_in_it = binary_tree_in_arg.begin (); + std::vector::const_iterator binary_tree_in_it_end = binary_tree_in_arg.end (); - deserializeTreeRecursive (rootNode_, depthMask_, newKey, - binaryTreeVectorIterator, binaryTreeVectorIteratorEnd, 0, 0, false, - doXORDecoding_arg); + deserializeTreeRecursive (root_node_, depth_mask_, new_key, + binary_tree_in_it, binary_tree_in_it_end, 0, 0, false, + do_XOR_decoding_arg); // we modified the octree structure -> clean-up/tree-reset might be required - treeDirtyFlag_ = false; + tree_dirty_flag_ = false; } ////////////////////////////////////////////////////////////////////////////////////////////// template void - Octree2BufBase::deserializeTree (std::vector& binaryTreeIn_arg, - std::vector& dataVector_arg, bool doXORDecoding_arg) + Octree2BufBase::deserializeTree (std::vector& binary_tree_in_arg, + std::vector& leaf_container_vector_arg, + bool do_XOR_decoding_arg) { - OctreeKey newKey; + OctreeKey new_key; // set data iterator to first element - typename std::vector::const_iterator dataVectorIterator = dataVector_arg.begin (); + typename std::vector::const_iterator leaf_container_vector_it = leaf_container_vector_arg.begin (); // set data iterator to last element - typename std::vector::const_iterator dataVectorEndIterator = dataVector_arg.end (); + typename std::vector::const_iterator leaf_container_vector_it_end = leaf_container_vector_arg.end (); // we will rebuild an octree -> reset leafCount - leafCount_ = 0; + leaf_count_ = 0; // iterator for binary tree structure vector - std::vector::const_iterator binaryTreeVectorIterator = binaryTreeIn_arg.begin (); - std::vector::const_iterator binaryTreeVectorIteratorEnd = binaryTreeIn_arg.end (); + std::vector::const_iterator binary_tree_in_it = binary_tree_in_arg.begin (); + std::vector::const_iterator binary_tree_in_it_end = binary_tree_in_arg.end (); - deserializeTreeRecursive (rootNode_, depthMask_, newKey, binaryTreeVectorIterator, binaryTreeVectorIteratorEnd, &dataVectorIterator, - &dataVectorEndIterator, false, doXORDecoding_arg); + deserializeTreeRecursive (root_node_, + depth_mask_, + new_key, + binary_tree_in_it, + binary_tree_in_it_end, + &leaf_container_vector_it, + &leaf_container_vector_it_end, + false, + do_XOR_decoding_arg); // we modified the octree structure -> clean-up/tree-reset might be required - treeDirtyFlag_ = false; + tree_dirty_flag_ = false; } ////////////////////////////////////////////////////////////////////////////////////////////// template void - Octree2BufBase::serializeNewLeafs (std::vector& dataVector_arg) + Octree2BufBase::serializeNewLeafs (std::vector& leaf_container_vector_arg) { - OctreeKey newKey; + OctreeKey new_key; // clear output vector - dataVector_arg.clear (); + leaf_container_vector_arg.clear (); + leaf_container_vector_arg.reserve (leaf_count_); - dataVector_arg.reserve (leafCount_); + serializeTreeRecursive (root_node_, new_key, 0, &leaf_container_vector_arg, false, true); - serializeTreeRecursive (rootNode_, newKey, 0, &dataVector_arg, false, true); - - // serializeLeafsRecursive cleans-up unused octree nodes in previous octree - treeDirtyFlag_ = false; + // serializeLeafsRecursive cleans-up unused octree nodes in previous octree buffer + tree_dirty_flag_ = false; } ////////////////////////////////////////////////////////////////////////////////////////////// template unsigned int Octree2BufBase::createLeafRecursive (const OctreeKey& key_arg, - unsigned int depthMask_arg, + unsigned int depth_mask_arg, BranchNode* branch_arg, - LeafNode*& returnLeaf_arg, - BranchNode*& leafParent_arg, - bool branchReset_arg) + LeafNode*& return_leaf_arg, + BranchNode*& parent_of_leaf_arg, + bool branch_reset_arg) { // index to branch child - unsigned char childIdx; + unsigned char child_idx; // branch reset -> this branch has been taken from previous buffer - if (branchReset_arg) + if (branch_reset_arg) { // we can safely remove children references - for (childIdx = 0; childIdx < 8; childIdx++) + for (child_idx = 0; child_idx < 8; child_idx++) { - branch_arg->setChildPtr(bufferSelector_, childIdx, 0); + branch_arg->setChildPtr(buffer_selector_, child_idx, 0); } } // find branch child from key - childIdx = key_arg.getChildIdxWithDepthMask (depthMask_arg); + child_idx = key_arg.getChildIdxWithDepthMask (depth_mask_arg); - if (depthMask_arg > 1) + if (depth_mask_arg > 1) { // we have not reached maximum tree depth - BranchNode* childBranch; + BranchNode* child_branch; bool doNodeReset; doNodeReset = false; // if required branch does not exist - if (!branch_arg->hasChild(bufferSelector_, childIdx)) + if (!branch_arg->hasChild(buffer_selector_, child_idx)) { // check if we find a branch node reference in previous buffer - if (branch_arg->hasChild(!bufferSelector_, childIdx)) + if (branch_arg->hasChild(!buffer_selector_, child_idx)) { - OctreeNode* childNode = branch_arg->getChildPtr(!bufferSelector_,childIdx); + OctreeNode* child_node = branch_arg->getChildPtr(!buffer_selector_,child_idx); - if (childNode->getNodeType()==BRANCH_NODE) { - childBranch = static_cast (childNode); - branch_arg->setChildPtr(bufferSelector_, childIdx, childNode); + if (child_node->getNodeType()==BRANCH_NODE) { + child_branch = static_cast (child_node); + branch_arg->setChildPtr(buffer_selector_, child_idx, child_node); } else { // depth has changed.. child in preceeding buffer is a leaf node. - deleteBranchChild (*branch_arg, !bufferSelector_, childIdx); - childBranch = createBranchChild (*branch_arg, childIdx); + deleteBranchChild (*branch_arg, !buffer_selector_, child_idx); + child_branch = createBranchChild (*branch_arg, child_idx); } // take child branch from previous buffer @@ -371,96 +376,94 @@ namespace pcl else { // if required branch does not exist -> create it - childBranch = createBranchChild (*branch_arg, childIdx); + child_branch = createBranchChild (*branch_arg, child_idx); } - branchCount_++; + branch_count_++; } // required branch node already exists - use it else - childBranch = static_cast (branch_arg->getChildPtr(bufferSelector_,childIdx)); + child_branch = static_cast (branch_arg->getChildPtr(buffer_selector_,child_idx)); // recursively proceed with indexed child branch - return createLeafRecursive (key_arg, depthMask_arg / 2, childBranch, returnLeaf_arg, leafParent_arg, doNodeReset); + return createLeafRecursive (key_arg, depth_mask_arg / 2, child_branch, return_leaf_arg, parent_of_leaf_arg, doNodeReset); } else { // branch childs are leaf nodes - LeafNode* childLeaf; - if (!branch_arg->hasChild(bufferSelector_, childIdx)) + LeafNode* child_leaf; + if (!branch_arg->hasChild(buffer_selector_, child_idx)) { - // leaf node at childIdx does not exist + // leaf node at child_idx does not exist // check if we can take copy a reference from previous buffer - if (branch_arg->hasChild(!bufferSelector_, childIdx)) + if (branch_arg->hasChild(!buffer_selector_, child_idx)) { - OctreeNode * childNode = branch_arg->getChildPtr(!bufferSelector_,childIdx); - if (childNode->getNodeType () == LEAF_NODE) + OctreeNode * child_node = branch_arg->getChildPtr(!buffer_selector_,child_idx); + if (child_node->getNodeType () == LEAF_NODE) { - childLeaf = static_cast (childNode); - branch_arg->setChildPtr(bufferSelector_, childIdx, childNode); -// childLeaf->reset (); + child_leaf = static_cast (child_node); + branch_arg->setChildPtr(buffer_selector_, child_idx, child_node); } else { // depth has changed.. child in preceeding buffer is a leaf node. - deleteBranchChild (*branch_arg, !bufferSelector_, childIdx); - childLeaf = createLeafChild (*branch_arg, childIdx); + deleteBranchChild (*branch_arg, !buffer_selector_, child_idx); + child_leaf = createLeafChild (*branch_arg, child_idx); } - leafCount_++; + leaf_count_++; } else { // if required leaf does not exist -> create it - childLeaf = createLeafChild (*branch_arg, childIdx); - leafCount_++; + child_leaf = createLeafChild (*branch_arg, child_idx); + leaf_count_++; } // return leaf node - returnLeaf_arg = childLeaf; - leafParent_arg = branch_arg; + return_leaf_arg = child_leaf; + parent_of_leaf_arg = branch_arg; } else { // leaf node already exist - childLeaf = static_cast (branch_arg->getChildPtr(bufferSelector_,childIdx)); - - // return leaf node - returnLeaf_arg = childLeaf; - leafParent_arg = branch_arg; + return_leaf_arg = static_cast (branch_arg->getChildPtr(buffer_selector_,child_idx));; + parent_of_leaf_arg = branch_arg; } } - return depthMask_arg; + return depth_mask_arg; } ////////////////////////////////////////////////////////////////////////////////////////////// template void - Octree2BufBase::findLeafRecursive ( - const OctreeKey& key_arg, unsigned int depthMask_arg, BranchNode* branch_arg, LeafContainerT*& result_arg) const + Octree2BufBase::findLeafRecursive (const OctreeKey& key_arg, + unsigned int depth_mask_arg, + BranchNode* branch_arg, + LeafContainerT*& result_arg) const { // return leaf node - unsigned char childIdx; + unsigned char child_idx; // find branch child from key - childIdx = key_arg.getChildIdxWithDepthMask (depthMask_arg); + child_idx = key_arg.getChildIdxWithDepthMask (depth_mask_arg); - if (depthMask_arg > 1) + if (depth_mask_arg > 1) { // we have not reached maximum tree depth - BranchNode* childBranch; - childBranch = static_cast (branch_arg->getChildPtr(bufferSelector_,childIdx)); + BranchNode* child_branch; + child_branch = static_cast (branch_arg->getChildPtr(buffer_selector_,child_idx)); - if (childBranch) + if (child_branch) // recursively proceed with indexed child branch - findLeafRecursive (key_arg, depthMask_arg / 2, childBranch, result_arg); + findLeafRecursive (key_arg, depth_mask_arg / 2, child_branch, result_arg); } else { // we reached leaf node level - if (branch_arg->hasChild(bufferSelector_, childIdx)) + if (branch_arg->hasChild(buffer_selector_, child_idx)) { // return existing leaf node - LeafNode* leaf_node = static_cast (branch_arg->getChildPtr(bufferSelector_,childIdx)); + LeafNode* leaf_node = static_cast (branch_arg->getChildPtr(buffer_selector_,child_idx)); result_arg = leaf_node->getContainerPtr();; } } @@ -468,51 +471,52 @@ namespace pcl ////////////////////////////////////////////////////////////////////////////////////////////// template bool - Octree2BufBase::deleteLeafRecursive (const OctreeKey& key_arg, unsigned int depthMask_arg, - BranchNode* branch_arg) + Octree2BufBase::deleteLeafRecursive (const OctreeKey& key_arg, + unsigned int depth_mask_arg, + BranchNode* branch_arg) { // index to branch child - unsigned char childIdx; + unsigned char child_idx; // indicates if branch is empty and can be safely removed bool bNoChilds; // find branch child from key - childIdx = key_arg.getChildIdxWithDepthMask (depthMask_arg); + child_idx = key_arg.getChildIdxWithDepthMask (depth_mask_arg); - if (depthMask_arg > 1) + if (depth_mask_arg > 1) { // we have not reached maximum tree depth - BranchNode* childBranch; + BranchNode* child_branch; bool bBranchOccupied; // next branch child on our path through the tree - childBranch = static_cast (branch_arg->getChildPtr(bufferSelector_,childIdx)); + child_branch = static_cast (branch_arg->getChildPtr(buffer_selector_,child_idx)); - if (childBranch) + if (child_branch) { // recursively explore the indexed child branch - bBranchOccupied = deleteLeafRecursive (key_arg, depthMask_arg / 2, childBranch); + bBranchOccupied = deleteLeafRecursive (key_arg, depth_mask_arg / 2, child_branch); if (!bBranchOccupied) { // child branch does not own any sub-child nodes anymore -> delete child branch - deleteBranchChild (*branch_arg, bufferSelector_, childIdx); - branchCount_--; + deleteBranchChild (*branch_arg, buffer_selector_, child_idx); + branch_count_--; } } } else { // our child is a leaf node -> delete it - deleteBranchChild (*branch_arg, bufferSelector_, childIdx); - leafCount_--; + deleteBranchChild (*branch_arg, buffer_selector_, child_idx); + leaf_count_--; } // check if current branch still owns childs bNoChilds = false; - for (childIdx = 0; childIdx < 8; childIdx++) + for (child_idx = 0; child_idx < 8; child_idx++) { - bNoChilds = branch_arg->hasChild(bufferSelector_, childIdx); + bNoChilds = branch_arg->hasChild(buffer_selector_, child_idx); if (bNoChilds) break; } @@ -524,78 +528,79 @@ namespace pcl ////////////////////////////////////////////////////////////////////////////////////////////// template void Octree2BufBase< LeafContainerT, BranchContainerT>::serializeTreeRecursive (BranchNode* branch_arg, - OctreeKey& key_arg, std::vector* binaryTreeOut_arg, - typename std::vector* dataVector_arg, bool doXOREncoding_arg, - bool newLeafsFilter_arg) + OctreeKey& key_arg, + std::vector* binary_tree_out_arg, + typename std::vector* leaf_container_vector_arg, + bool do_XOR_encoding_arg, + bool new_leafs_filter_arg) { // child iterator - unsigned char childIdx; + unsigned char child_idx; // bit pattern - char branchBitPatternCurrBuffer; - char branchBitPatternPrevBuffer; - char nodeXORBitPattern; + char branch_bit_pattern_curr_buffer; + char branch_bit_pattern_prev_buffer; + char node_XOR_bit_pattern; // occupancy bit patterns of branch node (current and previous octree buffer) - branchBitPatternCurrBuffer = getBranchBitPattern (*branch_arg, bufferSelector_); - branchBitPatternPrevBuffer = getBranchBitPattern (*branch_arg, !bufferSelector_); + branch_bit_pattern_curr_buffer = getBranchBitPattern (*branch_arg, buffer_selector_); + branch_bit_pattern_prev_buffer = getBranchBitPattern (*branch_arg, !buffer_selector_); // XOR of current and previous occupancy bit patterns - nodeXORBitPattern = branchBitPatternCurrBuffer ^ branchBitPatternPrevBuffer; - if (binaryTreeOut_arg) + node_XOR_bit_pattern = branch_bit_pattern_curr_buffer ^ branch_bit_pattern_prev_buffer; + if (do_XOR_encoding_arg) { - if (doXOREncoding_arg) + if (do_XOR_encoding_arg) { // write XOR bit pattern to output vector - binaryTreeOut_arg->push_back (nodeXORBitPattern); + binary_tree_out_arg->push_back (node_XOR_bit_pattern); } else { // write bit pattern of current buffer to output vector - binaryTreeOut_arg->push_back (branchBitPatternCurrBuffer); + binary_tree_out_arg->push_back (branch_bit_pattern_curr_buffer); } } // iterate over all children - for (childIdx = 0; childIdx < 8; childIdx++) + for (child_idx = 0; child_idx < 8; child_idx++) { - if (branch_arg->hasChild(bufferSelector_, childIdx)) + if (branch_arg->hasChild(buffer_selector_, child_idx)) { // add current branch voxel to key - key_arg.pushBranch(childIdx); + key_arg.pushBranch(child_idx); - OctreeNode *childNode = branch_arg->getChildPtr(bufferSelector_,childIdx); + OctreeNode *child_node = branch_arg->getChildPtr(buffer_selector_,child_idx); - switch (childNode->getNodeType ()) + switch (child_node->getNodeType ()) { case BRANCH_NODE: { - // recursively proceed with indexed child branch - serializeTreeRecursive (static_cast (childNode), - key_arg, binaryTreeOut_arg, dataVector_arg, doXOREncoding_arg, - newLeafsFilter_arg); - break; + // recursively proceed with indexed child branch + serializeTreeRecursive (static_cast (child_node), key_arg, binary_tree_out_arg, + leaf_container_vector_arg, do_XOR_encoding_arg, new_leafs_filter_arg); + break; } case LEAF_NODE: { - LeafNode* childLeaf = static_cast (childNode); + LeafNode* child_leaf = static_cast (child_node); - if (newLeafsFilter_arg) + if (new_leafs_filter_arg) { - if (!branch_arg->hasChild (!bufferSelector_, childIdx)) + if (!branch_arg->hasChild (!buffer_selector_, child_idx)) { - if (dataVector_arg) - dataVector_arg->push_back(childLeaf->getContainerPtr()); + if (leaf_container_vector_arg) + leaf_container_vector_arg->push_back(child_leaf->getContainerPtr()); - serializeTreeCallback (**childLeaf, key_arg); + serializeTreeCallback (**child_leaf, key_arg); } } else { - if (dataVector_arg) - dataVector_arg->push_back(childLeaf->getContainerPtr()); + if (leaf_container_vector_arg) + leaf_container_vector_arg->push_back(child_leaf->getContainerPtr()); - serializeTreeCallback (**childLeaf, key_arg); + serializeTreeCallback (**child_leaf, key_arg); } break; @@ -607,10 +612,10 @@ namespace pcl // pop current branch voxel from key key_arg.popBranch(); } - else if (branch_arg->hasChild (!bufferSelector_, childIdx)) + else if (branch_arg->hasChild (!buffer_selector_, child_idx)) { // delete branch, free memory - deleteBranchChild (*branch_arg, !bufferSelector_, childIdx); + deleteBranchChild (*branch_arg, !buffer_selector_, child_idx); } @@ -621,27 +626,27 @@ namespace pcl ////////////////////////////////////////////////////////////////////////////////////////////// template void Octree2BufBase::deserializeTreeRecursive (BranchNode* branch_arg, - unsigned int depthMask_arg, OctreeKey& key_arg, + unsigned int depth_mask_arg, OctreeKey& key_arg, typename std::vector::const_iterator& binaryTreeIT_arg, typename std::vector::const_iterator& binaryTreeIT_End_arg, typename std::vector::const_iterator* dataVectorIterator_arg, typename std::vector::const_iterator* dataVectorEndIterator_arg, - bool branchReset_arg, bool doXORDecoding_arg) + bool branch_reset_arg, bool do_XOR_decoding_arg) { // child iterator - unsigned char childIdx; + unsigned char child_idx; // node bits char nodeBits; char recoveredNodeBits; // branch reset -> this branch has been taken from previous buffer - if (branchReset_arg) + if (branch_reset_arg) { // we can safely remove children references - for (childIdx = 0; childIdx < 8; childIdx++) + for (child_idx = 0; child_idx < 8; child_idx++) { - branch_arg->setChildPtr(bufferSelector_, childIdx, 0); + branch_arg->setChildPtr(buffer_selector_, child_idx, 0); } } @@ -651,9 +656,9 @@ namespace pcl // recover branch occupancy bit pattern - if (doXORDecoding_arg) + if (do_XOR_decoding_arg) { - recoveredNodeBits = getBranchBitPattern (*branch_arg, !bufferSelector_) ^ nodeBits; + recoveredNodeBits = getBranchBitPattern (*branch_arg, !buffer_selector_) ^ nodeBits; } else { @@ -661,39 +666,39 @@ namespace pcl } // iterate over all children - for (childIdx = 0; childIdx < 8; childIdx++) + for (child_idx = 0; child_idx < 8; child_idx++) { - // if occupancy bit for childIdx is set.. - if (recoveredNodeBits & (1 << childIdx)) + // if occupancy bit for child_idx is set.. + if (recoveredNodeBits & (1 << child_idx)) { // add current branch voxel to key - key_arg.pushBranch(childIdx); + key_arg.pushBranch(child_idx); bool doNodeReset; doNodeReset = false; - if (depthMask_arg > 1) + if (depth_mask_arg > 1) { // we have not reached maximum tree depth - BranchNode* childBranch; + BranchNode* child_branch; // check if we find a branch node reference in previous buffer - if (!branch_arg->hasChild(bufferSelector_, childIdx)) + if (!branch_arg->hasChild(buffer_selector_, child_idx)) { - if (branch_arg->hasChild(!bufferSelector_, childIdx)) + if (branch_arg->hasChild(!buffer_selector_, child_idx)) { - OctreeNode* childNode = branch_arg->getChildPtr(!bufferSelector_,childIdx); + OctreeNode* child_node = branch_arg->getChildPtr(!buffer_selector_,child_idx); - if (childNode->getNodeType()==BRANCH_NODE) { - childBranch = static_cast (childNode); - branch_arg->setChildPtr(bufferSelector_, childIdx, childNode); + if (child_node->getNodeType()==BRANCH_NODE) { + child_branch = static_cast (child_node); + branch_arg->setChildPtr(buffer_selector_, child_idx, child_node); } else { // depth has changed.. child in preceeding buffer is a leaf node. - deleteBranchChild (*branch_arg, !bufferSelector_, childIdx); - childBranch = createBranchChild (*branch_arg, childIdx); + deleteBranchChild (*branch_arg, !buffer_selector_, child_idx); + child_branch = createBranchChild (*branch_arg, child_idx); } // take child branch from previous buffer @@ -702,48 +707,48 @@ namespace pcl else { // if required branch does not exist -> create it - childBranch = createBranchChild (*branch_arg, childIdx); + child_branch = createBranchChild (*branch_arg, child_idx); } - branchCount_++; + branch_count_++; } else { // required branch node already exists - use it - childBranch = static_cast (branch_arg->getChildPtr(bufferSelector_,childIdx)); + child_branch = static_cast (branch_arg->getChildPtr(buffer_selector_,child_idx)); } // recursively proceed with indexed child branch - deserializeTreeRecursive (childBranch, depthMask_arg / 2, key_arg, + deserializeTreeRecursive (child_branch, depth_mask_arg / 2, key_arg, binaryTreeIT_arg, binaryTreeIT_End_arg, dataVectorIterator_arg, dataVectorEndIterator_arg, - doNodeReset, doXORDecoding_arg); + doNodeReset, do_XOR_decoding_arg); } else { // branch childs are leaf nodes - LeafNode* childLeaf; + LeafNode* child_leaf; // check if we can take copy a reference pointer from previous buffer - if (branch_arg->hasChild(!bufferSelector_, childIdx)) + if (branch_arg->hasChild(!buffer_selector_, child_idx)) { // take child leaf node from previous buffer - OctreeNode* childNode = branch_arg->getChildPtr(!bufferSelector_,childIdx); - if (childNode->getNodeType()==LEAF_NODE) { - childLeaf = static_cast (childNode); - branch_arg->setChildPtr(bufferSelector_, childIdx, childNode); + OctreeNode* child_node = branch_arg->getChildPtr(!buffer_selector_,child_idx); + if (child_node->getNodeType()==LEAF_NODE) { + child_leaf = static_cast (child_node); + branch_arg->setChildPtr(buffer_selector_, child_idx, child_node); } else { // depth has changed.. child in preceeding buffer is a leaf node. - deleteBranchChild (*branch_arg, !bufferSelector_, childIdx); - childLeaf = createLeafChild (*branch_arg, childIdx); + deleteBranchChild (*branch_arg, !buffer_selector_, child_idx); + child_leaf = createLeafChild (*branch_arg, child_idx); } } else { // if required leaf does not exist -> create it - childLeaf = createLeafChild (*branch_arg, childIdx); + child_leaf = createLeafChild (*branch_arg, child_idx); } // we reached leaf node level @@ -751,28 +756,28 @@ namespace pcl if (dataVectorIterator_arg && (*dataVectorIterator_arg != *dataVectorEndIterator_arg)) { - LeafContainerT& container = **childLeaf; + LeafContainerT& container = **child_leaf; container = ***dataVectorIterator_arg; ++*dataVectorIterator_arg; } - leafCount_++; + leaf_count_++; // execute deserialization callback - deserializeTreeCallback (**childLeaf, key_arg); + deserializeTreeCallback (**child_leaf, key_arg); } // pop current branch voxel from key key_arg.popBranch(); } - else if (branch_arg->hasChild (!bufferSelector_, childIdx)) + else if (branch_arg->hasChild (!buffer_selector_, child_idx)) { // remove old branch pointer information in current branch - branch_arg->setChildPtr(bufferSelector_, childIdx, 0); + branch_arg->setChildPtr(buffer_selector_, child_idx, 0); // remove unused branches in previous buffer - deleteBranchChild (*branch_arg, !bufferSelector_, childIdx); + deleteBranchChild (*branch_arg, !buffer_selector_, child_idx); } } } @@ -784,35 +789,35 @@ namespace pcl Octree2BufBase::treeCleanUpRecursive (BranchNode* branch_arg) { // child iterator - unsigned char childIdx; + unsigned char child_idx; // bit pattern - char nodeBitPatternLastBuffer; - char nodeXORBitPattern; - char unusedBranchesBits; + char occupied_children_bit_pattern_prev_buffer; + char node_XOR_bit_pattern; + char unused_branches_bit_pattern; // occupancy bit pattern of branch node (previous octree buffer) - nodeBitPatternLastBuffer = getBranchBitPattern (*branch_arg, !bufferSelector_); + occupied_children_bit_pattern_prev_buffer = getBranchBitPattern (*branch_arg, !buffer_selector_); // XOR of current and previous occupancy bit patterns - nodeXORBitPattern = getBranchXORBitPattern (*branch_arg); + node_XOR_bit_pattern = getBranchXORBitPattern (*branch_arg); // bit pattern indicating unused octree nodes in previous branch - unusedBranchesBits = nodeXORBitPattern & nodeBitPatternLastBuffer; + unused_branches_bit_pattern = node_XOR_bit_pattern & occupied_children_bit_pattern_prev_buffer; // iterate over all children - for (childIdx = 0; childIdx < 8; childIdx++) + for (child_idx = 0; child_idx < 8; child_idx++) { - if (branch_arg->hasChild(bufferSelector_, childIdx)) + if (branch_arg->hasChild(buffer_selector_, child_idx)) { - OctreeNode *childNode = branch_arg->getChildPtr(bufferSelector_,childIdx); + OctreeNode *child_node = branch_arg->getChildPtr(buffer_selector_,child_idx); - switch (childNode->getNodeType ()) + switch (child_node->getNodeType ()) { case BRANCH_NODE: { // recursively proceed with indexed child branch - treeCleanUpRecursive (static_cast (childNode)); + treeCleanUpRecursive (static_cast (child_node)); break; } case LEAF_NODE: @@ -824,10 +829,10 @@ namespace pcl } // check for unused branches in previous buffer - if (unusedBranchesBits & (1 << childIdx)) + if (unused_branches_bit_pattern & (1 << child_idx)) { // delete branch, free memory - deleteBranchChild (*branch_arg, !bufferSelector_, childIdx); + deleteBranchChild (*branch_arg, !buffer_selector_, child_idx); } } } diff --git a/octree/include/pcl/octree/impl/octree_base.hpp b/octree/include/pcl/octree/impl/octree_base.hpp index ebb353da978..c49de275234 100644 --- a/octree/include/pcl/octree/impl/octree_base.hpp +++ b/octree/include/pcl/octree/impl/octree_base.hpp @@ -51,532 +51,519 @@ namespace pcl { ////////////////////////////////////////////////////////////////////////////////////////////// template - OctreeBase::OctreeBase () : - leafCount_ (0), - branchCount_ (1), - rootNode_ (new BranchNode ()), - depthMask_ (0), - octreeDepth_ (0), - dynamic_depth_enabled_(false), - maxKey_ () - { - } + OctreeBase::OctreeBase () : + leaf_count_ (0), + branch_count_ (1), + root_node_ (new BranchNode ()), + depth_mask_ (0), + octree_depth_ (0), + dynamic_depth_enabled_ (false), + max_key_ () + { + } ////////////////////////////////////////////////////////////////////////////////////////////// template - OctreeBase::~OctreeBase () - { - // deallocate tree structure - deleteTree (); - delete (rootNode_); - } + OctreeBase::~OctreeBase () + { + // deallocate tree structure + deleteTree (); + delete (root_node_); + } ////////////////////////////////////////////////////////////////////////////////////////////// - template void - OctreeBase::setMaxVoxelIndex (unsigned int maxVoxelIndex_arg) - { - unsigned int treeDepth; + template + void + OctreeBase::setMaxVoxelIndex (unsigned int max_voxel_index_arg) + { + unsigned int tree_depth; - assert (maxVoxelIndex_arg>0); + assert(max_voxel_index_arg>0); - // tree depth == amount of bits of maxVoxels - treeDepth = std::max ((std::min (static_cast (OCT_MAXTREEDEPTH), - static_cast (std::ceil (Log2 (maxVoxelIndex_arg))))), - static_cast (0)); + // tree depth == bitlength of maxVoxels + tree_depth = std::max ( (std::min (static_cast (OctreeKey::maxDepth), static_cast (std::ceil (Log2 (max_voxel_index_arg))))), 0u); - // define depthMask_ by setting a single bit to 1 at bit position == tree depth - depthMask_ = (1 << (treeDepth - 1)); - } + // define depthMask_ by setting a single bit to 1 at bit position == tree depth + depth_mask_ = (1 << (tree_depth - 1)); + } ////////////////////////////////////////////////////////////////////////////////////////////// template - void - OctreeBase::setTreeDepth (unsigned int depth_arg) - { - assert(depth_arg>0); - - // set octree depth - octreeDepth_ = depth_arg; + void + OctreeBase::setTreeDepth (unsigned int depth_arg) + { + assert(depth_arg>0); - // define depthMask_ by setting a single bit to 1 at bit position == tree depth - depthMask_ = (1 << (depth_arg - 1)); + // set octree depth + octree_depth_ = depth_arg; - // define max. keys - maxKey_.x = maxKey_.y = maxKey_.z = (1 << depth_arg) - 1; - } + // define depthMask_ by setting a single bit to 1 at bit position == tree depth + depth_mask_ = (1 << (depth_arg - 1)); + // define max. keys + max_key_.x = max_key_.y = max_key_.z = (1 << depth_arg) - 1; + } ////////////////////////////////////////////////////////////////////////////////////////////// - template LeafContainerT* - OctreeBase::findLeaf (unsigned int idxX_arg, unsigned int idxY_arg, unsigned int idxZ_arg) - { - // generate key - OctreeKey key (idxX_arg, idxY_arg, idxZ_arg); + template + LeafContainerT* + OctreeBase::findLeaf (unsigned int idx_x_arg, + unsigned int idx_y_arg, + unsigned int idx_z_arg) + { + // generate key + OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg); - // check if key exist in octree - return ( findLeaf (key)); - } + // check if key exist in octree + return (findLeaf (key)); + } ////////////////////////////////////////////////////////////////////////////////////////////// - template LeafContainerT* - OctreeBase::createLeaf (unsigned int idxX_arg, unsigned int idxY_arg, unsigned int idxZ_arg) - { - // generate key - OctreeKey key (idxX_arg, idxY_arg, idxZ_arg); - - // check if key exist in octree - return ( createLeaf (key)); - } + template + LeafContainerT* + OctreeBase::createLeaf (unsigned int idx_x_arg, + unsigned int idx_y_arg, + unsigned int idx_z_arg) + { + // generate key + OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg); + // check if key exist in octree + return (createLeaf (key)); + } ////////////////////////////////////////////////////////////////////////////////////////////// - template bool - OctreeBase::existLeaf (unsigned int idxX_arg, unsigned int idxY_arg, - unsigned int idxZ_arg) const - { - // generate key - OctreeKey key (idxX_arg, idxY_arg, idxZ_arg); + template + bool + OctreeBase::existLeaf (unsigned int idx_x_arg, + unsigned int idx_y_arg, + unsigned int idx_z_arg) const + { + // generate key + OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg); - // check if key exist in octree - return ( existLeaf (key)); - } + // check if key exist in octree + return (existLeaf (key)); + } ////////////////////////////////////////////////////////////////////////////////////////////// - template void - OctreeBase::removeLeaf (unsigned int idxX_arg, unsigned int idxY_arg, - unsigned int idxZ_arg) - { - // generate key - OctreeKey key (idxX_arg, idxY_arg, idxZ_arg); + template + void + OctreeBase::removeLeaf (unsigned int idx_x_arg, + unsigned int idx_y_arg, + unsigned int idx_z_arg) + { + // generate key + OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg); - // check if key exist in octree - deleteLeafRecursive (key, depthMask_, rootNode_); - } + // check if key exist in octree + deleteLeafRecursive (key, depth_mask_, root_node_); + } ////////////////////////////////////////////////////////////////////////////////////////////// - template void - OctreeBase::deleteTree () - { - - if (rootNode_) + template + void + OctreeBase::deleteTree () { - // reset octree - deleteBranch (*rootNode_); - leafCount_ = 0; - branchCount_ = 1; - } + if (root_node_) + { + // reset octree + deleteBranch (*root_node_); + leaf_count_ = 0; + branch_count_ = 1; + } - } + } ////////////////////////////////////////////////////////////////////////////////////////////// - template void - OctreeBase::serializeTree (std::vector& binaryTreeOut_arg) - { + template + void + OctreeBase::serializeTree (std::vector& binary_tree_out_arg) + { - OctreeKey newKey; + OctreeKey new_key; - // clear binary vector - binaryTreeOut_arg.clear (); - binaryTreeOut_arg.reserve (this->branchCount_); + // clear binary vector + binary_tree_out_arg.clear (); + binary_tree_out_arg.reserve (this->branch_count_); - serializeTreeRecursive (rootNode_, newKey, &binaryTreeOut_arg, 0 ); - } + serializeTreeRecursive (root_node_, new_key, &binary_tree_out_arg, 0); + } ////////////////////////////////////////////////////////////////////////////////////////////// - template void - OctreeBase::serializeTree (std::vector& binaryTreeOut_arg, std::vector& dataVector_arg) - { + template + void + OctreeBase::serializeTree (std::vector& binary_tree_out_arg, + std::vector& leaf_container_vector_arg) + { - OctreeKey newKey; + OctreeKey new_key; - // clear output vectors - binaryTreeOut_arg.clear (); - dataVector_arg.clear (); + // clear output vectors + binary_tree_out_arg.clear (); + leaf_container_vector_arg.clear (); - dataVector_arg.reserve (this->leafCount_); - binaryTreeOut_arg.reserve (this->branchCount_); + binary_tree_out_arg.reserve (this->branch_count_); + leaf_container_vector_arg.reserve (this->leaf_count_); - serializeTreeRecursive (rootNode_, newKey, &binaryTreeOut_arg, &dataVector_arg ); - } + serializeTreeRecursive (root_node_, new_key, &binary_tree_out_arg, &leaf_container_vector_arg); + } ////////////////////////////////////////////////////////////////////////////////////////////// - template void - OctreeBase::serializeLeafs (std::vector& dataVector_arg) - { - OctreeKey newKey; + template + void + OctreeBase::serializeLeafs (std::vector& leaf_container_vector_arg) + { + OctreeKey new_key; - // clear output vector - dataVector_arg.clear (); + // clear output vector + leaf_container_vector_arg.clear (); - dataVector_arg.reserve(this->leafCount_); + leaf_container_vector_arg.reserve (this->leaf_count_); - serializeTreeRecursive (rootNode_, newKey, 0, &dataVector_arg ); - } + serializeTreeRecursive (root_node_, new_key, 0, &leaf_container_vector_arg); + } ////////////////////////////////////////////////////////////////////////////////////////////// - template void - OctreeBase::deserializeTree (std::vector& binaryTreeIn_arg) - { - OctreeKey newKey; + template + void + OctreeBase::deserializeTree (std::vector& binary_tree_out_arg) + { + OctreeKey new_key; - // free existing tree before tree rebuild - deleteTree (); + // free existing tree before tree rebuild + deleteTree (); - //iterator for binary tree structure vector - std::vector::const_iterator binaryTreeVectorIterator = binaryTreeIn_arg.begin (); - std::vector::const_iterator binaryTreeVectorIteratorEnd = binaryTreeIn_arg.end (); + //iterator for binary tree structure vector + std::vector::const_iterator binary_tree_out_it = binary_tree_out_arg.begin (); + std::vector::const_iterator binary_tree_out_it_end = binary_tree_out_arg.end (); - deserializeTreeRecursive (rootNode_, depthMask_, newKey, - binaryTreeVectorIterator, binaryTreeVectorIteratorEnd, 0, 0); + deserializeTreeRecursive (root_node_, + depth_mask_, + new_key, + binary_tree_out_it, + binary_tree_out_it_end, + 0, + 0); - } + } ////////////////////////////////////////////////////////////////////////////////////////////// - template void - OctreeBase::deserializeTree (std::vector& binaryTreeIn_arg, - std::vector& dataVector_arg) - { - OctreeKey newKey; + template + void + OctreeBase::deserializeTree (std::vector& binary_tree_in_arg, + std::vector& leaf_container_vector_arg) + { + OctreeKey new_key; - // set data iterator to first element - typename std::vector::const_iterator dataVectorIterator = dataVector_arg.begin (); + // set data iterator to first element + typename std::vector::const_iterator leaf_vector_it = leaf_container_vector_arg.begin (); - // set data iterator to last element - typename std::vector::const_iterator dataVectorEndIterator = dataVector_arg.end (); + // set data iterator to last element + typename std::vector::const_iterator leaf_vector_it_end = leaf_container_vector_arg.end (); - // free existing tree before tree rebuild - deleteTree (); + // free existing tree before tree rebuild + deleteTree (); - //iterator for binary tree structure vector - std::vector::const_iterator binaryTreeVectorIterator = binaryTreeIn_arg.begin (); - std::vector::const_iterator binaryTreeVectorIteratorEnd = binaryTreeIn_arg.end (); + //iterator for binary tree structure vector + std::vector::const_iterator binary_tree_input_it = binary_tree_in_arg.begin (); + std::vector::const_iterator binary_tree_input_it_end = binary_tree_in_arg.end (); - deserializeTreeRecursive (rootNode_, depthMask_, newKey, - binaryTreeVectorIterator, binaryTreeVectorIteratorEnd, &dataVectorIterator, &dataVectorEndIterator); + deserializeTreeRecursive (root_node_, + depth_mask_, + new_key, + binary_tree_input_it, + binary_tree_input_it_end, + &leaf_vector_it, + &leaf_vector_it_end); - } + } ////////////////////////////////////////////////////////////////////////////////////////////// template unsigned int OctreeBase::createLeafRecursive (const OctreeKey& key_arg, - unsigned int depthMask_arg, + unsigned int depth_mask_arg, BranchNode* branch_arg, - LeafNode*& returnLeaf_arg, - BranchNode*& leafParent_arg) + LeafNode*& return_leaf_arg, + BranchNode*& parent_of_leaf_arg) { // index to branch child - unsigned char childIdx; - - // find branch child from key - childIdx = key_arg.getChildIdxWithDepthMask(depthMask_arg); + unsigned char child_idx; - OctreeNode* childNode = (*branch_arg)[childIdx]; - - if (!childNode) - { - if ((!dynamic_depth_enabled_) && (depthMask_arg > 1)) { - // if required branch does not exist -> create it - BranchNode* childBranch = createBranchChild (*branch_arg, childIdx); + // find branch child from key + child_idx = key_arg.getChildIdxWithDepthMask (depth_mask_arg); - branchCount_++; + OctreeNode* child_node = (*branch_arg)[child_idx]; - // recursively proceed with indexed child branch - return createLeafRecursive (key_arg, depthMask_arg / 2, childBranch, returnLeaf_arg, leafParent_arg); + if (!child_node) + { + if ((!dynamic_depth_enabled_) && (depth_mask_arg > 1)) + { + // if required branch does not exist -> create it + BranchNode* childBranch = createBranchChild (*branch_arg, child_idx); - } else { - // if leaf node at childIdx does not exist - LeafNode* leaf_node = createLeafChild (*branch_arg, childIdx); - returnLeaf_arg = leaf_node; - leafParent_arg = branch_arg; - leafCount_++; - } - } else { + branch_count_++; - // Node exists already - switch (childNode->getNodeType()) { - case BRANCH_NODE: // recursively proceed with indexed child branch - return createLeafRecursive (key_arg, depthMask_arg / 2, static_cast (childNode), returnLeaf_arg, leafParent_arg); - break; - - case LEAF_NODE: - LeafNode* childLeaf = static_cast (childNode); - returnLeaf_arg = childLeaf; - leafParent_arg = branch_arg; - /* - // get amount of objects in leaf container - size_t leafObjCount = childLeaf->getSize (); - - if (! ( (!maxObjsPerLeaf_) || (!depthMask_arg) ) && (leafObjCount >= maxObjsPerLeaf_) ) - { - // leaf node needs to be expanded - - // copy leaf data - std::vector leafData; - leafData.reserve(leafObjCount); + return createLeafRecursive (key_arg, depth_mask_arg / 2, childBranch, return_leaf_arg, parent_of_leaf_arg); - childLeaf->getData (leafData); - - // delete current leaf node - deleteBranchChild(*branch_arg,childIdx); - leafCount_ --; - - // create new branch node - BranchNode* childBranch; - createBranchChild (*branch_arg, childIdx, childBranch); - branchCount_ ++; + } + else + { + // if leaf node at child_idx does not exist + LeafNode* leaf_node = createLeafChild (*branch_arg, child_idx); + return_leaf_arg = leaf_node; + parent_of_leaf_arg = branch_arg; + this->leaf_count_++; + } + } + else + { + // Node exists already + switch (child_node->getNodeType ()) + { + case BRANCH_NODE: + // recursively proceed with indexed child branch + return createLeafRecursive (key_arg, depth_mask_arg / 2, static_cast (child_node), + return_leaf_arg, parent_of_leaf_arg); + break; - typename std::vector::const_iterator lData = leafData.begin(); - typename std::vector::const_iterator lDataEnd = leafData.end(); + case LEAF_NODE: + return_leaf_arg = static_cast (child_node);; + parent_of_leaf_arg = branch_arg; + break; + } - // add data to new branch - OctreeKey dataKey; + } - while (lData!=lDataEnd) { - // get data object - const DataT& data = *lData; - ++lData; + return (depth_mask_arg >> 1); + } - // generate new key for data object - if (this->genOctreeKeyForDataT(data, dataKey)) { - LeafNode* newLeaf; + ////////////////////////////////////////////////////////////////////////////////////////////// + template + void + OctreeBase::findLeafRecursive (const OctreeKey& key_arg, + unsigned int depth_mask_arg, + BranchNode* branch_arg, + LeafContainerT*& result_arg) const + { + // index to branch child + unsigned char child_idx; - createLeafRecursive (dataKey, depthMask_arg / 2, data, childBranch, newLeaf); - // add data to leaf - newLeaf->setData (data); - objectCount_++; - } - } + // find branch child from key + child_idx = key_arg.getChildIdxWithDepthMask (depth_mask_arg); - // and return new leaf node - createLeafRecursive (key_arg, depthMask_arg / 2, data_arg, childBranch, returnLeaf_arg); + OctreeNode* child_node = (*branch_arg)[child_idx]; - // correct object counter - objectCount_ -= leafObjCount; - } - */ - break; - } + if (child_node) + { + switch (child_node->getNodeType ()) + { + case BRANCH_NODE: + // we have not reached maximum tree depth + BranchNode* child_branch; + child_branch = static_cast (child_node); - } + findLeafRecursive (key_arg, depth_mask_arg / 2, child_branch, result_arg); + break; - return (depthMask_arg>>1); - } + case LEAF_NODE: + // return existing leaf node + LeafNode* child_leaf; + child_leaf = static_cast (child_node); - ////////////////////////////////////////////////////////////////////////////////////////////// - template void - OctreeBase::findLeafRecursive ( - const OctreeKey& key_arg, unsigned int depthMask_arg, BranchNode* branch_arg, LeafContainerT*& result_arg) const - { - // index to branch child - unsigned char childIdx; - - // find branch child from key - childIdx = key_arg.getChildIdxWithDepthMask(depthMask_arg); - - OctreeNode* childNode = (*branch_arg)[childIdx]; - - if (childNode) { - switch (childNode->getNodeType()) { - case BRANCH_NODE: - // we have not reached maximum tree depth - BranchNode* childBranch; - childBranch = static_cast (childNode); - - findLeafRecursive (key_arg, depthMask_arg / 2, childBranch, result_arg); - break; - - case LEAF_NODE: - // return existing leaf node - LeafNode* childLeaf; - childLeaf = static_cast (childNode); - - result_arg = childLeaf->getContainerPtr(); - break; + result_arg = child_leaf->getContainerPtr (); + break; + } } } - } ////////////////////////////////////////////////////////////////////////////////////////////// - template bool - OctreeBase::deleteLeafRecursive (const OctreeKey& key_arg, unsigned int depthMask_arg, - BranchNode* branch_arg) - { - // index to branch child - unsigned char childIdx; - // indicates if branch is empty and can be safely removed - bool bNoChilds; + template + bool + OctreeBase::deleteLeafRecursive (const OctreeKey& key_arg, + unsigned int depth_mask_arg, + BranchNode* branch_arg) + { + // index to branch child + unsigned char child_idx; + // indicates if branch is empty and can be safely removed + bool b_no_children; - // find branch child from key - childIdx = key_arg.getChildIdxWithDepthMask(depthMask_arg); + // find branch child from key + child_idx = key_arg.getChildIdxWithDepthMask (depth_mask_arg); - OctreeNode* childNode = (*branch_arg)[childIdx]; + OctreeNode* child_node = (*branch_arg)[child_idx]; - if (childNode) { - switch (childNode->getNodeType()) { + if (child_node) + { + switch (child_node->getNodeType ()) + { - case BRANCH_NODE: - BranchNode* childBranch; - childBranch = static_cast (childNode); + case BRANCH_NODE: + BranchNode* child_branch; + child_branch = static_cast (child_node); - // recursively explore the indexed child branch - bNoChilds = deleteLeafRecursive (key_arg, depthMask_arg / 2, childBranch); + // recursively explore the indexed child branch + b_no_children = deleteLeafRecursive (key_arg, depth_mask_arg / 2, child_branch); - if (!bNoChilds) - { - // child branch does not own any sub-child nodes anymore -> delete child branch - deleteBranchChild(*branch_arg, childIdx); - branchCount_--; - } - break; + if (!b_no_children) + { + // child branch does not own any sub-child nodes anymore -> delete child branch + deleteBranchChild (*branch_arg, child_idx); + branch_count_--; + } + break; - case LEAF_NODE: - // return existing leaf node + case LEAF_NODE: + // return existing leaf node - // our child is a leaf node -> delete it - deleteBranchChild (*branch_arg, childIdx); - leafCount_--; - break; + // our child is a leaf node -> delete it + deleteBranchChild (*branch_arg, child_idx); + this->leaf_count_--; + break; + } } - } - // check if current branch still owns children - bNoChilds = false; - for (childIdx = 0; (!bNoChilds) && (childIdx < 8); childIdx++) - { - bNoChilds = branch_arg->hasChild(childIdx); + // check if current branch still owns children + b_no_children = false; + for (child_idx = 0; (!b_no_children) && (child_idx < 8); child_idx++) + { + b_no_children = branch_arg->hasChild (child_idx); + } + // return true if current branch can be deleted + return (b_no_children); } - // return true if current branch can be deleted - return (bNoChilds); - } ////////////////////////////////////////////////////////////////////////////////////////////// - template void OctreeBase< - LeafContainerT, BranchContainerT>::serializeTreeRecursive (const BranchNode* branch_arg, OctreeKey& key_arg, - std::vector* binaryTreeOut_arg, - typename std::vector* dataVector_arg) const - { + template + void + OctreeBase::serializeTreeRecursive (const BranchNode* branch_arg, + OctreeKey& key_arg, + std::vector* binary_tree_out_arg, + typename std::vector* leaf_container_vector_arg) const + { - // child iterator - unsigned char childIdx; - char nodeBitPattern; + // child iterator + unsigned char child_idx; + char node_bit_pattern; - // branch occupancy bit pattern - nodeBitPattern = getBranchBitPattern (*branch_arg); + // branch occupancy bit pattern + node_bit_pattern = getBranchBitPattern (*branch_arg); - // write bit pattern to output vector - if (binaryTreeOut_arg) - binaryTreeOut_arg->push_back (nodeBitPattern); + // write bit pattern to output vector + if (binary_tree_out_arg) + binary_tree_out_arg->push_back (node_bit_pattern); - // iterate over all children - for (childIdx = 0; childIdx < 8; childIdx++) - { - - // if child exist - if (branch_arg->hasChild(childIdx)) + // iterate over all children + for (child_idx = 0; child_idx < 8; child_idx++) { - // add current branch voxel to key - key_arg.pushBranch(childIdx); - OctreeNode *childNode = branch_arg->getChildPtr(childIdx); - - switch (childNode->getNodeType ()) + // if child exist + if (branch_arg->hasChild (child_idx)) { - case BRANCH_NODE: - { - // recursively proceed with indexed child branch - serializeTreeRecursive ( - static_cast (childNode), key_arg, - binaryTreeOut_arg, dataVector_arg); - break; - } - case LEAF_NODE: + // add current branch voxel to key + key_arg.pushBranch (child_idx); + + OctreeNode *childNode = branch_arg->getChildPtr (child_idx); + + switch (childNode->getNodeType ()) { - LeafNode* childLeaf = static_cast (childNode); + case BRANCH_NODE: + { + // recursively proceed with indexed child branch + serializeTreeRecursive (static_cast (childNode), key_arg, binary_tree_out_arg, + leaf_container_vector_arg); + break; + } + case LEAF_NODE: + { + LeafNode* child_leaf = static_cast (childNode); - if (dataVector_arg) - dataVector_arg->push_back(childLeaf->getContainerPtr()); + if (leaf_container_vector_arg) + leaf_container_vector_arg->push_back (child_leaf->getContainerPtr ()); - // we reached a leaf node -> execute serialization callback - serializeTreeCallback (**childLeaf, key_arg); - break; + // we reached a leaf node -> execute serialization callback + serializeTreeCallback (**child_leaf, key_arg); + break; + } + default: + break; } - default: - break; - } - // pop current branch voxel from key - key_arg.popBranch(); + // pop current branch voxel from key + key_arg.popBranch (); + } } } - } ////////////////////////////////////////////////////////////////////////////////////////////// - template void - OctreeBase::deserializeTreeRecursive (BranchNode* branch_arg, - unsigned int depthMask_arg, OctreeKey& key_arg, - typename std::vector::const_iterator& binaryTreeIT_arg, - typename std::vector::const_iterator& binaryTreeIT_End_arg, - typename std::vector::const_iterator* dataVectorIterator_arg, - typename std::vector::const_iterator* dataVectorEndIterator_arg) - { - // child iterator - unsigned char childIdx; - char nodeBits; - - if (binaryTreeIT_arg != binaryTreeIT_End_arg ) { - // read branch occupancy bit pattern from input vector - nodeBits = (*binaryTreeIT_arg); - binaryTreeIT_arg++; + template + void + OctreeBase::deserializeTreeRecursive (BranchNode* branch_arg, unsigned int depth_mask_arg, OctreeKey& key_arg, + typename std::vector::const_iterator& binary_tree_input_it_arg, + typename std::vector::const_iterator& binary_tree_input_it_end_arg, + typename std::vector::const_iterator* leaf_container_vector_it_arg, + typename std::vector::const_iterator* leaf_container_vector_it_end_arg) + { + // child iterator + unsigned char child_idx; + char node_bits; - // iterate over all children - for (childIdx = 0; childIdx < 8; childIdx++) + if (binary_tree_input_it_arg != binary_tree_input_it_end_arg) { - // if occupancy bit for childIdx is set.. - if (nodeBits & (1 << childIdx)) - { - // add current branch voxel to key - key_arg.pushBranch(childIdx); + // read branch occupancy bit pattern from input vector + node_bits = (*binary_tree_input_it_arg); + binary_tree_input_it_arg++; - if (depthMask_arg > 1) + // iterate over all children + for (child_idx = 0; child_idx < 8; child_idx++) + { + // if occupancy bit for child_idx is set.. + if (node_bits & (1 << child_idx)) { - // we have not reached maximum tree depth - BranchNode * newBranch = createBranchChild (*branch_arg, childIdx); + // add current branch voxel to key + key_arg.pushBranch (child_idx); - branchCount_++; - - // recursively proceed with new child branch - deserializeTreeRecursive (newBranch, depthMask_arg / 2, key_arg, binaryTreeIT_arg,binaryTreeIT_End_arg, dataVectorIterator_arg, - dataVectorEndIterator_arg); - } - else - { - // we reached leaf node level + if (depth_mask_arg > 1) + { + // we have not reached maximum tree depth + BranchNode * newBranch = createBranchChild (*branch_arg, child_idx); - LeafNode* childLeaf = createLeafChild (*branch_arg, childIdx); + branch_count_++; - if (dataVectorIterator_arg - && (*dataVectorIterator_arg != *dataVectorEndIterator_arg)) - { - LeafContainerT& container = **childLeaf; - container = ***dataVectorIterator_arg; - ++*dataVectorIterator_arg; + // recursively proceed with new child branch + deserializeTreeRecursive (newBranch, depth_mask_arg / 2, key_arg, + binary_tree_input_it_arg, binary_tree_input_it_end_arg, + leaf_container_vector_it_arg, leaf_container_vector_it_end_arg); } + else + { + // we reached leaf node level - leafCount_++; + LeafNode* child_leaf = createLeafChild (*branch_arg, child_idx); - // execute deserialization callback - deserializeTreeCallback (**childLeaf, key_arg); - } + if (leaf_container_vector_it_arg && (*leaf_container_vector_it_arg != *leaf_container_vector_it_end_arg)) + { + LeafContainerT& container = **child_leaf; + container = ***leaf_container_vector_it_arg; + ++*leaf_container_vector_it_arg; + } - // pop current branch voxel from key - key_arg.popBranch(); + leaf_count_++; + + // execute deserialization callback + deserializeTreeCallback (**child_leaf, key_arg); + } + + // pop current branch voxel from key + key_arg.popBranch (); + } } } } - } } } diff --git a/octree/include/pcl/octree/impl/octree_iterator.hpp b/octree/include/pcl/octree/impl/octree_iterator.hpp index d792a88bcad..5d95a028d3a 100644 --- a/octree/include/pcl/octree/impl/octree_iterator.hpp +++ b/octree/include/pcl/octree/impl/octree_iterator.hpp @@ -50,8 +50,8 @@ namespace pcl { ////////////////////////////////////////////////////////////////////////////////////////////// template - OctreeDepthFirstIterator::OctreeDepthFirstIterator (unsigned int maxDepth_arg) : - OctreeIteratorBase (maxDepth_arg), stack_ () + OctreeDepthFirstIterator::OctreeDepthFirstIterator (unsigned int max_depth_arg) : + OctreeIteratorBase (max_depth_arg), stack_ () { // initialize iterator this->reset (); @@ -59,8 +59,8 @@ namespace pcl ////////////////////////////////////////////////////////////////////////////////////////////// template - OctreeDepthFirstIterator::OctreeDepthFirstIterator (OctreeT* octree_arg, unsigned int maxDepth_arg) : - OctreeIteratorBase (octree_arg, maxDepth_arg), stack_ () + OctreeDepthFirstIterator::OctreeDepthFirstIterator (OctreeT* octree_arg, unsigned int max_depth_arg) : + OctreeIteratorBase (octree_arg, max_depth_arg), stack_ () { // initialize iterator this->reset (); @@ -81,20 +81,20 @@ namespace pcl if (this->octree_) { // allocate stack - stack_.reserve (this->maxOctreeDepth_); + stack_.reserve (this->max_octree_depth_); // empty stack stack_.clear (); // pushing root node to stack - IteratorState stackEntry; - stackEntry.node_ = this->octree_->getRootNode (); - stackEntry.depth_ = 0; - stackEntry.key_.x = stackEntry.key_.y = stackEntry.key_.z = 0; + IteratorState stack_entry; + stack_entry.node_ = this->octree_->getRootNode (); + stack_entry.depth_ = 0; + stack_entry.key_.x = stack_entry.key_.y = stack_entry.key_.z = 0; - stack_.push_back(stackEntry); + stack_.push_back(stack_entry); - this->currentState_ = &stack_.back(); + this->current_state_ = &stack_.back(); } } @@ -115,10 +115,10 @@ namespace pcl if (stack_.size ()) { - this->currentState_ = &stack_.back(); + this->current_state_ = &stack_.back(); } else { - this->currentState_ = 0; + this->current_state_ = 0; } } @@ -133,47 +133,47 @@ namespace pcl if (stack_.size ()) { // get stack element - IteratorState stackEntry = stack_.back (); + IteratorState stack_entry = stack_.back (); stack_.pop_back (); - stackEntry.depth_ ++; - OctreeKey& currentKey = stackEntry.key_; + stack_entry.depth_ ++; + OctreeKey& current_key = stack_entry.key_; - if ( (this->maxOctreeDepth_>=stackEntry.depth_) && - (stackEntry.node_->getNodeType () == BRANCH_NODE) ) + if ( (this->max_octree_depth_>=stack_entry.depth_) && + (stack_entry.node_->getNodeType () == BRANCH_NODE) ) { - unsigned char childIdx; + unsigned char child_idx; // current node is a branch node - BranchNode* currentBranch = - static_cast (stackEntry.node_); + BranchNode* current_branch = + static_cast (stack_entry.node_); // add all children to stack - for (childIdx = 0; childIdx < 8; ++childIdx) + for (child_idx = 0; child_idx < 8; ++child_idx) { // if child exist - if (this->octree_->branchHasChild(*currentBranch, childIdx)) + if (this->octree_->branchHasChild(*current_branch, child_idx)) { // add child to stack - currentKey.pushBranch (childIdx); + current_key.pushBranch (child_idx); - stackEntry.node_ = this->octree_->getBranchChildPtr(*currentBranch, childIdx); + stack_entry.node_ = this->octree_->getBranchChildPtr(*current_branch, child_idx); - stack_.push_back(stackEntry); + stack_.push_back(stack_entry); - currentKey.popBranch(); + current_key.popBranch(); } } } if (stack_.size ()) { - this->currentState_ = &stack_.back(); + this->current_state_ = &stack_.back(); } else { - this->currentState_ = 0; + this->current_state_ = 0; } } @@ -182,8 +182,8 @@ namespace pcl ////////////////////////////////////////////////////////////////////////////////////////////// template - OctreeBreadthFirstIterator::OctreeBreadthFirstIterator (unsigned int maxDepth_arg) : - OctreeIteratorBase (maxDepth_arg), FIFO_ () + OctreeBreadthFirstIterator::OctreeBreadthFirstIterator (unsigned int max_depth_arg) : + OctreeIteratorBase (max_depth_arg), FIFO_ () { OctreeIteratorBase::reset (); @@ -193,8 +193,8 @@ namespace pcl ////////////////////////////////////////////////////////////////////////////////////////////// template - OctreeBreadthFirstIterator::OctreeBreadthFirstIterator (OctreeT* octree_arg, unsigned int maxDepth_arg) : - OctreeIteratorBase (octree_arg, maxDepth_arg), FIFO_ () + OctreeBreadthFirstIterator::OctreeBreadthFirstIterator (OctreeT* octree_arg, unsigned int max_depth_arg) : + OctreeIteratorBase (octree_arg, max_depth_arg), FIFO_ () { OctreeIteratorBase::reset (); @@ -220,14 +220,14 @@ namespace pcl if (this->octree_) { // pushing root node to stack - IteratorState FIFOEntry; - FIFOEntry.node_ = this->octree_->getRootNode (); - FIFOEntry.depth_ = 0; - FIFOEntry.key_.x = FIFOEntry.key_.y = FIFOEntry.key_.z = 0; + IteratorState FIFO_entry; + FIFO_entry.node_ = this->octree_->getRootNode (); + FIFO_entry.depth_ = 0; + FIFO_entry.key_.x = FIFO_entry.key_.y = FIFO_entry.key_.z = 0; - FIFO_.push_back(FIFOEntry); + FIFO_.push_back(FIFO_entry); - this->currentState_ = &FIFO_.front(); + this->current_state_ = &FIFO_.front(); } } @@ -240,46 +240,46 @@ namespace pcl if (FIFO_.size ()) { // get stack element - IteratorState FIFOEntry = FIFO_.front (); + IteratorState FIFO_entry = FIFO_.front (); FIFO_.pop_front (); - FIFOEntry.depth_ ++; - OctreeKey& currentKey = FIFOEntry.key_; + FIFO_entry.depth_ ++; + OctreeKey& current_key = FIFO_entry.key_; - if ( (this->maxOctreeDepth_>=FIFOEntry.depth_) && - (FIFOEntry.node_->getNodeType () == BRANCH_NODE) ) + if ( (this->max_octree_depth_>=FIFO_entry.depth_) && + (FIFO_entry.node_->getNodeType () == BRANCH_NODE) ) { - unsigned char childIdx; + unsigned char child_idx; // current node is a branch node - BranchNode* currentBranch = - static_cast (FIFOEntry.node_); + BranchNode* current_branch = + static_cast (FIFO_entry.node_); // iterate over all children - for (childIdx = 0; childIdx < 8 ; ++childIdx) + for (child_idx = 0; child_idx < 8 ; ++child_idx) { // if child exist - if (this->octree_->branchHasChild(*currentBranch, childIdx)) + if (this->octree_->branchHasChild(*current_branch, child_idx)) { // add child to stack - currentKey.pushBranch (static_cast (childIdx)); + current_key.pushBranch (static_cast (child_idx)); - FIFOEntry.node_ = this->octree_->getBranchChildPtr(*currentBranch, childIdx); + FIFO_entry.node_ = this->octree_->getBranchChildPtr(*current_branch, child_idx); - FIFO_.push_back(FIFOEntry); + FIFO_.push_back(FIFO_entry); - currentKey.popBranch(); + current_key.popBranch(); } } } if (FIFO_.size ()) { - this->currentState_ = &FIFO_.front(); + this->current_state_ = &FIFO_.front(); } else { - this->currentState_ = 0; + this->current_state_ = 0; } } diff --git a/octree/include/pcl/octree/impl/octree_pointcloud.hpp b/octree/include/pcl/octree/impl/octree_pointcloud.hpp index 4fa47faba5f..dfc9beaff48 100644 --- a/octree/include/pcl/octree/impl/octree_pointcloud.hpp +++ b/octree/include/pcl/octree/impl/octree_pointcloud.hpp @@ -50,8 +50,8 @@ using namespace std; template pcl::octree::OctreePointCloud::OctreePointCloud (const double resolution) : OctreeT (), input_ (PointCloudConstPtr ()), indices_ (IndicesConstPtr ()), - epsilon_ (0), resolution_ (resolution), minX_ (0.0f), maxX_ (resolution), minY_ (0.0f), - maxY_ (resolution), minZ_ (0.0f), maxZ_ (resolution), boundingBoxDefined_ (false), maxObjsPerLeaf_(0) + epsilon_ (0), resolution_ (resolution), min_x_ (0.0f), max_x_ (resolution), min_y_ (0.0f), + max_y_ (resolution), min_z_ (0.0f), max_z_ (resolution), bounding_box_defined_ (false), max_objs_per_leaf_(0) { assert (resolution > 0.0f); } @@ -96,11 +96,11 @@ pcl::octree::OctreePointCloud ////////////////////////////////////////////////////////////////////////////////////////////// template void -pcl::octree::OctreePointCloud::addPointFromCloud (const int pointIdx_arg, IndicesPtr indices_arg) +pcl::octree::OctreePointCloud::addPointFromCloud (const int point_idx_arg, IndicesPtr indices_arg) { - this->addPointIdx (pointIdx_arg); + this->addPointIdx (point_idx_arg); if (indices_arg) - indices_arg->push_back (pointIdx_arg); + indices_arg->push_back (point_idx_arg); } ////////////////////////////////////////////////////////////////////////////////////////////// @@ -142,10 +142,10 @@ pcl::octree::OctreePointCloud ////////////////////////////////////////////////////////////////////////////////////////////// template bool -pcl::octree::OctreePointCloud::isVoxelOccupiedAtPoint (const int& pointIdx_arg) const +pcl::octree::OctreePointCloud::isVoxelOccupiedAtPoint (const int& point_idx_arg) const { // retrieve point from input cloud - const PointT& point = this->input_->points[pointIdx_arg]; + const PointT& point = this->input_->points[point_idx_arg]; // search for voxel at point in octree return (this->isVoxelOccupiedAtPoint (point)); @@ -154,12 +154,12 @@ pcl::octree::OctreePointCloud ////////////////////////////////////////////////////////////////////////////////////////////// template bool pcl::octree::OctreePointCloud::isVoxelOccupiedAtPoint ( - const double pointX_arg, const double pointY_arg, const double pointZ_arg) const + const double point_x_arg, const double point_y_arg, const double point_z_arg) const { OctreeKey key; // generate key for point - this->genOctreeKeyforPoint (pointX_arg, pointY_arg, pointZ_arg, key); + this->genOctreeKeyforPoint (point_x_arg, point_y_arg, point_z_arg, key); return (this->existLeaf (key)); } @@ -178,10 +178,10 @@ pcl::octree::OctreePointCloud ////////////////////////////////////////////////////////////////////////////////////////////// template void -pcl::octree::OctreePointCloud::deleteVoxelAtPoint (const int& pointIdx_arg) +pcl::octree::OctreePointCloud::deleteVoxelAtPoint (const int& point_idx_arg) { // retrieve point from input cloud - const PointT& point = this->input_->points[pointIdx_arg]; + const PointT& point = this->input_->points[point_idx_arg]; // delete leaf at point this->deleteVoxelAtPoint (point); @@ -190,14 +190,14 @@ pcl::octree::OctreePointCloud ////////////////////////////////////////////////////////////////////////////////////////////// template int pcl::octree::OctreePointCloud::getOccupiedVoxelCenters ( - AlignedPointTVector &voxelCenterList_arg) const + AlignedPointTVector &voxel_center_list_arg) const { OctreeKey key; key.x = key.y = key.z = 0; - voxelCenterList_arg.clear (); + voxel_center_list_arg.clear (); - return getOccupiedVoxelCentersRecursive (this->rootNode_, key, voxelCenterList_arg); + return getOccupiedVoxelCentersRecursive (this->root_node_, key, voxel_center_list_arg); } @@ -273,7 +273,7 @@ pcl::octree::OctreePointCloud PointT max_pt; // bounding box cannot be changed once the octree contains elements - assert (this->leafCount_ == 0); + assert (this->leaf_count_ == 0); pcl::getMinMax3D (*input_, min_pt, max_pt); @@ -293,76 +293,76 @@ pcl::octree::OctreePointCloud ////////////////////////////////////////////////////////////////////////////////////////////// template void -pcl::octree::OctreePointCloud::defineBoundingBox (const double minX_arg, - const double minY_arg, - const double minZ_arg, - const double maxX_arg, - const double maxY_arg, - const double maxZ_arg) +pcl::octree::OctreePointCloud::defineBoundingBox (const double min_x_arg, + const double min_y_arg, + const double min_z_arg, + const double max_x_arg, + const double max_y_arg, + const double max_z_arg) { // bounding box cannot be changed once the octree contains elements - assert (this->leafCount_ == 0); + assert (this->leaf_count_ == 0); - assert (maxX_arg >= minX_arg); - assert (maxY_arg >= minY_arg); - assert (maxZ_arg >= minZ_arg); + assert (max_x_arg >= min_x_arg); + assert (max_y_arg >= min_y_arg); + assert (max_z_arg >= min_z_arg); - minX_ = minX_arg; - maxX_ = maxX_arg; + min_x_ = min_x_arg; + max_x_ = max_x_arg; - minY_ = minY_arg; - maxY_ = maxY_arg; + min_y_ = min_y_arg; + max_y_ = max_y_arg; - minZ_ = minZ_arg; - maxZ_ = maxZ_arg; + min_z_ = min_z_arg; + max_z_ = max_z_arg; - minX_ = min (minX_, maxX_); - minY_ = min (minY_, maxY_); - minZ_ = min (minZ_, maxZ_); + min_x_ = min (min_x_, max_x_); + min_y_ = min (min_y_, max_y_); + min_z_ = min (min_z_, max_z_); - maxX_ = max (minX_, maxX_); - maxY_ = max (minY_, maxY_); - maxZ_ = max (minZ_, maxZ_); + max_x_ = max (min_x_, max_x_); + max_y_ = max (min_y_, max_y_); + max_z_ = max (min_z_, max_z_); // generate bit masks for octree getKeyBitSize (); - boundingBoxDefined_ = true; + bounding_box_defined_ = true; } ////////////////////////////////////////////////////////////////////////////////////////////// template void pcl::octree::OctreePointCloud::defineBoundingBox ( - const double maxX_arg, const double maxY_arg, const double maxZ_arg) + const double max_x_arg, const double max_y_arg, const double max_z_arg) { // bounding box cannot be changed once the octree contains elements - assert (this->leafCount_ == 0); + assert (this->leaf_count_ == 0); - assert (maxX_arg >= 0.0f); - assert (maxY_arg >= 0.0f); - assert (maxZ_arg >= 0.0f); + assert (max_x_arg >= 0.0f); + assert (max_y_arg >= 0.0f); + assert (max_z_arg >= 0.0f); - minX_ = 0.0f; - maxX_ = maxX_arg; + min_x_ = 0.0f; + max_x_ = max_x_arg; - minY_ = 0.0f; - maxY_ = maxY_arg; + min_y_ = 0.0f; + max_y_ = max_y_arg; - minZ_ = 0.0f; - maxZ_ = maxZ_arg; + min_z_ = 0.0f; + max_z_ = max_z_arg; - minX_ = min (minX_, maxX_); - minY_ = min (minY_, maxY_); - minZ_ = min (minZ_, maxZ_); + min_x_ = min (min_x_, max_x_); + min_y_ = min (min_y_, max_y_); + min_z_ = min (min_z_, max_z_); - maxX_ = max (minX_, maxX_); - maxY_ = max (minY_, maxY_); - maxZ_ = max (minZ_, maxZ_); + max_x_ = max (min_x_, max_x_); + max_y_ = max (min_y_, max_y_); + max_z_ = max (min_z_, max_z_); // generate bit masks for octree getKeyBitSize (); - boundingBoxDefined_ = true; + bounding_box_defined_ = true; } ////////////////////////////////////////////////////////////////////////////////////////////// @@ -370,53 +370,53 @@ template::defineBoundingBox (const double cubeLen_arg) { // bounding box cannot be changed once the octree contains elements - assert (this->leafCount_ == 0); + assert (this->leaf_count_ == 0); assert (cubeLen_arg >= 0.0f); - minX_ = 0.0f; - maxX_ = cubeLen_arg; + min_x_ = 0.0f; + max_x_ = cubeLen_arg; - minY_ = 0.0f; - maxY_ = cubeLen_arg; + min_y_ = 0.0f; + max_y_ = cubeLen_arg; - minZ_ = 0.0f; - maxZ_ = cubeLen_arg; + min_z_ = 0.0f; + max_z_ = cubeLen_arg; - minX_ = min (minX_, maxX_); - minY_ = min (minY_, maxY_); - minZ_ = min (minZ_, maxZ_); + min_x_ = min (min_x_, max_x_); + min_y_ = min (min_y_, max_y_); + min_z_ = min (min_z_, max_z_); - maxX_ = max (minX_, maxX_); - maxY_ = max (minY_, maxY_); - maxZ_ = max (minZ_, maxZ_); + max_x_ = max (min_x_, max_x_); + max_y_ = max (min_y_, max_y_); + max_z_ = max (min_z_, max_z_); // generate bit masks for octree getKeyBitSize (); - boundingBoxDefined_ = true; + bounding_box_defined_ = true; } ////////////////////////////////////////////////////////////////////////////////////////////// template void pcl::octree::OctreePointCloud::getBoundingBox ( - double& minX_arg, double& minY_arg, double& minZ_arg, - double& maxX_arg, double& maxY_arg, double& maxZ_arg) const + double& min_x_arg, double& min_y_arg, double& min_z_arg, + double& max_x_arg, double& max_y_arg, double& max_z_arg) const { - minX_arg = minX_; - minY_arg = minY_; - minZ_arg = minZ_; + min_x_arg = min_x_; + min_y_arg = min_y_; + min_z_arg = min_z_; - maxX_arg = maxX_; - maxY_arg = maxY_; - maxZ_arg = maxZ_; + max_x_arg = max_x_; + max_y_arg = max_y_; + max_z_arg = max_z_; } ////////////////////////////////////////////////////////////////////////////////////////////// template void -pcl::octree::OctreePointCloud::adoptBoundingBoxToPoint (const PointT& pointIdx_arg) +pcl::octree::OctreePointCloud::adoptBoundingBoxToPoint (const PointT& point_idx_arg) { const float minValue = std::numeric_limits::epsilon (); @@ -424,77 +424,77 @@ pcl::octree::OctreePointCloud // increase octree size until point fits into bounding box while (true) { - bool bLowerBoundViolationX = (pointIdx_arg.x < minX_); - bool bLowerBoundViolationY = (pointIdx_arg.y < minY_); - bool bLowerBoundViolationZ = (pointIdx_arg.z < minZ_); + bool bLowerBoundViolationX = (point_idx_arg.x < min_x_); + bool bLowerBoundViolationY = (point_idx_arg.y < min_y_); + bool bLowerBoundViolationZ = (point_idx_arg.z < min_z_); - bool bUpperBoundViolationX = (pointIdx_arg.x >= maxX_); - bool bUpperBoundViolationY = (pointIdx_arg.y >= maxY_); - bool bUpperBoundViolationZ = (pointIdx_arg.z >= maxZ_); + bool bUpperBoundViolationX = (point_idx_arg.x >= max_x_); + bool bUpperBoundViolationY = (point_idx_arg.y >= max_y_); + bool bUpperBoundViolationZ = (point_idx_arg.z >= max_z_); // do we violate any bounds? if (bLowerBoundViolationX || bLowerBoundViolationY || bLowerBoundViolationZ || bUpperBoundViolationX || bUpperBoundViolationY || bUpperBoundViolationZ ) { - if (boundingBoxDefined_) + if (bounding_box_defined_) { double octreeSideLen; - unsigned char childIdx; + unsigned char child_idx; // octree not empty - we add another tree level and thus increase its size by a factor of 2*2*2 - childIdx = static_cast (((!bUpperBoundViolationX) << 2) | ((!bUpperBoundViolationY) << 1) + child_idx = static_cast (((!bUpperBoundViolationX) << 2) | ((!bUpperBoundViolationY) << 1) | ((!bUpperBoundViolationZ))); BranchNode* newRootBranch; newRootBranch = new BranchNode(); - this->branchCount_++; + this->branch_count_++; - this->setBranchChildPtr (*newRootBranch, childIdx, this->rootNode_); + this->setBranchChildPtr (*newRootBranch, child_idx, this->root_node_); - this->rootNode_ = newRootBranch; + this->root_node_ = newRootBranch; - octreeSideLen = static_cast (1 << this->octreeDepth_) * resolution_; + octreeSideLen = static_cast (1 << this->octree_depth_) * resolution_; if (!bUpperBoundViolationX) - minX_ -= octreeSideLen; + min_x_ -= octreeSideLen; if (!bUpperBoundViolationY) - minY_ -= octreeSideLen; + min_y_ -= octreeSideLen; if (!bUpperBoundViolationZ) - minZ_ -= octreeSideLen; + min_z_ -= octreeSideLen; // configure tree depth of octree - this->octreeDepth_++; - this->setTreeDepth (this->octreeDepth_); + this->octree_depth_++; + this->setTreeDepth (this->octree_depth_); // recalculate bounding box width - octreeSideLen = static_cast (1 << this->octreeDepth_) * resolution_ - minValue; + octreeSideLen = static_cast (1 << this->octree_depth_) * resolution_ - minValue; // increase octree bounding box - maxX_ = minX_ + octreeSideLen; - maxY_ = minY_ + octreeSideLen; - maxZ_ = minZ_ + octreeSideLen; + max_x_ = min_x_ + octreeSideLen; + max_y_ = min_y_ + octreeSideLen; + max_z_ = min_z_ + octreeSideLen; } // bounding box is not defined - set it to point position else { // octree is empty - we set the center of the bounding box to our first pixel - this->minX_ = pointIdx_arg.x - this->resolution_ / 2; - this->minY_ = pointIdx_arg.y - this->resolution_ / 2; - this->minZ_ = pointIdx_arg.z - this->resolution_ / 2; + this->min_x_ = point_idx_arg.x - this->resolution_ / 2; + this->min_y_ = point_idx_arg.y - this->resolution_ / 2; + this->min_z_ = point_idx_arg.z - this->resolution_ / 2; - this->maxX_ = pointIdx_arg.x + this->resolution_ / 2; - this->maxY_ = pointIdx_arg.y + this->resolution_ / 2; - this->maxZ_ = pointIdx_arg.z + this->resolution_ / 2; + this->max_x_ = point_idx_arg.x + this->resolution_ / 2; + this->max_y_ = point_idx_arg.y + this->resolution_ / 2; + this->max_z_ = point_idx_arg.z + this->resolution_ / 2; getKeyBitSize (); - boundingBoxDefined_ = true; + bounding_box_defined_ = true; } } @@ -512,21 +512,21 @@ pcl::octree::OctreePointCloud if (depth_mask) { // get amount of objects in leaf container - size_t leafObjCount = (*leaf_node)->getSize (); + size_t leaf_obj_count = (*leaf_node)->getSize (); // copy leaf data std::vector leafIndices; - leafIndices.reserve(leafObjCount); + leafIndices.reserve(leaf_obj_count); (*leaf_node)->getPointIndices(leafIndices); // delete current leaf node this->deleteBranchChild(*parent_branch, child_idx); - this->leafCount_ --; + this->leaf_count_ --; // create new branch node BranchNode* childBranch = this->createBranchChild (*parent_branch, child_idx); - this->branchCount_ ++; + this->branch_count_ ++; typename std::vector::iterator it = leafIndices.begin(); typename std::vector::const_iterator it_end = leafIndices.end(); @@ -555,13 +555,13 @@ pcl::octree::OctreePointCloud ////////////////////////////////////////////////////////////////////////////////////////////// template void -pcl::octree::OctreePointCloud::addPointIdx (const int pointIdx_arg) +pcl::octree::OctreePointCloud::addPointIdx (const int point_idx_arg) { OctreeKey key; - assert (pointIdx_arg < static_cast (input_->points.size ())); + assert (point_idx_arg < static_cast (input_->points.size ())); - const PointT& point = input_->points[pointIdx_arg]; + const PointT& point = input_->points[point_idx_arg]; // make sure bounding box is big enough adoptBoundingBoxToPoint (point); @@ -569,32 +569,32 @@ pcl::octree::OctreePointCloud // generate key genOctreeKeyforPoint (point, key); - LeafNode* leafNode; - BranchNode* parentBranchOfLeafNode; - unsigned int depth_mask = this->createLeafRecursive (key, this->depthMask_ ,this->rootNode_, leafNode, parentBranchOfLeafNode); + LeafNode* leaf_node; + BranchNode* parent_branch_of_leaf_node; + unsigned int depth_mask = createLeafRecursive (key, this->depth_mask_ ,this->root_node_, leaf_node, parent_branch_of_leaf_node); if (this->dynamic_depth_enabled_ && depth_mask) { // get amount of objects in leaf container - size_t leafObjCount = (*leafNode)->getSize (); + size_t leaf_obj_count = (*leaf_node)->getSize (); - while (leafObjCount>=maxObjsPerLeaf_ && depth_mask) + while (leaf_obj_count>=max_objs_per_leaf_ && depth_mask) { // index to branch child - unsigned char childIdx = key.getChildIdxWithDepthMask (depth_mask*2); + unsigned char child_idx = key.getChildIdxWithDepthMask (depth_mask*2); - expandLeafNode (leafNode, - parentBranchOfLeafNode, - childIdx, + expandLeafNode (leaf_node, + parent_branch_of_leaf_node, + child_idx, depth_mask); - depth_mask = this->createLeafRecursive (key, this->depthMask_ ,this->rootNode_, leafNode, parentBranchOfLeafNode); - leafObjCount = (*leafNode)->getSize (); + depth_mask = createLeafRecursive (key, this->depth_mask_ ,this->root_node_, leaf_node, parent_branch_of_leaf_node); + leaf_obj_count = (*leaf_node)->getSize (); } } - (*leafNode)->addPointIndex (pointIdx_arg); + (*leaf_node)->addPointIndex (point_idx_arg); } ////////////////////////////////////////////////////////////////////////////////////////////// @@ -610,58 +610,58 @@ pcl::octree::OctreePointCloud template void pcl::octree::OctreePointCloud::getKeyBitSize () { - unsigned int maxVoxels; + unsigned int max_voxels; - unsigned int maxKeyX; - unsigned int maxKeyY; - unsigned int maxKeyZ; + unsigned int max_key_x; + unsigned int max_key_y; + unsigned int max_key_z; - double octreeSideLen; + double octree_side_len; const float minValue = std::numeric_limits::epsilon(); // find maximum key values for x, y, z - maxKeyX = static_cast ((maxX_ - minX_) / resolution_); - maxKeyY = static_cast ((maxY_ - minY_) / resolution_); - maxKeyZ = static_cast ((maxZ_ - minZ_) / resolution_); + max_key_x = static_cast ((max_x_ - min_x_) / resolution_); + max_key_y = static_cast ((max_y_ - min_y_) / resolution_); + max_key_z = static_cast ((max_z_ - min_z_) / resolution_); // find maximum amount of keys - maxVoxels = max (max (max (maxKeyX, maxKeyY), maxKeyZ), static_cast (2)); + max_voxels = max (max (max (max_key_x, max_key_y), max_key_z), static_cast (2)); - // tree depth == amount of bits of maxVoxels - this->octreeDepth_ = max ((min (static_cast (OCT_MAXTREEDEPTH), static_cast (ceil (this->Log2 (maxVoxels)-minValue)))), + // tree depth == amount of bits of max_voxels + this->octree_depth_ = max ((min (static_cast (OctreeKey::maxDepth), static_cast (ceil (this->Log2 (max_voxels)-minValue)))), static_cast (0)); - octreeSideLen = static_cast (1 << this->octreeDepth_) * resolution_-minValue; + octree_side_len = static_cast (1 << this->octree_depth_) * resolution_-minValue; - if (this->leafCount_ == 0) + if (this->leaf_count_ == 0) { - double octreeOversizeX; - double octreeOversizeY; - double octreeOversizeZ; + double octree_oversize_x; + double octree_oversize_y; + double octree_oversize_z; - octreeOversizeX = (octreeSideLen - (maxX_ - minX_)) / 2.0; - octreeOversizeY = (octreeSideLen - (maxY_ - minY_)) / 2.0; - octreeOversizeZ = (octreeSideLen - (maxZ_ - minZ_)) / 2.0; + octree_oversize_x = (octree_side_len - (max_x_ - min_x_)) / 2.0; + octree_oversize_y = (octree_side_len - (max_y_ - min_y_)) / 2.0; + octree_oversize_z = (octree_side_len - (max_z_ - min_z_)) / 2.0; - minX_ -= octreeOversizeX; - minY_ -= octreeOversizeY; - minZ_ -= octreeOversizeZ; + min_x_ -= octree_oversize_x; + min_y_ -= octree_oversize_y; + min_z_ -= octree_oversize_z; - maxX_ += octreeOversizeX; - maxY_ += octreeOversizeY; - maxZ_ += octreeOversizeZ; + max_x_ += octree_oversize_x; + max_y_ += octree_oversize_y; + max_z_ += octree_oversize_z; } else { - maxX_ = minX_ + octreeSideLen; - maxY_ = minY_ + octreeSideLen; - maxZ_ = minZ_ + octreeSideLen; + max_x_ = min_x_ + octree_side_len; + max_y_ = min_y_ + octree_side_len; + max_z_ = min_z_ + octree_side_len; } // configure tree depth of octree - this->setTreeDepth (this->octreeDepth_); + this->setTreeDepth (this->octree_depth_); } @@ -671,35 +671,35 @@ pcl::octree::OctreePointCloud OctreeKey & key_arg) const { // calculate integer key for point coordinates - key_arg.x = static_cast ((point_arg.x - this->minX_) / this->resolution_); - key_arg.y = static_cast ((point_arg.y - this->minY_) / this->resolution_); - key_arg.z = static_cast ((point_arg.z - this->minZ_) / this->resolution_); + key_arg.x = static_cast ((point_arg.x - this->min_x_) / this->resolution_); + key_arg.y = static_cast ((point_arg.y - this->min_y_) / this->resolution_); + key_arg.z = static_cast ((point_arg.z - this->min_z_) / this->resolution_); } ////////////////////////////////////////////////////////////////////////////////////////////// template void pcl::octree::OctreePointCloud::genOctreeKeyforPoint ( - const double pointX_arg, const double pointY_arg, - const double pointZ_arg, OctreeKey & key_arg) const + const double point_x_arg, const double point_y_arg, + const double point_z_arg, OctreeKey & key_arg) const { - PointT tempPoint; + PointT temp_point; - tempPoint.x = static_cast (pointX_arg); - tempPoint.y = static_cast (pointY_arg); - tempPoint.z = static_cast (pointZ_arg); + temp_point.x = static_cast (point_x_arg); + temp_point.y = static_cast (point_y_arg); + temp_point.z = static_cast (point_z_arg); // generate key for point - genOctreeKeyforPoint (tempPoint, key_arg); + genOctreeKeyforPoint (temp_point, key_arg); } ////////////////////////////////////////////////////////////////////////////////////////////// template bool pcl::octree::OctreePointCloud::genOctreeKeyForDataT (const int& data_arg, OctreeKey & key_arg) const { - const PointT tempPoint = getPointByIndex (data_arg); + const PointT temp_point = getPointByIndex (data_arg); // generate key for point - genOctreeKeyforPoint (tempPoint, key_arg); + genOctreeKeyforPoint (temp_point, key_arg); return (true); } @@ -709,66 +709,66 @@ template::genLeafNodeCenterFromOctreeKey (const OctreeKey & key, PointT & point) const { // define point to leaf node voxel center - point.x = static_cast ((static_cast (key.x) + 0.5f) * this->resolution_ + this->minX_); - point.y = static_cast ((static_cast (key.y) + 0.5f) * this->resolution_ + this->minY_); - point.z = static_cast ((static_cast (key.z) + 0.5f) * this->resolution_ + this->minZ_); + point.x = static_cast ((static_cast (key.x) + 0.5f) * this->resolution_ + this->min_x_); + point.y = static_cast ((static_cast (key.y) + 0.5f) * this->resolution_ + this->min_y_); + point.z = static_cast ((static_cast (key.z) + 0.5f) * this->resolution_ + this->min_z_); } ////////////////////////////////////////////////////////////////////////////////////////////// template void pcl::octree::OctreePointCloud::genVoxelCenterFromOctreeKey ( const OctreeKey & key_arg, - unsigned int treeDepth_arg, + unsigned int tree_depth_arg, PointT& point_arg) const { // generate point for voxel center defined by treedepth (bitLen) and key - point_arg.x = static_cast ((static_cast (key_arg.x) + 0.5f) * (this->resolution_ * static_cast (1 << (this->octreeDepth_ - treeDepth_arg))) + this->minX_); - point_arg.y = static_cast ((static_cast (key_arg.y) + 0.5f) * (this->resolution_ * static_cast (1 << (this->octreeDepth_ - treeDepth_arg))) + this->minY_); - point_arg.z = static_cast ((static_cast (key_arg.z) + 0.5f) * (this->resolution_ * static_cast (1 << (this->octreeDepth_ - treeDepth_arg))) + this->minZ_); + point_arg.x = static_cast ((static_cast (key_arg.x) + 0.5f) * (this->resolution_ * static_cast (1 << (this->octree_depth_ - tree_depth_arg))) + this->min_x_); + point_arg.y = static_cast ((static_cast (key_arg.y) + 0.5f) * (this->resolution_ * static_cast (1 << (this->octree_depth_ - tree_depth_arg))) + this->min_y_); + point_arg.z = static_cast ((static_cast (key_arg.z) + 0.5f) * (this->resolution_ * static_cast (1 << (this->octree_depth_ - tree_depth_arg))) + this->min_z_); } ////////////////////////////////////////////////////////////////////////////////////////////// template void pcl::octree::OctreePointCloud::genVoxelBoundsFromOctreeKey ( const OctreeKey & key_arg, - unsigned int treeDepth_arg, + unsigned int tree_depth_arg, Eigen::Vector3f &min_pt, Eigen::Vector3f &max_pt) const { // calculate voxel size of current tree depth - double voxel_side_len = this->resolution_ * static_cast (1 << (this->octreeDepth_ - treeDepth_arg)); + double voxel_side_len = this->resolution_ * static_cast (1 << (this->octree_depth_ - tree_depth_arg)); // calculate voxel bounds - min_pt (0) = static_cast (static_cast (key_arg.x) * voxel_side_len + this->minX_); - min_pt (1) = static_cast (static_cast (key_arg.y) * voxel_side_len + this->minY_); - min_pt (2) = static_cast (static_cast (key_arg.z) * voxel_side_len + this->minZ_); + min_pt (0) = static_cast (static_cast (key_arg.x) * voxel_side_len + this->min_x_); + min_pt (1) = static_cast (static_cast (key_arg.y) * voxel_side_len + this->min_y_); + min_pt (2) = static_cast (static_cast (key_arg.z) * voxel_side_len + this->min_z_); - max_pt (0) = static_cast (static_cast (key_arg.x + 1) * voxel_side_len + this->minX_); - max_pt (1) = static_cast (static_cast (key_arg.y + 1) * voxel_side_len + this->minY_); - max_pt (2) = static_cast (static_cast (key_arg.z + 1) * voxel_side_len + this->minZ_); + max_pt (0) = static_cast (static_cast (key_arg.x + 1) * voxel_side_len + this->min_x_); + max_pt (1) = static_cast (static_cast (key_arg.y + 1) * voxel_side_len + this->min_y_); + max_pt (2) = static_cast (static_cast (key_arg.z + 1) * voxel_side_len + this->min_z_); } ////////////////////////////////////////////////////////////////////////////////////////////// template double -pcl::octree::OctreePointCloud::getVoxelSquaredSideLen (unsigned int treeDepth_arg) const +pcl::octree::OctreePointCloud::getVoxelSquaredSideLen (unsigned int tree_depth_arg) const { - double sideLen; + double side_len; // side length of the voxel cube increases exponentially with the octree depth - sideLen = this->resolution_ * static_cast(1 << (this->octreeDepth_ - treeDepth_arg)); + side_len = this->resolution_ * static_cast(1 << (this->octree_depth_ - tree_depth_arg)); // squared voxel side length - sideLen *= sideLen; + side_len *= side_len; - return (sideLen); + return (side_len); } ////////////////////////////////////////////////////////////////////////////////////////////// template double -pcl::octree::OctreePointCloud::getVoxelSquaredDiameter (unsigned int treeDepth_arg) const +pcl::octree::OctreePointCloud::getVoxelSquaredDiameter (unsigned int tree_depth_arg) const { // return the squared side length of the voxel cube as a function of the octree depth - return (getVoxelSquaredSideLen (treeDepth_arg) * 3); + return (getVoxelSquaredSideLen (tree_depth_arg) * 3); } ////////////////////////////////////////////////////////////////////////////////////////////// @@ -776,51 +776,51 @@ template::getOccupiedVoxelCentersRecursive ( const BranchNode* node_arg, const OctreeKey& key_arg, - AlignedPointTVector &voxelCenterList_arg) const + AlignedPointTVector &voxel_center_list_arg) const { // child iterator - unsigned char childIdx; + unsigned char child_idx; - int voxelCount = 0; + int voxel_count = 0; // iterate over all children - for (childIdx = 0; childIdx < 8; childIdx++) + for (child_idx = 0; child_idx < 8; child_idx++) { - if (!this->branchHasChild (*node_arg, childIdx)) + if (!this->branchHasChild (*node_arg, child_idx)) continue; - const OctreeNode * childNode; - childNode = this->getBranchChildPtr (*node_arg, childIdx); + const OctreeNode * child_node; + child_node = this->getBranchChildPtr (*node_arg, child_idx); // generate new key for current branch voxel - OctreeKey newKey; - newKey.x = (key_arg.x << 1) | (!!(childIdx & (1 << 2))); - newKey.y = (key_arg.y << 1) | (!!(childIdx & (1 << 1))); - newKey.z = (key_arg.z << 1) | (!!(childIdx & (1 << 0))); + OctreeKey new_key; + new_key.x = (key_arg.x << 1) | (!!(child_idx & (1 << 2))); + new_key.y = (key_arg.y << 1) | (!!(child_idx & (1 << 1))); + new_key.z = (key_arg.z << 1) | (!!(child_idx & (1 << 0))); - switch (childNode->getNodeType ()) + switch (child_node->getNodeType ()) { case BRANCH_NODE: { // recursively proceed with indexed child branch - voxelCount += getOccupiedVoxelCentersRecursive (static_cast (childNode), newKey, voxelCenterList_arg); + voxel_count += getOccupiedVoxelCentersRecursive (static_cast (child_node), new_key, voxel_center_list_arg); break; } case LEAF_NODE: { - PointT newPoint; + PointT new_point; - genLeafNodeCenterFromOctreeKey (newKey, newPoint); - voxelCenterList_arg.push_back (newPoint); + genLeafNodeCenterFromOctreeKey (new_key, new_point); + voxel_center_list_arg.push_back (new_point); - voxelCount++; + voxel_count++; break; } default: break; } } - return (voxelCount); + return (voxel_count); } #define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithLeafDataTVector(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud >; diff --git a/octree/include/pcl/octree/impl/octree_pointcloud_supervoxel.hpp b/octree/include/pcl/octree/impl/octree_pointcloud_supervoxel.hpp index 3c4c73dfd89..a3577f44ece 100644 --- a/octree/include/pcl/octree/impl/octree_pointcloud_supervoxel.hpp +++ b/octree/include/pcl/octree/impl/octree_pointcloud_supervoxel.hpp @@ -159,7 +159,7 @@ pcl::octree::OctreePointCloudSuperVoxel void pcl::octree::OctreePointCloudSuperVoxel::insertNormals (const std::vector > &voxel_centroids_arg) { - assert (voxel_centroids_arg.size () == this->leafCount_); + assert (voxel_centroids_arg.size () == this->leaf_count_); typename OctreeSuperVoxelT::LeafNodeIterator leaf_itr; leaf_itr = this->leaf_begin (); LeafContainerT* leafContainer; diff --git a/octree/include/pcl/octree/impl/octree_pointcloud_voxelcentroid.hpp b/octree/include/pcl/octree/impl/octree_pointcloud_voxelcentroid.hpp index b4f7cd16dab..d6a5cf8a268 100644 --- a/octree/include/pcl/octree/impl/octree_pointcloud_voxelcentroid.hpp +++ b/octree/include/pcl/octree/impl/octree_pointcloud_voxelcentroid.hpp @@ -73,9 +73,9 @@ pcl::octree::OctreePointCloudVoxelCentroidleafCount_); + voxel_centroid_list_arg.reserve (this->leaf_count_); - getVoxelCentroidsRecursive (this->rootNode_, new_key, voxel_centroid_list_arg ); + getVoxelCentroidsRecursive (this->root_node_, new_key, voxel_centroid_list_arg ); // return size of centroid vector return (voxel_centroid_list_arg.size ()); @@ -99,21 +99,21 @@ pcl::octree::OctreePointCloudVoxelCentroidgetChildPtr (child_idx); + OctreeNode *child_node = branch_arg->getChildPtr (child_idx); - switch (childNode->getNodeType ()) + switch (child_node->getNodeType ()) { case BRANCH_NODE: { // recursively proceed with indexed child branch - getVoxelCentroidsRecursive (static_cast (childNode), key_arg, voxel_centroid_list_arg); + getVoxelCentroidsRecursive (static_cast (child_node), key_arg, voxel_centroid_list_arg); break; } case LEAF_NODE: { PointT new_centroid; - LeafNode* container = static_cast (childNode); + LeafNode* container = static_cast (child_node); container->getContainer().getCentroid (new_centroid); diff --git a/octree/include/pcl/octree/impl/octree_search.hpp b/octree/include/pcl/octree/impl/octree_search.hpp index 671c1b7c84c..95fadc41cf9 100644 --- a/octree/include/pcl/octree/impl/octree_search.hpp +++ b/octree/include/pcl/octree/impl/octree_search.hpp @@ -50,7 +50,7 @@ using namespace std; ////////////////////////////////////////////////////////////////////////////////////////////// template bool pcl::octree::OctreePointCloudSearch::voxelSearch (const PointT& point, - std::vector& pointIdx_data) + std::vector& point_idx_data) { assert (isFinite (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"); OctreeKey key; @@ -63,7 +63,7 @@ pcl::octree::OctreePointCloudSearch::v if (leaf) { - (*leaf).getPointIndices (pointIdx_data); + (*leaf).getPointIndices (point_idx_data); b_success = true; } @@ -73,10 +73,10 @@ pcl::octree::OctreePointCloudSearch::v ////////////////////////////////////////////////////////////////////////////////////////////// template bool pcl::octree::OctreePointCloudSearch::voxelSearch (const int index, - std::vector& pointIdx_data) + std::vector& point_idx_data) { const PointT search_point = this->getPointByIndex (index); - return (this->voxelSearch (search_point, pointIdx_data)); + return (this->voxelSearch (search_point, point_idx_data)); } ////////////////////////////////////////////////////////////////////////////////////////////// @@ -85,7 +85,7 @@ pcl::octree::OctreePointCloudSearch::n std::vector &k_indices, std::vector &k_sqr_distances) { - assert(this->leafCount_>0); + assert(this->leaf_count_>0); assert (isFinite (p_q) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"); k_indices.clear (); @@ -95,28 +95,28 @@ pcl::octree::OctreePointCloudSearch::n return 0; unsigned int i; - unsigned int resultCount; + unsigned int result_count; - prioPointQueueEntry pointEntry; - std::vector pointCandidates; + prioPointQueueEntry point_entry; + std::vector point_candidates; OctreeKey key; key.x = key.y = key.z = 0; // initalize smallest point distance in search with high value - double smallestDist = numeric_limits::max (); + double smallest_dist = numeric_limits::max (); - getKNearestNeighborRecursive (p_q, k, this->rootNode_, key, 1, smallestDist, pointCandidates); + getKNearestNeighborRecursive (p_q, k, this->root_node_, key, 1, smallest_dist, point_candidates); - resultCount = static_cast (pointCandidates.size ()); + result_count = static_cast (point_candidates.size ()); - k_indices.resize (resultCount); - k_sqr_distances.resize (resultCount); + k_indices.resize (result_count); + k_sqr_distances.resize (result_count); - for (i = 0; i < resultCount; ++i) + for (i = 0; i < result_count; ++i) { - k_indices [i] = pointCandidates [i].pointIdx_; - k_sqr_distances [i] = pointCandidates [i].pointDistance_; + k_indices [i] = point_candidates [i].point_idx_; + k_sqr_distances [i] = point_candidates [i].point_distance_; } return static_cast (k_indices.size ()); @@ -138,13 +138,13 @@ pcl::octree::OctreePointCloudSearch::a int &result_index, float &sqr_distance) { - assert(this->leafCount_>0); + assert(this->leaf_count_>0); assert (isFinite (p_q) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"); OctreeKey key; key.x = key.y = key.z = 0; - approxNearestSearchRecursive (p_q, this->rootNode_, key, 1, result_index, sqr_distance); + approxNearestSearchRecursive (p_q, this->root_node_, key, 1, result_index, sqr_distance); return; } @@ -154,9 +154,9 @@ template vo pcl::octree::OctreePointCloudSearch::approxNearestSearch (int query_index, int &result_index, float &sqr_distance) { - const PointT searchPoint = this->getPointByIndex (query_index); + const PointT search_point = this->getPointByIndex (query_index); - return (approxNearestSearch (searchPoint, result_index, sqr_distance)); + return (approxNearestSearch (search_point, result_index, sqr_distance)); } ////////////////////////////////////////////////////////////////////////////////////////////// @@ -173,7 +173,7 @@ pcl::octree::OctreePointCloudSearch::r k_indices.clear (); k_sqr_distances.clear (); - getNeighborsWithinRadiusRecursive (p_q, radius * radius, this->rootNode_, key, 1, k_indices, k_sqr_distances, + getNeighborsWithinRadiusRecursive (p_q, radius * radius, this->root_node_, key, 1, k_indices, k_sqr_distances, max_nn); return (static_cast (k_indices.size ())); @@ -203,7 +203,7 @@ pcl::octree::OctreePointCloudSearch::b k_indices.clear (); - boxSearchRecursive (min_pt, max_pt, this->rootNode_, key, 1, k_indices); + boxSearchRecursive (min_pt, max_pt, this->root_node_, key, 1, k_indices); return (static_cast (k_indices.size ())); @@ -212,160 +212,158 @@ pcl::octree::OctreePointCloudSearch::b ////////////////////////////////////////////////////////////////////////////////////////////// template double pcl::octree::OctreePointCloudSearch::getKNearestNeighborRecursive ( - const PointT & point, unsigned int K, const BranchNode* node, const OctreeKey& key, unsigned int treeDepth, - const double squaredSearchRadius, std::vector& pointCandidates) const + const PointT & point, unsigned int K, const BranchNode* node, const OctreeKey& key, unsigned int tree_depth, + const double squared_search_radius, std::vector& point_candidates) const { - std::vector searchEntryHeap; - searchEntryHeap.resize (8); + std::vector search_heap; + search_heap.resize (8); - unsigned char childIdx; + unsigned char child_idx; - OctreeKey newKey; + OctreeKey new_key; - double smallestSquaredDist = squaredSearchRadius; + double smallest_squared_dist = squared_search_radius; // get spatial voxel information - double voxelSquaredDiameter = this->getVoxelSquaredDiameter (treeDepth); + double voxelSquaredDiameter = this->getVoxelSquaredDiameter (tree_depth); // iterate over all children - for (childIdx = 0; childIdx < 8; childIdx++) + for (child_idx = 0; child_idx < 8; child_idx++) { - if (this->branchHasChild (*node, childIdx)) + if (this->branchHasChild (*node, child_idx)) { - PointT voxelCenter; + PointT voxel_center; - searchEntryHeap[childIdx].key.x = (key.x << 1) + (!!(childIdx & (1 << 2))); - searchEntryHeap[childIdx].key.y = (key.y << 1) + (!!(childIdx & (1 << 1))); - searchEntryHeap[childIdx].key.z = (key.z << 1) + (!!(childIdx & (1 << 0))); + search_heap[child_idx].key.x = (key.x << 1) + (!!(child_idx & (1 << 2))); + search_heap[child_idx].key.y = (key.y << 1) + (!!(child_idx & (1 << 1))); + search_heap[child_idx].key.z = (key.z << 1) + (!!(child_idx & (1 << 0))); // generate voxel center point for voxel at key - this->genVoxelCenterFromOctreeKey (searchEntryHeap[childIdx].key, treeDepth, voxelCenter); + this->genVoxelCenterFromOctreeKey (search_heap[child_idx].key, tree_depth, voxel_center); // generate new priority queue element - searchEntryHeap[childIdx].node = this->getBranchChildPtr (*node, childIdx); - searchEntryHeap[childIdx].pointDistance = pointSquaredDist (voxelCenter, point); + search_heap[child_idx].node = this->getBranchChildPtr (*node, child_idx); + search_heap[child_idx].point_distance = pointSquaredDist (voxel_center, point); } else { - searchEntryHeap[childIdx].pointDistance = numeric_limits::infinity (); + search_heap[child_idx].point_distance = numeric_limits::infinity (); } } - std::sort (searchEntryHeap.begin (), searchEntryHeap.end ()); + std::sort (search_heap.begin (), search_heap.end ()); // iterate over all children in priority queue - // check if the distance to search candidate is smaller than the best point distance (smallestSquaredDist) - while ((!searchEntryHeap.empty ()) - && (searchEntryHeap.back ().pointDistance - < smallestSquaredDist + voxelSquaredDiameter / 4.0 + sqrt (smallestSquaredDist * voxelSquaredDiameter) - - this->epsilon_)) + // check if the distance to search candidate is smaller than the best point distance (smallest_squared_dist) + while ((!search_heap.empty ()) && (search_heap.back ().point_distance < + smallest_squared_dist + voxelSquaredDiameter / 4.0 + sqrt (smallest_squared_dist * voxelSquaredDiameter) - this->epsilon_)) { - const OctreeNode* childNode; + const OctreeNode* child_node; // read from priority queue element - childNode = searchEntryHeap.back ().node; - newKey = searchEntryHeap.back ().key; + child_node = search_heap.back ().node; + new_key = search_heap.back ().key; - if (treeDepth < this->octreeDepth_) + if (tree_depth < this->octree_depth_) { // we have not reached maximum tree depth - smallestSquaredDist = getKNearestNeighborRecursive (point, K, static_cast (childNode), newKey, treeDepth + 1, - smallestSquaredDist, pointCandidates); + smallest_squared_dist = getKNearestNeighborRecursive (point, K, static_cast (child_node), new_key, tree_depth + 1, + smallest_squared_dist, point_candidates); } else { // we reached leaf node level - float squaredDist; + float squared_dist; size_t i; - vector decodedPointVector; + vector decoded_point_vector; - const LeafNode* childLeaf = static_cast (childNode); + const LeafNode* child_leaf = static_cast (child_node); - // decode leaf node into decodedPointVector - (*childLeaf)->getPointIndices (decodedPointVector); + // decode leaf node into decoded_point_vector + (*child_leaf)->getPointIndices (decoded_point_vector); // Linearly iterate over all decoded (unsorted) points - for (i = 0; i < decodedPointVector.size (); i++) + for (i = 0; i < decoded_point_vector.size (); i++) { - const PointT& candidatePoint = this->getPointByIndex (decodedPointVector[i]); + const PointT& candidate_point = this->getPointByIndex (decoded_point_vector[i]); // calculate point distance to search point - squaredDist = pointSquaredDist (candidatePoint, point); + squared_dist = pointSquaredDist (candidate_point, point); // check if a closer match is found - if (squaredDist < smallestSquaredDist) + if (squared_dist < smallest_squared_dist) { - prioPointQueueEntry pointEntry; + prioPointQueueEntry point_entry; - pointEntry.pointDistance_ = squaredDist; - pointEntry.pointIdx_ = decodedPointVector[i]; - pointCandidates.push_back (pointEntry); + point_entry.point_distance_ = squared_dist; + point_entry.point_idx_ = decoded_point_vector[i]; + point_candidates.push_back (point_entry); } } - std::sort (pointCandidates.begin (), pointCandidates.end ()); + std::sort (point_candidates.begin (), point_candidates.end ()); - if (pointCandidates.size () > K) - pointCandidates.resize (K); + if (point_candidates.size () > K) + point_candidates.resize (K); - if (pointCandidates.size () == K) - smallestSquaredDist = pointCandidates.back ().pointDistance_; + if (point_candidates.size () == K) + smallest_squared_dist = point_candidates.back ().point_distance_; } // pop element from priority queue - searchEntryHeap.pop_back (); + search_heap.pop_back (); } - return (smallestSquaredDist); + return (smallest_squared_dist); } ////////////////////////////////////////////////////////////////////////////////////////////// template void pcl::octree::OctreePointCloudSearch::getNeighborsWithinRadiusRecursive ( const PointT & point, const double radiusSquared, const BranchNode* node, const OctreeKey& key, - unsigned int treeDepth, std::vector& k_indices, std::vector& k_sqr_distances, + unsigned int tree_depth, std::vector& k_indices, std::vector& k_sqr_distances, unsigned int max_nn) const { // child iterator - unsigned char childIdx; + unsigned char child_idx; // get spatial voxel information - double voxelSquaredDiameter = this->getVoxelSquaredDiameter (treeDepth); + double voxel_squared_diameter = this->getVoxelSquaredDiameter (tree_depth); // iterate over all children - for (childIdx = 0; childIdx < 8; childIdx++) + for (child_idx = 0; child_idx < 8; child_idx++) { - if (!this->branchHasChild (*node, childIdx)) + if (!this->branchHasChild (*node, child_idx)) continue; - const OctreeNode* childNode; - childNode = this->getBranchChildPtr (*node, childIdx); + const OctreeNode* child_node; + child_node = this->getBranchChildPtr (*node, child_idx); - OctreeKey newKey; - PointT voxelCenter; - float squaredDist; + OctreeKey new_key; + PointT voxel_center; + float squared_dist; // generate new key for current branch voxel - newKey.x = (key.x << 1) + (!!(childIdx & (1 << 2))); - newKey.y = (key.y << 1) + (!!(childIdx & (1 << 1))); - newKey.z = (key.z << 1) + (!!(childIdx & (1 << 0))); + new_key.x = (key.x << 1) + (!!(child_idx & (1 << 2))); + new_key.y = (key.y << 1) + (!!(child_idx & (1 << 1))); + new_key.z = (key.z << 1) + (!!(child_idx & (1 << 0))); // generate voxel center point for voxel at key - this->genVoxelCenterFromOctreeKey (newKey, treeDepth, voxelCenter); + this->genVoxelCenterFromOctreeKey (new_key, tree_depth, voxel_center); // calculate distance to search point - squaredDist = pointSquaredDist (static_cast (voxelCenter), point); + squared_dist = pointSquaredDist (static_cast (voxel_center), point); // if distance is smaller than search radius - if (squaredDist + this->epsilon_ - <= voxelSquaredDiameter / 4.0 + radiusSquared + sqrt (voxelSquaredDiameter * radiusSquared)) + if (squared_dist + this->epsilon_ + <= voxel_squared_diameter / 4.0 + radiusSquared + sqrt (voxel_squared_diameter * radiusSquared)) { - if (treeDepth < this->octreeDepth_) + if (tree_depth < this->octree_depth_) { // we have not reached maximum tree depth - getNeighborsWithinRadiusRecursive (point, radiusSquared, static_cast (childNode), newKey, treeDepth + 1, + getNeighborsWithinRadiusRecursive (point, radiusSquared, static_cast (child_node), new_key, tree_depth + 1, k_indices, k_sqr_distances, max_nn); if (max_nn != 0 && k_indices.size () == static_cast (max_nn)) return; @@ -375,27 +373,27 @@ pcl::octree::OctreePointCloudSearch::g // we reached leaf node level size_t i; - const LeafNode* childLeaf = static_cast (childNode); - vector decodedPointVector; + const LeafNode* child_leaf = static_cast (child_node); + vector decoded_point_vector; - // decode leaf node into decodedPointVector - (*childLeaf)->getPointIndices (decodedPointVector); + // decode leaf node into decoded_point_vector + (*child_leaf)->getPointIndices (decoded_point_vector); // Linearly iterate over all decoded (unsorted) points - for (i = 0; i < decodedPointVector.size (); i++) + for (i = 0; i < decoded_point_vector.size (); i++) { - const PointT& candidatePoint = this->getPointByIndex (decodedPointVector[i]); + const PointT& candidate_point = this->getPointByIndex (decoded_point_vector[i]); // calculate point distance to search point - squaredDist = pointSquaredDist (candidatePoint, point); + squared_dist = pointSquaredDist (candidate_point, point); // check if a match is found - if (squaredDist > radiusSquared) + if (squared_dist > radiusSquared) continue; // add point to result vector - k_indices.push_back (decodedPointVector[i]); - k_sqr_distances.push_back (squaredDist); + k_indices.push_back (decoded_point_vector[i]); + k_sqr_distances.push_back (squared_dist); if (max_nn != 0 && k_indices.size () == static_cast (max_nn)) return; @@ -410,103 +408,103 @@ template vo pcl::octree::OctreePointCloudSearch::approxNearestSearchRecursive (const PointT & point, const BranchNode* node, const OctreeKey& key, - unsigned int treeDepth, + unsigned int tree_depth, int& result_index, float& sqr_distance) { - unsigned char childIdx; - unsigned char minChildIdx; - double minVoxelCenterDistance; + unsigned char child_idx; + unsigned char min_child_idx; + double min_voxel_center_distance; OctreeKey minChildKey; - OctreeKey newKey; + OctreeKey new_key; - const OctreeNode* childNode; + const OctreeNode* child_node; // set minimum voxel distance to maximum value - minVoxelCenterDistance = numeric_limits::max (); + min_voxel_center_distance = numeric_limits::max (); - minChildIdx = 0xFF; + min_child_idx = 0xFF; // iterate over all children - for (childIdx = 0; childIdx < 8; childIdx++) + for (child_idx = 0; child_idx < 8; child_idx++) { - if (!this->branchHasChild (*node, childIdx)) + if (!this->branchHasChild (*node, child_idx)) continue; - PointT voxelCenter; + PointT voxel_center; double voxelPointDist; - newKey.x = (key.x << 1) + (!!(childIdx & (1 << 2))); - newKey.y = (key.y << 1) + (!!(childIdx & (1 << 1))); - newKey.z = (key.z << 1) + (!!(childIdx & (1 << 0))); + new_key.x = (key.x << 1) + (!!(child_idx & (1 << 2))); + new_key.y = (key.y << 1) + (!!(child_idx & (1 << 1))); + new_key.z = (key.z << 1) + (!!(child_idx & (1 << 0))); // generate voxel center point for voxel at key - this->genVoxelCenterFromOctreeKey (newKey, treeDepth, voxelCenter); + this->genVoxelCenterFromOctreeKey (new_key, tree_depth, voxel_center); - voxelPointDist = pointSquaredDist (voxelCenter, point); + voxelPointDist = pointSquaredDist (voxel_center, point); // search for child voxel with shortest distance to search point - if (voxelPointDist >= minVoxelCenterDistance) + if (voxelPointDist >= min_voxel_center_distance) continue; - minVoxelCenterDistance = voxelPointDist; - minChildIdx = childIdx; - minChildKey = newKey; + min_voxel_center_distance = voxelPointDist; + min_child_idx = child_idx; + minChildKey = new_key; } // make sure we found at least one branch child - assert(minChildIdx<8); + assert(min_child_idx<8); - childNode = this->getBranchChildPtr (*node, minChildIdx); + child_node = this->getBranchChildPtr (*node, min_child_idx); - if (treeDepth < this->octreeDepth_) + if (tree_depth < this->octree_depth_) { // we have not reached maximum tree depth - approxNearestSearchRecursive (point, static_cast (childNode), minChildKey, treeDepth + 1, result_index, + approxNearestSearchRecursive (point, static_cast (child_node), minChildKey, tree_depth + 1, result_index, sqr_distance); } else { // we reached leaf node level - double squaredDist; - double smallestSquaredDist; + double squared_dist; + double smallest_squared_dist; size_t i; - vector decodedPointVector; + vector decoded_point_vector; - const LeafNode* childLeaf = static_cast (childNode); + const LeafNode* child_leaf = static_cast (child_node); - smallestSquaredDist = numeric_limits::max (); + smallest_squared_dist = numeric_limits::max (); - // decode leaf node into decodedPointVector - (**childLeaf).getPointIndices (decodedPointVector); + // decode leaf node into decoded_point_vector + (**child_leaf).getPointIndices (decoded_point_vector); // Linearly iterate over all decoded (unsorted) points - for (i = 0; i < decodedPointVector.size (); i++) + for (i = 0; i < decoded_point_vector.size (); i++) { - const PointT& candidatePoint = this->getPointByIndex (decodedPointVector[i]); + const PointT& candidate_point = this->getPointByIndex (decoded_point_vector[i]); // calculate point distance to search point - squaredDist = pointSquaredDist (candidatePoint, point); + squared_dist = pointSquaredDist (candidate_point, point); // check if a closer match is found - if (squaredDist >= smallestSquaredDist) + if (squared_dist >= smallest_squared_dist) continue; - result_index = decodedPointVector[i]; - smallestSquaredDist = squaredDist; - sqr_distance = static_cast (squaredDist); + result_index = decoded_point_vector[i]; + smallest_squared_dist = squared_dist; + sqr_distance = static_cast (squared_dist); } } } ////////////////////////////////////////////////////////////////////////////////////////////// template float -pcl::octree::OctreePointCloudSearch::pointSquaredDist (const PointT & pointA, - const PointT & pointB) const +pcl::octree::OctreePointCloudSearch::pointSquaredDist (const PointT & point_a, + const PointT & point_b) const { - return (pointA.getVector3fMap () - pointB.getVector3fMap ()).squaredNorm (); + return (point_a.getVector3fMap () - point_b.getVector3fMap ()).squaredNorm (); } ////////////////////////////////////////////////////////////////////////////////////////////// @@ -515,71 +513,71 @@ pcl::octree::OctreePointCloudSearch::b const Eigen::Vector3f &max_pt, const BranchNode* node, const OctreeKey& key, - unsigned int treeDepth, + unsigned int tree_depth, std::vector& k_indices) const { // child iterator - unsigned char childIdx; + unsigned char child_idx; // iterate over all children - for (childIdx = 0; childIdx < 8; childIdx++) + for (child_idx = 0; child_idx < 8; child_idx++) { - const OctreeNode* childNode; - childNode = this->getBranchChildPtr (*node, childIdx); + const OctreeNode* child_node; + child_node = this->getBranchChildPtr (*node, child_idx); - if (!childNode) + if (!child_node) continue; - OctreeKey newKey; + OctreeKey new_key; // generate new key for current branch voxel - newKey.x = (key.x << 1) + (!!(childIdx & (1 << 2))); - newKey.y = (key.y << 1) + (!!(childIdx & (1 << 1))); - newKey.z = (key.z << 1) + (!!(childIdx & (1 << 0))); + new_key.x = (key.x << 1) + (!!(child_idx & (1 << 2))); + new_key.y = (key.y << 1) + (!!(child_idx & (1 << 1))); + new_key.z = (key.z << 1) + (!!(child_idx & (1 << 0))); // voxel corners - Eigen::Vector3f lowerVoxelCorner; - Eigen::Vector3f upperVoxelCorner; + Eigen::Vector3f lower_voxel_corner; + Eigen::Vector3f upper_voxel_corner; // get voxel coordinates - this->genVoxelBoundsFromOctreeKey (newKey, treeDepth, lowerVoxelCorner, upperVoxelCorner); + this->genVoxelBoundsFromOctreeKey (new_key, tree_depth, lower_voxel_corner, upper_voxel_corner); // test if search region overlap with voxel space - if ( !( (lowerVoxelCorner (0) > max_pt (0)) || (min_pt (0) > upperVoxelCorner(0)) || - (lowerVoxelCorner (1) > max_pt (1)) || (min_pt (1) > upperVoxelCorner(1)) || - (lowerVoxelCorner (2) > max_pt (2)) || (min_pt (2) > upperVoxelCorner(2)) ) ) + if ( !( (lower_voxel_corner (0) > max_pt (0)) || (min_pt (0) > upper_voxel_corner(0)) || + (lower_voxel_corner (1) > max_pt (1)) || (min_pt (1) > upper_voxel_corner(1)) || + (lower_voxel_corner (2) > max_pt (2)) || (min_pt (2) > upper_voxel_corner(2)) ) ) { - if (treeDepth < this->octreeDepth_) + if (tree_depth < this->octree_depth_) { // we have not reached maximum tree depth - boxSearchRecursive (min_pt, max_pt, static_cast (childNode), newKey, treeDepth + 1, k_indices); + boxSearchRecursive (min_pt, max_pt, static_cast (child_node), new_key, tree_depth + 1, k_indices); } else { // we reached leaf node level size_t i; - vector decodedPointVector; + vector decoded_point_vector; bool bInBox; - const LeafNode* childLeaf = static_cast (childNode); + const LeafNode* child_leaf = static_cast (child_node); - // decode leaf node into decodedPointVector - (**childLeaf).getPointIndices (decodedPointVector); + // decode leaf node into decoded_point_vector + (**child_leaf).getPointIndices (decoded_point_vector); // Linearly iterate over all decoded (unsorted) points - for (i = 0; i < decodedPointVector.size (); i++) + for (i = 0; i < decoded_point_vector.size (); i++) { - const PointT& candidatePoint = this->getPointByIndex (decodedPointVector[i]); + const PointT& candidate_point = this->getPointByIndex (decoded_point_vector[i]); // check if point falls within search box - bInBox = ( (candidatePoint.x > min_pt (0)) && (candidatePoint.x < max_pt (0)) && - (candidatePoint.y > min_pt (1)) && (candidatePoint.y < max_pt (1)) && - (candidatePoint.z > min_pt (2)) && (candidatePoint.z < max_pt (2)) ); + bInBox = ( (candidate_point.x > min_pt (0)) && (candidate_point.x < max_pt (0)) && + (candidate_point.y > min_pt (1)) && (candidate_point.y < max_pt (1)) && + (candidate_point.z > min_pt (2)) && (candidate_point.z < max_pt (2)) ); if (bInBox) // add to result vector - k_indices.push_back (decodedPointVector[i]); + k_indices.push_back (decoded_point_vector[i]); } } } @@ -589,24 +587,24 @@ pcl::octree::OctreePointCloudSearch::b ////////////////////////////////////////////////////////////////////////////////////////////// template int pcl::octree::OctreePointCloudSearch::getIntersectedVoxelCenters ( - Eigen::Vector3f origin, Eigen::Vector3f direction, AlignedPointTVector &voxelCenterList, - int maxVoxelCount) const + Eigen::Vector3f origin, Eigen::Vector3f direction, AlignedPointTVector &voxel_center_list, + int max_voxel_count) const { OctreeKey key; key.x = key.y = key.z = 0; - voxelCenterList.clear (); + voxel_center_list.clear (); - // Voxel childIdx remapping + // Voxel child_idx remapping unsigned char a = 0; - double minX, minY, minZ, maxX, maxY, maxZ; + double min_x, min_y, min_z, max_x, max_y, max_z; - initIntersectedVoxel (origin, direction, minX, minY, minZ, maxX, maxY, maxZ, a); + initIntersectedVoxel (origin, direction, min_x, min_y, min_z, max_x, max_y, max_z, a); - if (max (max (minX, minY), minZ) < min (min (maxX, maxY), maxZ)) - return getIntersectedVoxelCentersRecursive (minX, minY, minZ, maxX, maxY, maxZ, a, this->rootNode_, key, - voxelCenterList, maxVoxelCount); + if (max (max (min_x, min_y), min_z) < min (min (max_x, max_y), max_z)) + return getIntersectedVoxelCentersRecursive (min_x, min_y, min_z, max_x, max_y, max_z, a, this->root_node_, key, + voxel_center_list, max_voxel_count); return (0); } @@ -615,32 +613,32 @@ pcl::octree::OctreePointCloudSearch::g template int pcl::octree::OctreePointCloudSearch::getIntersectedVoxelIndices ( Eigen::Vector3f origin, Eigen::Vector3f direction, std::vector &k_indices, - int maxVoxelCount) const + int max_voxel_count) const { OctreeKey key; key.x = key.y = key.z = 0; k_indices.clear (); - // Voxel childIdx remapping + // Voxel child_idx remapping unsigned char a = 0; - double minX, minY, minZ, maxX, maxY, maxZ; + double min_x, min_y, min_z, max_x, max_y, max_z; - initIntersectedVoxel (origin, direction, minX, minY, minZ, maxX, maxY, maxZ, a); + initIntersectedVoxel (origin, direction, min_x, min_y, min_z, max_x, max_y, max_z, a); - if (max (max (minX, minY), minZ) < min (min (maxX, maxY), maxZ)) - return getIntersectedVoxelIndicesRecursive (minX, minY, minZ, maxX, maxY, maxZ, a, this->rootNode_, key, - k_indices, maxVoxelCount); + if (max (max (min_x, min_y), min_z) < min (min (max_x, max_y), max_z)) + return getIntersectedVoxelIndicesRecursive (min_x, min_y, min_z, max_x, max_y, max_z, a, this->root_node_, key, + k_indices, max_voxel_count); return (0); } ////////////////////////////////////////////////////////////////////////////////////////////// template int pcl::octree::OctreePointCloudSearch::getIntersectedVoxelCentersRecursive ( - double minX, double minY, double minZ, double maxX, double maxY, double maxZ, unsigned char a, - const OctreeNode* node, const OctreeKey& key, AlignedPointTVector &voxelCenterList, int maxVoxelCount) const + double min_x, double min_y, double min_z, double max_x, double max_y, double max_z, unsigned char a, + const OctreeNode* node, const OctreeKey& key, AlignedPointTVector &voxel_center_list, int max_voxel_count) const { - if (maxX < 0.0 || maxY < 0.0 || maxZ < 0.0) + if (max_x < 0.0 || max_y < 0.0 || max_z < 0.0) return (0); // If leaf node, get voxel center and increment intersection count @@ -650,115 +648,115 @@ pcl::octree::OctreePointCloudSearch::g this->genLeafNodeCenterFromOctreeKey (key, newPoint); - voxelCenterList.push_back (newPoint); + voxel_center_list.push_back (newPoint); return (1); } // Voxel intersection count for branches children - int voxelCount = 0; + int voxel_count = 0; // Voxel mid lines - double midX = 0.5 * (minX + maxX); - double midY = 0.5 * (minY + maxY); - double midZ = 0.5 * (minZ + maxZ); + double mid_x = 0.5 * (min_x + max_x); + double mid_y = 0.5 * (min_y + max_y); + double mid_z = 0.5 * (min_z + max_z); // First voxel node ray will intersect - int currNode = getFirstIntersectedNode (minX, minY, minZ, midX, midY, midZ); + int curr_node = getFirstIntersectedNode (min_x, min_y, min_z, mid_x, mid_y, mid_z); // Child index, node and key - unsigned char childIdx; - const OctreeNode *childNode; - OctreeKey childKey; + unsigned char child_idx; + const OctreeNode *child_node; + OctreeKey child_key; do { - if (currNode != 0) - childIdx = static_cast (currNode ^ a); + if (curr_node != 0) + child_idx = static_cast (curr_node ^ a); else - childIdx = a; + child_idx = a; - // childNode == 0 if childNode doesn't exist - childNode = this->getBranchChildPtr (static_cast (*node), childIdx); + // child_node == 0 if child_node doesn't exist + child_node = this->getBranchChildPtr (static_cast (*node), child_idx); // Generate new key for current branch voxel - childKey.x = (key.x << 1) | (!!(childIdx & (1 << 2))); - childKey.y = (key.y << 1) | (!!(childIdx & (1 << 1))); - childKey.z = (key.z << 1) | (!!(childIdx & (1 << 0))); + child_key.x = (key.x << 1) | (!!(child_idx & (1 << 2))); + child_key.y = (key.y << 1) | (!!(child_idx & (1 << 1))); + child_key.z = (key.z << 1) | (!!(child_idx & (1 << 0))); // Recursively call each intersected child node, selecting the next // node intersected by the ray. Children that do not intersect will // not be traversed. - switch (currNode) + switch (curr_node) { case 0: - if (childNode) - voxelCount += getIntersectedVoxelCentersRecursive (minX, minY, minZ, midX, midY, midZ, a, childNode, - childKey, voxelCenterList, maxVoxelCount); - currNode = getNextIntersectedNode (midX, midY, midZ, 4, 2, 1); + if (child_node) + voxel_count += getIntersectedVoxelCentersRecursive (min_x, min_y, min_z, mid_x, mid_y, mid_z, a, child_node, + child_key, voxel_center_list, max_voxel_count); + curr_node = getNextIntersectedNode (mid_x, mid_y, mid_z, 4, 2, 1); break; case 1: - if (childNode) - voxelCount += getIntersectedVoxelCentersRecursive (minX, minY, midZ, midX, midY, maxZ, a, childNode, - childKey, voxelCenterList, maxVoxelCount); - currNode = getNextIntersectedNode (midX, midY, maxZ, 5, 3, 8); + if (child_node) + voxel_count += getIntersectedVoxelCentersRecursive (min_x, min_y, mid_z, mid_x, mid_y, max_z, a, child_node, + child_key, voxel_center_list, max_voxel_count); + curr_node = getNextIntersectedNode (mid_x, mid_y, max_z, 5, 3, 8); break; case 2: - if (childNode) - voxelCount += getIntersectedVoxelCentersRecursive (minX, midY, minZ, midX, maxY, midZ, a, childNode, - childKey, voxelCenterList, maxVoxelCount); - currNode = getNextIntersectedNode (midX, maxY, midZ, 6, 8, 3); + if (child_node) + voxel_count += getIntersectedVoxelCentersRecursive (min_x, mid_y, min_z, mid_x, max_y, mid_z, a, child_node, + child_key, voxel_center_list, max_voxel_count); + curr_node = getNextIntersectedNode (mid_x, max_y, mid_z, 6, 8, 3); break; case 3: - if (childNode) - voxelCount += getIntersectedVoxelCentersRecursive (minX, midY, midZ, midX, maxY, maxZ, a, childNode, - childKey, voxelCenterList, maxVoxelCount); - currNode = getNextIntersectedNode (midX, maxY, maxZ, 7, 8, 8); + if (child_node) + voxel_count += getIntersectedVoxelCentersRecursive (min_x, mid_y, mid_z, mid_x, max_y, max_z, a, child_node, + child_key, voxel_center_list, max_voxel_count); + curr_node = getNextIntersectedNode (mid_x, max_y, max_z, 7, 8, 8); break; case 4: - if (childNode) - voxelCount += getIntersectedVoxelCentersRecursive (midX, minY, minZ, maxX, midY, midZ, a, childNode, - childKey, voxelCenterList, maxVoxelCount); - currNode = getNextIntersectedNode (maxX, midY, midZ, 8, 6, 5); + if (child_node) + voxel_count += getIntersectedVoxelCentersRecursive (mid_x, min_y, min_z, max_x, mid_y, mid_z, a, child_node, + child_key, voxel_center_list, max_voxel_count); + curr_node = getNextIntersectedNode (max_x, mid_y, mid_z, 8, 6, 5); break; case 5: - if (childNode) - voxelCount += getIntersectedVoxelCentersRecursive (midX, minY, midZ, maxX, midY, maxZ, a, childNode, - childKey, voxelCenterList, maxVoxelCount); - currNode = getNextIntersectedNode (maxX, midY, maxZ, 8, 7, 8); + if (child_node) + voxel_count += getIntersectedVoxelCentersRecursive (mid_x, min_y, mid_z, max_x, mid_y, max_z, a, child_node, + child_key, voxel_center_list, max_voxel_count); + curr_node = getNextIntersectedNode (max_x, mid_y, max_z, 8, 7, 8); break; case 6: - if (childNode) - voxelCount += getIntersectedVoxelCentersRecursive (midX, midY, minZ, maxX, maxY, midZ, a, childNode, - childKey, voxelCenterList, maxVoxelCount); - currNode = getNextIntersectedNode (maxX, maxY, midZ, 8, 8, 7); + if (child_node) + voxel_count += getIntersectedVoxelCentersRecursive (mid_x, mid_y, min_z, max_x, max_y, mid_z, a, child_node, + child_key, voxel_center_list, max_voxel_count); + curr_node = getNextIntersectedNode (max_x, max_y, mid_z, 8, 8, 7); break; case 7: - if (childNode) - voxelCount += getIntersectedVoxelCentersRecursive (midX, midY, midZ, maxX, maxY, maxZ, a, childNode, - childKey, voxelCenterList, maxVoxelCount); - currNode = 8; + if (child_node) + voxel_count += getIntersectedVoxelCentersRecursive (mid_x, mid_y, mid_z, max_x, max_y, max_z, a, child_node, + child_key, voxel_center_list, max_voxel_count); + curr_node = 8; break; } - } while ((currNode < 8) && (maxVoxelCount <= 0 || voxelCount < maxVoxelCount)); - return (voxelCount); + } while ((curr_node < 8) && (max_voxel_count <= 0 || voxel_count < max_voxel_count)); + return (voxel_count); } ////////////////////////////////////////////////////////////////////////////////////////////// template int pcl::octree::OctreePointCloudSearch::getIntersectedVoxelIndicesRecursive ( - double minX, double minY, double minZ, double maxX, double maxY, double maxZ, unsigned char a, - const OctreeNode* node, const OctreeKey& key, std::vector &k_indices, int maxVoxelCount) const + double min_x, double min_y, double min_z, double max_x, double max_y, double max_z, unsigned char a, + const OctreeNode* node, const OctreeKey& key, std::vector &k_indices, int max_voxel_count) const { - if (maxX < 0.0 || maxY < 0.0 || maxZ < 0.0) + if (max_x < 0.0 || max_y < 0.0 || max_z < 0.0) return (0); // If leaf node, get voxel center and increment intersection count @@ -773,98 +771,98 @@ pcl::octree::OctreePointCloudSearch::g } // Voxel intersection count for branches children - int voxelCount = 0; + int voxel_count = 0; // Voxel mid lines - double midX = 0.5 * (minX + maxX); - double midY = 0.5 * (minY + maxY); - double midZ = 0.5 * (minZ + maxZ); + double mid_x = 0.5 * (min_x + max_x); + double mid_y = 0.5 * (min_y + max_y); + double mid_z = 0.5 * (min_z + max_z); // First voxel node ray will intersect - int currNode = getFirstIntersectedNode (minX, minY, minZ, midX, midY, midZ); + int curr_node = getFirstIntersectedNode (min_x, min_y, min_z, mid_x, mid_y, mid_z); // Child index, node and key - unsigned char childIdx; - const OctreeNode *childNode; - OctreeKey childKey; + unsigned char child_idx; + const OctreeNode *child_node; + OctreeKey child_key; do { - if (currNode != 0) - childIdx = static_cast (currNode ^ a); + if (curr_node != 0) + child_idx = static_cast (curr_node ^ a); else - childIdx = a; + child_idx = a; - // childNode == 0 if childNode doesn't exist - childNode = this->getBranchChildPtr (static_cast (*node), childIdx); + // child_node == 0 if child_node doesn't exist + child_node = this->getBranchChildPtr (static_cast (*node), child_idx); // Generate new key for current branch voxel - childKey.x = (key.x << 1) | (!!(childIdx & (1 << 2))); - childKey.y = (key.y << 1) | (!!(childIdx & (1 << 1))); - childKey.z = (key.z << 1) | (!!(childIdx & (1 << 0))); + child_key.x = (key.x << 1) | (!!(child_idx & (1 << 2))); + child_key.y = (key.y << 1) | (!!(child_idx & (1 << 1))); + child_key.z = (key.z << 1) | (!!(child_idx & (1 << 0))); // Recursively call each intersected child node, selecting the next // node intersected by the ray. Children that do not intersect will // not be traversed. - switch (currNode) + switch (curr_node) { case 0: - if (childNode) - voxelCount += getIntersectedVoxelIndicesRecursive (minX, minY, minZ, midX, midY, midZ, a, childNode, - childKey, k_indices, maxVoxelCount); - currNode = getNextIntersectedNode (midX, midY, midZ, 4, 2, 1); + if (child_node) + voxel_count += getIntersectedVoxelIndicesRecursive (min_x, min_y, min_z, mid_x, mid_y, mid_z, a, child_node, + child_key, k_indices, max_voxel_count); + curr_node = getNextIntersectedNode (mid_x, mid_y, mid_z, 4, 2, 1); break; case 1: - if (childNode) - voxelCount += getIntersectedVoxelIndicesRecursive (minX, minY, midZ, midX, midY, maxZ, a, childNode, - childKey, k_indices, maxVoxelCount); - currNode = getNextIntersectedNode (midX, midY, maxZ, 5, 3, 8); + if (child_node) + voxel_count += getIntersectedVoxelIndicesRecursive (min_x, min_y, mid_z, mid_x, mid_y, max_z, a, child_node, + child_key, k_indices, max_voxel_count); + curr_node = getNextIntersectedNode (mid_x, mid_y, max_z, 5, 3, 8); break; case 2: - if (childNode) - voxelCount += getIntersectedVoxelIndicesRecursive (minX, midY, minZ, midX, maxY, midZ, a, childNode, - childKey, k_indices, maxVoxelCount); - currNode = getNextIntersectedNode (midX, maxY, midZ, 6, 8, 3); + if (child_node) + voxel_count += getIntersectedVoxelIndicesRecursive (min_x, mid_y, min_z, mid_x, max_y, mid_z, a, child_node, + child_key, k_indices, max_voxel_count); + curr_node = getNextIntersectedNode (mid_x, max_y, mid_z, 6, 8, 3); break; case 3: - if (childNode) - voxelCount += getIntersectedVoxelIndicesRecursive (minX, midY, midZ, midX, maxY, maxZ, a, childNode, - childKey, k_indices, maxVoxelCount); - currNode = getNextIntersectedNode (midX, maxY, maxZ, 7, 8, 8); + if (child_node) + voxel_count += getIntersectedVoxelIndicesRecursive (min_x, mid_y, mid_z, mid_x, max_y, max_z, a, child_node, + child_key, k_indices, max_voxel_count); + curr_node = getNextIntersectedNode (mid_x, max_y, max_z, 7, 8, 8); break; case 4: - if (childNode) - voxelCount += getIntersectedVoxelIndicesRecursive (midX, minY, minZ, maxX, midY, midZ, a, childNode, - childKey, k_indices, maxVoxelCount); - currNode = getNextIntersectedNode (maxX, midY, midZ, 8, 6, 5); + if (child_node) + voxel_count += getIntersectedVoxelIndicesRecursive (mid_x, min_y, min_z, max_x, mid_y, mid_z, a, child_node, + child_key, k_indices, max_voxel_count); + curr_node = getNextIntersectedNode (max_x, mid_y, mid_z, 8, 6, 5); break; case 5: - if (childNode) - voxelCount += getIntersectedVoxelIndicesRecursive (midX, minY, midZ, maxX, midY, maxZ, a, childNode, - childKey, k_indices, maxVoxelCount); - currNode = getNextIntersectedNode (maxX, midY, maxZ, 8, 7, 8); + if (child_node) + voxel_count += getIntersectedVoxelIndicesRecursive (mid_x, min_y, mid_z, max_x, mid_y, max_z, a, child_node, + child_key, k_indices, max_voxel_count); + curr_node = getNextIntersectedNode (max_x, mid_y, max_z, 8, 7, 8); break; case 6: - if (childNode) - voxelCount += getIntersectedVoxelIndicesRecursive (midX, midY, minZ, maxX, maxY, midZ, a, childNode, - childKey, k_indices, maxVoxelCount); - currNode = getNextIntersectedNode (maxX, maxY, midZ, 8, 8, 7); + if (child_node) + voxel_count += getIntersectedVoxelIndicesRecursive (mid_x, mid_y, min_z, max_x, max_y, mid_z, a, child_node, + child_key, k_indices, max_voxel_count); + curr_node = getNextIntersectedNode (max_x, max_y, mid_z, 8, 8, 7); break; case 7: - if (childNode) - voxelCount += getIntersectedVoxelIndicesRecursive (midX, midY, midZ, maxX, maxY, maxZ, a, childNode, - childKey, k_indices, maxVoxelCount); - currNode = 8; + if (child_node) + voxel_count += getIntersectedVoxelIndicesRecursive (mid_x, mid_y, mid_z, max_x, max_y, max_z, a, child_node, + child_key, k_indices, max_voxel_count); + curr_node = 8; break; } - } while ((currNode < 8) && (maxVoxelCount <= 0 || voxelCount < maxVoxelCount)); + } while ((curr_node < 8) && (max_voxel_count <= 0 || voxel_count < max_voxel_count)); - return (voxelCount); + return (voxel_count); } #endif // PCL_OCTREE_SEARCH_IMPL_H_ diff --git a/octree/include/pcl/octree/octree2buf_base.h b/octree/include/pcl/octree/octree2buf_base.h index 104373be4aa..6b8dab481f0 100644 --- a/octree/include/pcl/octree/octree2buf_base.h +++ b/octree/include/pcl/octree/octree2buf_base.h @@ -78,12 +78,12 @@ namespace pcl unsigned char i, b; - memset (childNodeArray_, 0, sizeof(childNodeArray_)); + memset (child_node_array_, 0, sizeof(child_node_array_)); for (b = 0; b < 2; ++b) - for (i = 0; i < 8; ++i) - if (source_arg.childNodeArray_[b][i]) - childNodeArray_[b][i] = source_arg.childNodeArray_[b][i]->deepCopy (); + for (i = 0; i < 8; ++i) + if (source_arg.child_node_array_[b][i]) + child_node_array_[b][i] = source_arg.child_node_array_[b][i]->deepCopy (); return (*this); @@ -110,7 +110,7 @@ namespace pcl getChildPtr (unsigned char buffer_arg, unsigned char index_arg) const { assert( (buffer_arg<2) && (index_arg<8)); - return childNodeArray_[buffer_arg][index_arg]; + return child_node_array_[buffer_arg][index_arg]; } /** \brief Set child pointer in current branch node @@ -122,7 +122,7 @@ namespace pcl OctreeNode* newNode_arg) { assert( (buffer_arg<2) && (index_arg<8)); - childNodeArray_[buffer_arg][index_arg] = newNode_arg; + child_node_array_[buffer_arg][index_arg] = newNode_arg; } /** \brief Check if branch is pointing to a particular child node @@ -133,7 +133,7 @@ namespace pcl inline bool hasChild (unsigned char buffer_arg, unsigned char index_arg) const { assert( (buffer_arg<2) && (index_arg<8)); - return (childNodeArray_[buffer_arg][index_arg] != 0); + return (child_node_array_[buffer_arg][index_arg] != 0); } /** \brief Get the type of octree node. Returns LEAVE_NODE type */ @@ -145,7 +145,7 @@ namespace pcl /** \brief Reset branch node container for every branch buffer. */ inline void reset () { - memset (&childNodeArray_[0][0], 0, sizeof(OctreeNode*) * 8 * 2); + memset (&child_node_array_[0][0], 0, sizeof(OctreeNode*) * 8 * 2); } /** \brief Get const pointer to container */ @@ -207,16 +207,15 @@ namespace pcl protected: ContainerT container_; - OctreeNode* childNodeArray_[2][8]; + OctreeNode* child_node_array_[2][8]; }; /** \brief @b Octree double buffer class * * \note This octree implementation keeps two separate octree structures - * in memory. This enables to create octree structures at high rate due to - * an advanced memory management. + * in memory. * - * \note Furthermore, it allows for detecting and differentially compare the adjacent octree structures. + * \note This allows for differentially compare the octree structures (change detection, differential encoding). * \note The tree depth defines the maximum amount of octree voxels / leaf nodes (should be initially defined). * \note All leaf nodes are addressed by integer indices. * \note Note: The tree depth equates to the bit length of the voxel indices. @@ -247,13 +246,13 @@ namespace pcl // Octree default iterators typedef OctreeDepthFirstIterator Iterator; typedef const OctreeDepthFirstIterator ConstIterator; - Iterator begin(unsigned int maxDepth_arg = 0) {return Iterator(this, maxDepth_arg);}; + Iterator begin(unsigned int max_depth_arg = 0) {return Iterator(this, max_depth_arg);}; const Iterator end() {return Iterator();}; // Octree leaf node iterators typedef OctreeLeafNodeIterator LeafNodeIterator; typedef const OctreeLeafNodeIterator ConstLeafNodeIterator; - LeafNodeIterator leaf_begin(unsigned int maxDepth_arg = 0) {return LeafNodeIterator(this, maxDepth_arg);}; + LeafNodeIterator leaf_begin(unsigned int max_depth_arg = 0) {return LeafNodeIterator(this, max_depth_arg);}; const LeafNodeIterator leaf_end() {return LeafNodeIterator();}; // Octree depth-first iterators @@ -265,7 +264,7 @@ namespace pcl // Octree breadth-first iterators typedef OctreeBreadthFirstIterator BreadthFirstIterator; typedef const OctreeBreadthFirstIterator ConstBreadthFirstIterator; - BreadthFirstIterator breadth_begin(unsigned int maxDepth_arg = 0) {return BreadthFirstIterator(this, maxDepth_arg);}; + BreadthFirstIterator breadth_begin(unsigned int max_depth_arg = 0) {return BreadthFirstIterator(this, max_depth_arg);}; const BreadthFirstIterator breadth_end() {return BreadthFirstIterator();}; /** \brief Empty constructor. */ @@ -277,10 +276,14 @@ namespace pcl /** \brief Copy constructor. */ Octree2BufBase (const Octree2BufBase& source) : - leafCount_ (source.leafCount_), branchCount_ (source.branchCount_), - rootNode_ (new (BranchNode) (*(source.rootNode_))), depthMask_ (source.depthMask_), - maxKey_ (source.maxKey_),bufferSelector_ (source.bufferSelector_), - treeDirtyFlag_ (source.treeDirtyFlag_), octreeDepth_ (source.octreeDepth_), + leaf_count_ (source.leaf_count_), + branch_count_ (source.branch_count_), + root_node_ (new (BranchNode) (*(source.root_node_))), + depth_mask_ (source.depth_mask_), + max_key_ (source.max_key_), + buffer_selector_ (source.buffer_selector_), + tree_dirty_flag_ (source.tree_dirty_flag_), + octree_depth_ (source.octree_depth_), dynamic_depth_enabled_(source.dynamic_depth_enabled_) { } @@ -289,23 +292,23 @@ namespace pcl inline Octree2BufBase& operator = (const Octree2BufBase& source) { - leafCount_ = source.leafCount_; - branchCount_ = source.branchCount_; - rootNode_ = new (BranchNode) (* (source.rootNode_)); - depthMask_ = source.depthMask_; - maxKey_ = source.maxKey_; - bufferSelector_ = source.bufferSelector_; - treeDirtyFlag_ = source.treeDirtyFlag_; - octreeDepth_ = source.octreeDepth_; + leaf_count_ = source.leaf_count_; + branch_count_ = source.branch_count_; + root_node_ = new (BranchNode) (* (source.root_node_)); + depth_mask_ = source.depth_mask_; + max_key_ = source.max_key_; + buffer_selector_ = source.buffer_selector_; + tree_dirty_flag_ = source.tree_dirty_flag_; + octree_depth_ = source.octree_depth_; dynamic_depth_enabled_ = source.dynamic_depth_enabled_; return (*this); } /** \brief Set the maximum amount of voxels per dimension. - * \param maxVoxelIndex_arg: maximum amount of voxels per dimension + * \param max_voxel_index_arg: maximum amount of voxels per dimension * */ void - setMaxVoxelIndex (unsigned int maxVoxelIndex_arg); + setMaxVoxelIndex (unsigned int max_voxel_index_arg); /** \brief Set the maximum depth of the octree. * \param depth_arg: maximum depth of octree @@ -318,54 +321,52 @@ namespace pcl * */ inline unsigned int getTreeDepth () const { - return this->octreeDepth_; + return this->octree_depth_; } - /** \brief Create new leaf node at (idxX, idxY, idxZ). + /** \brief Create new leaf node at (idx_x_arg, idx_y_arg, idx_z_arg). * \note If leaf node already exist, this method returns the existing node - * \param idxX_arg: index of leaf node in the X axis. - * \param idxY_arg: index of leaf node in the Y axis. - * \param idxZ_arg: index of leaf node in the Z axis. + * \param idx_x_arg: index of leaf node in the X axis. + * \param idx_y_arg: index of leaf node in the Y axis. + * \param idx_z_arg: index of leaf node in the Z axis. * \return pointer to new leaf node container. * */ LeafContainerT* - createLeaf (unsigned int idxX_arg, unsigned int idxY_arg, unsigned int idxZ_arg); + createLeaf (unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg); - /** \brief Find leaf node at (idxX, idxY, idxZ). + /** \brief Find leaf node at (idx_x_arg, idx_y_arg, idx_z_arg). * \note If leaf node already exist, this method returns the existing node - * \param idxX_arg: index of leaf node in the X axis. - * \param idxY_arg: index of leaf node in the Y axis. - * \param idxZ_arg: index of leaf node in the Z axis. + * \param idx_x_arg: index of leaf node in the X axis. + * \param idx_y_arg: index of leaf node in the Y axis. + * \param idx_z_arg: index of leaf node in the Z axis. * \return pointer to leaf node container if found, null pointer otherwise. * */ LeafContainerT* - findLeaf (unsigned int idxX_arg, unsigned int idxY_arg, unsigned int idxZ_arg); + findLeaf (unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg); - /** \brief Check for the existence of leaf node at (idxX, idxY, idxZ). - * \param idxX_arg: index of leaf node in the X axis. - * \param idxY_arg: index of leaf node in the Y axis. - * \param idxZ_arg: index of leaf node in the Z axis. + /** \brief Check for the existence of leaf node at (idx_x_arg, idx_y_arg, idx_z_arg). + * \param idx_x_arg: index of leaf node in the X axis. + * \param idx_y_arg: index of leaf node in the Y axis. + * \param idx_z_arg: index of leaf node in the Z axis. * \return "true" if leaf node search is successful, otherwise it returns "false". * */ bool - existLeaf (unsigned int idxX_arg, unsigned int idxY_arg, - unsigned int idxZ_arg) const; + existLeaf (unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg) const; - /** \brief Remove leaf node at (idxX_arg, idxY_arg, idxZ_arg). - * \param idxX_arg: index of leaf node in the X axis. - * \param idxY_arg: index of leaf node in the Y axis. - * \param idxZ_arg: index of leaf node in the Z axis. + /** \brief Remove leaf node at (idx_x_arg, idx_y_arg, idx_z_arg). + * \param idx_x_arg: index of leaf node in the X axis. + * \param idx_y_arg: index of leaf node in the Y axis. + * \param idx_z_arg: index of leaf node in the Z axis. * */ void - removeLeaf (unsigned int idxX_arg, unsigned int idxY_arg, - unsigned int idxZ_arg); + removeLeaf (unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg); /** \brief Return the amount of existing leafs in the octree. * \return amount of registered leaf nodes. * */ inline std::size_t getLeafCount () const { - return (leafCount_); + return (leaf_count_); } /** \brief Return the amount of existing branches in the octree. @@ -373,11 +374,10 @@ namespace pcl * */ inline std::size_t getBranchCount () const { - return (branchCount_); + return (branch_count_); } /** \brief Delete the octree structure and its leaf nodes. - * \param freeMemory_arg: if "true", allocated octree nodes are deleted, otherwise they are pushed to the octree node pool * */ void deleteTree (); @@ -385,15 +385,15 @@ namespace pcl /** \brief Delete octree structure of previous buffer. */ inline void deletePreviousBuffer () { - treeCleanUpRecursive (rootNode_); + treeCleanUpRecursive (root_node_); } /** \brief Delete the octree structure in the current buffer. */ inline void deleteCurrentBuffer () { - bufferSelector_ = !bufferSelector_; - treeCleanUpRecursive (rootNode_); - leafCount_ = 0; + buffer_selector_ = !buffer_selector_; + treeCleanUpRecursive (root_node_); + leaf_count_ = 0; } /** \brief Switch buffers and reset current octree structure. */ @@ -401,50 +401,52 @@ namespace pcl switchBuffers (); /** \brief Serialize octree into a binary output vector describing its branch node structure. - * \param binaryTreeOut_arg: reference to output vector for writing binary tree structure. - * \param doXOREncoding_arg: select if binary tree structure should be generated based on current octree (false) of based on a XOR comparison between current and previous octree + * \param binary_tree_out_arg: reference to output vector for writing binary tree structure. + * \param do_XOR_encoding_arg: select if binary tree structure should be generated based on current octree (false) of based on a XOR comparison between current and previous octree * */ void - serializeTree (std::vector& binaryTreeOut_arg, - bool doXOREncoding_arg = false); + serializeTree (std::vector& binary_tree_out_arg, + bool do_XOR_encoding_arg = false); /** \brief Serialize octree into a binary output vector describing its branch node structure and and push all DataT elements stored in the octree to a vector. - * \param binaryTreeOut_arg: reference to output vector for writing binary tree structure. - * \param dataVector_arg: pointer to all LeafContainerT objects in the octree - * \param doXOREncoding_arg: select if binary tree structure should be generated based on current octree (false) of based on a XOR comparison between current and previous octree + * \param binary_tree_out_arg: reference to output vector for writing binary tree structure. + * \param leaf_container_vector_arg: pointer to all LeafContainerT objects in the octree + * \param do_XOR_encoding_arg: select if binary tree structure should be generated based on current octree (false) of based on a XOR comparison between current and previous octree * */ void - serializeTree (std::vector& binaryTreeOut_arg, std::vector& dataVector_arg, - bool doXOREncoding_arg = false); + serializeTree (std::vector& binary_tree_out_arg, + std::vector& leaf_container_vector_arg, + bool do_XOR_encoding_arg = false); /** \brief Outputs a vector of all DataT elements that are stored within the octree leaf nodes. - * \param dataVector_arg: vector of pointers to all LeafContainerT objects in the octree + * \param leaf_container_vector_arg: vector of pointers to all LeafContainerT objects in the octree * */ void - serializeLeafs (std::vector& dataVector_arg); + serializeLeafs (std::vector& leaf_container_vector_arg); /** \brief Outputs a vector of all DataT elements from leaf nodes, that do not exist in the previous octree buffer. - * \param dataVector_arg: vector of pointers to all LeafContainerT objects in the octree + * \param leaf_container_vector_arg: vector of pointers to all LeafContainerT objects in the octree * */ void - serializeNewLeafs (std::vector& dataVector_arg); + serializeNewLeafs (std::vector& leaf_container_vector_arg); /** \brief Deserialize a binary octree description vector and create a corresponding octree structure. Leaf nodes are initialized with getDataTByKey(..). - * \param binaryTreeIn_arg: reference to input vector for reading binary tree structure. - * \param doXORDecoding_arg: select if binary tree structure is based on current octree (false) of based on a XOR comparison between current and previous octree + * \param binary_tree_in_arg: reference to input vector for reading binary tree structure. + * \param do_XOR_decoding_arg: select if binary tree structure is based on current octree (false) of based on a XOR comparison between current and previous octree * */ void - deserializeTree (std::vector& binaryTreeIn_arg, - bool doXORDecoding_arg = false); + deserializeTree (std::vector& binary_tree_in_arg, + bool do_XOR_decoding_arg = false); /** \brief Deserialize a binary octree description and create a corresponding octree structure. Leaf nodes are initialized with DataT elements from the dataVector. - * \param binaryTreeIn_arg: reference to inpvectoream for reading binary tree structure. - * \param dataVector_arg: vector of pointers to all LeafContainerT objects in the octree - * \param doXORDecoding_arg: select if binary tree structure is based on current octree (false) of based on a XOR comparison between current and previous octree + * \param binary_tree_in_arg: reference to inpvectoream for reading binary tree structure. + * \param leaf_container_vector_arg: vector of pointers to all LeafContainerT objects in the octree + * \param do_XOR_decoding_arg: select if binary tree structure is based on current octree (false) of based on a XOR comparison between current and previous octree * */ void - deserializeTree (std::vector& binaryTreeIn_arg, - std::vector& dataVector_arg, bool doXORDecoding_arg = false); + deserializeTree (std::vector& binary_tree_in_arg, + std::vector& leaf_container_vector_arg, + bool do_XOR_decoding_arg = false); protected: @@ -456,7 +458,7 @@ namespace pcl OctreeNode* getRootNode () const { - return (this->rootNode_); + return (this->root_node_); } /** \brief Find leaf node @@ -467,7 +469,7 @@ namespace pcl findLeaf (const OctreeKey& key_arg) const { LeafContainerT* result = 0; - findLeafRecursive (key_arg, depthMask_, rootNode_, result); + findLeafRecursive (key_arg, depth_mask_, root_node_, result); return result; } @@ -482,7 +484,7 @@ namespace pcl LeafNode* leaf_node; BranchNode* leaf_node_parent; - createLeafRecursive (key_arg, depthMask_ ,rootNode_, leaf_node, leaf_node_parent, false); + createLeafRecursive (key_arg, depth_mask_ ,root_node_, leaf_node, leaf_node_parent, false); LeafContainerT* ret = leaf_node->getContainerPtr(); @@ -503,12 +505,12 @@ namespace pcl * */ inline void removeLeaf (const OctreeKey& key_arg) { - if (key_arg <= maxKey_) + if (key_arg <= max_key_) { - deleteLeafRecursive (key_arg, depthMask_, rootNode_); + deleteLeafRecursive (key_arg, depth_mask_, root_node_); // we changed the octree structure -> dirty - treeDirtyFlag_ = true; + tree_dirty_flag_ = true; } } @@ -518,37 +520,37 @@ namespace pcl /** \brief Check if branch is pointing to a particular child node * \param branch_arg: reference to octree branch class - * \param childIdx_arg: index to child node + * \param child_idx_arg: index to child node * \return "true" if pointer to child node exists; "false" otherwise * */ inline bool - branchHasChild (const BranchNode& branch_arg, unsigned char childIdx_arg) const + branchHasChild (const BranchNode& branch_arg, unsigned char child_idx_arg) const { // test occupancyByte for child existence - return (branch_arg.getChildPtr(bufferSelector_, childIdx_arg) != 0); + return (branch_arg.getChildPtr(buffer_selector_, child_idx_arg) != 0); } - /** \brief Retrieve a child node pointer for child node at childIdx. + /** \brief Retrieve a child node pointer for child node at child_idx. * \param branch_arg: reference to octree branch class - * \param childIdx_arg: index to child node + * \param child_idx_arg: index to child node * \return pointer to octree child node class */ inline OctreeNode* getBranchChildPtr (const BranchNode& branch_arg, - unsigned char childIdx_arg) const + unsigned char child_idx_arg) const { - return branch_arg.getChildPtr(bufferSelector_, childIdx_arg); + return branch_arg.getChildPtr(buffer_selector_, child_idx_arg); } /** \brief Assign new child node to branch * \param branch_arg: reference to octree branch class - * \param childIdx_arg: index to child node - * \param newChild_arg: pointer to new child node + * \param child_idx_arg: index to child node + * \param new_child_arg: pointer to new child node * */ - inline void setBranchChildPtr (BranchNode& branch_arg, - unsigned char childIdx_arg, OctreeNode* newChild_arg) + inline void + setBranchChildPtr (BranchNode& branch_arg, unsigned char child_idx_arg, OctreeNode* new_child_arg) { - branch_arg.setChildPtr(bufferSelector_, childIdx_arg, newChild_arg); + branch_arg.setChildPtr (buffer_selector_, child_idx_arg, new_child_arg); } /** \brief Generate bit pattern reflecting the existence of child node pointers for current buffer @@ -558,17 +560,17 @@ namespace pcl inline char getBranchBitPattern (const BranchNode& branch_arg) const { unsigned char i; - char nodeBits; + char node_bits; // create bit pattern - nodeBits = 0; + node_bits = 0; for (i = 0; i < 8; i++) { - const OctreeNode* child = branch_arg.getChildPtr(bufferSelector_, i); - nodeBits |= static_cast ( (!!child) << i); + const OctreeNode* child = branch_arg.getChildPtr(buffer_selector_, i); + node_bits |= static_cast ( (!!child) << i); } - return (nodeBits); + return (node_bits); } /** \brief Generate bit pattern reflecting the existence of child node pointers in specific buffer @@ -580,17 +582,17 @@ namespace pcl unsigned char bufferSelector_arg) const { unsigned char i; - char nodeBits; + char node_bits; // create bit pattern - nodeBits = 0; + node_bits = 0; for (i = 0; i < 8; i++) { const OctreeNode* child = branch_arg.getChildPtr(bufferSelector_arg, i); - nodeBits |= static_cast ( (!!child) << i); + node_bits |= static_cast ( (!!child) << i); } - return (nodeBits); + return (node_bits); } /** \brief Generate XOR bit pattern reflecting differences between the two octree buffers @@ -601,21 +603,21 @@ namespace pcl const BranchNode& branch_arg) const { unsigned char i; - char nodeBits[2]; + char node_bits[2]; // create bit pattern for both buffers - nodeBits[0] = nodeBits[1] = 0; + node_bits[0] = node_bits[1] = 0; for (i = 0; i < 8; i++) { const OctreeNode* childA = branch_arg.getChildPtr(0, i); const OctreeNode* childB = branch_arg.getChildPtr(1, i); - nodeBits[0] |= static_cast ( (!!childA) << i); - nodeBits[1] |= static_cast ( (!!childB) << i); + node_bits[0] |= static_cast ( (!!childA) << i); + node_bits[1] |= static_cast ( (!!childB) << i); } - return nodeBits[0] ^ nodeBits[1]; + return node_bits[0] ^ node_bits[1]; } /** \brief Test if branch changed between previous and current buffer @@ -629,16 +631,16 @@ namespace pcl /** \brief Delete child node and all its subchilds from octree in specific buffer * \param branch_arg: reference to octree branch class - * \param bufferSelector_arg: buffer selector - * \param childIdx_arg: index to child node + * \param buffer_selector_arg: buffer selector + * \param child_idx_arg: index to child node * */ inline void deleteBranchChild (BranchNode& branch_arg, - unsigned char bufferSelector_arg, - unsigned char childIdx_arg) + unsigned char buffer_selector_arg, + unsigned char child_idx_arg) { - if (branch_arg.hasChild(bufferSelector_arg, childIdx_arg)) + if (branch_arg.hasChild(buffer_selector_arg, child_idx_arg)) { - OctreeNode* branchChild = branch_arg.getChildPtr(bufferSelector_arg, childIdx_arg); + OctreeNode* branchChild = branch_arg.getChildPtr(buffer_selector_arg, child_idx_arg); switch (branchChild->getNodeType ()) { @@ -663,17 +665,17 @@ namespace pcl } // set branch child pointer to 0 - branch_arg.setChildPtr(bufferSelector_arg, childIdx_arg, 0); + branch_arg.setChildPtr(buffer_selector_arg, child_idx_arg, 0); } } /** \brief Delete child node and all its subchilds from octree in current buffer * \param branch_arg: reference to octree branch class - * \param childIdx_arg: index to child node + * \param child_idx_arg: index to child node * */ - inline void deleteBranchChild (BranchNode& branch_arg, unsigned char childIdx_arg) + inline void deleteBranchChild (BranchNode& branch_arg, unsigned char child_idx_arg) { - deleteBranchChild(branch_arg, bufferSelector_, childIdx_arg); + deleteBranchChild(branch_arg, buffer_selector_, child_idx_arg); } /** \brief Delete branch and all its subchilds from octree (both buffers) @@ -706,33 +708,33 @@ namespace pcl /** \brief Fetch and add a new branch child to a branch class in current buffer * \param branch_arg: reference to octree branch class - * \param childIdx_arg: index to child node + * \param child_idx_arg: index to child node * \return pointer of new branch child to this reference * */ inline BranchNode* createBranchChild (BranchNode& branch_arg, - unsigned char childIdx_arg) + unsigned char child_idx_arg) { - BranchNode* newBranchChild = new BranchNode(); + BranchNode* new_branch_child = new BranchNode(); - branch_arg.setChildPtr (bufferSelector_, childIdx_arg, - static_cast (newBranchChild)); + branch_arg.setChildPtr (buffer_selector_, child_idx_arg, + static_cast (new_branch_child)); - return newBranchChild; + return new_branch_child; } /** \brief Fetch and add a new leaf child to a branch class * \param branch_arg: reference to octree branch class - * \param childIdx_arg: index to child node + * \param child_idx_arg: index to child node * \return pointer of new leaf child to this reference * */ - inline LeafNode* createLeafChild (BranchNode& branch_arg, - unsigned char childIdx_arg) + inline LeafNode* + createLeafChild (BranchNode& branch_arg, unsigned char child_idx_arg) { - LeafNode* newLeafChild = new LeafNode(); + LeafNode* new_leaf_child = new LeafNode(); - branch_arg.setChildPtr(bufferSelector_, childIdx_arg, newLeafChild); + branch_arg.setChildPtr(buffer_selector_, child_idx_arg, new_leaf_child); - return newLeafChild; + return new_leaf_child; } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -741,71 +743,84 @@ namespace pcl /** \brief Create a leaf node at octree key. If leaf node does already exist, it is returned. * \param key_arg: reference to an octree key - * \param depthMask_arg: depth mask used for octree key analysis and for branch depth indicator + * \param depth_mask_arg: depth mask used for octree key analysis and for branch depth indicator * \param branch_arg: current branch node - * \param returnLeaf_arg: return pointer to leaf container - * \param leafParent_arg: return pointer to parent of leaf node - * \param branchReset_arg: Reset pointer array of current branch - * \return depth mask at which leaf node was created + * \param return_leaf_arg: return pointer to leaf container + * \param parent_of_leaf_arg: return pointer to parent of leaf node + * \param branch_reset_arg: Reset pointer array of current branch + * \return depth mask at which leaf node was created/found **/ unsigned int - createLeafRecursive (const OctreeKey& key_arg, unsigned int depthMask_arg, BranchNode* branch_arg, - LeafNode*& returnLeaf_arg, BranchNode*& leafParent_arg, bool branchReset_arg = false); + createLeafRecursive (const OctreeKey& key_arg, + unsigned int depth_mask_arg, + BranchNode* branch_arg, + LeafNode*& return_leaf_arg, + BranchNode*& parent_of_leaf_arg, + bool branch_reset_arg = false); /** \brief Recursively search for a given leaf node and return a pointer. * \note If leaf node does not exist, a 0 pointer is returned. * \param key_arg: reference to an octree key - * \param depthMask_arg: depth mask used for octree key analysis and for branch depth indicator + * \param depth_mask_arg: depth mask used for octree key analysis and for branch depth indicator * \param branch_arg: current branch node * \param result_arg: pointer to leaf container class **/ void - findLeafRecursive (const OctreeKey& key_arg, unsigned int depthMask_arg, BranchNode* branch_arg, LeafContainerT*& result_arg) const; + findLeafRecursive (const OctreeKey& key_arg, + unsigned int depth_mask_arg, + BranchNode* branch_arg, + LeafContainerT*& result_arg) const; /** \brief Recursively search and delete leaf node * \param key_arg: reference to an octree key - * \param depthMask_arg: depth mask used for octree key analysis and branch depth indicator + * \param depth_mask_arg: depth mask used for octree key analysis and branch depth indicator * \param branch_arg: current branch node * \return "true" if branch does not contain any childs; "false" otherwise. This indicates if current branch can be deleted. **/ bool deleteLeafRecursive (const OctreeKey& key_arg, - unsigned int depthMask_arg, BranchNode* branch_arg); + unsigned int depth_mask_arg, + BranchNode* branch_arg); /** \brief Recursively explore the octree and output binary octree description together with a vector of leaf node DataT content. - * \param binaryTreeOut_arg: binary output vector * \param branch_arg: current branch node * \param key_arg: reference to an octree key - * \param dataVector_arg: vector to return pointers to all leaf container in the tree. - * \param doXOREncoding_arg: select if binary tree structure should be generated based on current octree (false) of based on a XOR comparison between current and previous octree - * \param newLeafsFilter_arg: execute callback only for leaf nodes that did not exist in preceding buffer + * \param binary_tree_out_arg: binary output vector + * \param leaf_container_vector_arg: vector to return pointers to all leaf container in the tree. + * \param do_XOR_encoding_arg: select if binary tree structure should be generated based on current octree (false) of based on a XOR comparison between current and previous octree + * \param new_leafs_filter_arg: execute callback only for leaf nodes that did not exist in preceding buffer **/ void - serializeTreeRecursive (BranchNode* branch_arg, OctreeKey& key_arg, - std::vector* binaryTreeOut_arg, - typename std::vector* dataVector_arg, bool doXOREncoding_arg = false, - bool newLeafsFilter_arg = false); + serializeTreeRecursive (BranchNode* branch_arg, + OctreeKey& key_arg, + std::vector* binary_tree_out_arg, + typename std::vector* leaf_container_vector_arg, + bool do_XOR_encoding_arg = false, + bool new_leafs_filter_arg = false); /** \brief Rebuild an octree based on binary XOR octree description and DataT objects for leaf node initialization. - * \param binaryTreeIn_arg: iterator to input vector * \param branch_arg: current branch node - * \param depthMask_arg: depth mask used for octree key analysis and branch depth indicator + * \param depth_mask_arg: depth mask used for octree key analysis and branch depth indicator * \param key_arg: reference to an octree key - * \param dataVectorIterator_arg: iterator pointing to leaf containter pointers to be added to a leaf node - * \param dataVectorEndIterator_arg: iterator pointing to leaf containter pointers pointing to last object in input container. - * \param branchReset_arg: Reset pointer array of current branch - * \param doXORDecoding_arg: select if binary tree structure is based on current octree (false) of based on a XOR comparison between current and previous octree + * \param binary_tree_in_it_arg iterator of binary input data + * \param leaf_container_vector__it_end_arg end iterator of binary input data + * \param leaf_container_vector_it_arg: iterator pointing to leaf containter pointers to be added to a leaf node + * \param leaf_container_vector_it_end_arg: iterator pointing to leaf containter pointers pointing to last object in input container. + * \param branch_reset_arg: Reset pointer array of current branch + * \param do_XOR_decoding_arg: select if binary tree structure is based on current octree (false) of based on a XOR comparison between current and previous octree **/ void deserializeTreeRecursive (BranchNode* branch_arg, - unsigned int depthMask_arg, OctreeKey& key_arg, - typename std::vector::const_iterator& binaryTreeIT_arg, - typename std::vector::const_iterator& binaryTreeIT_End_arg, - typename std::vector::const_iterator* dataVectorIterator_arg, - typename std::vector::const_iterator* dataVectorEndIterator_arg, - bool branchReset_arg = false, bool doXORDecoding_arg = false); + unsigned int depth_mask_arg, + OctreeKey& key_arg, + typename std::vector::const_iterator& binary_tree_in_it_arg, + typename std::vector::const_iterator& binary_tree_in_it_end_arg, + typename std::vector::const_iterator* leaf_container_vector_it_arg, + typename std::vector::const_iterator* leaf_container_vector_it_end_arg, + bool branch_reset_arg = false, + bool do_XOR_decoding_arg = false); ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -874,28 +889,28 @@ namespace pcl ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** \brief Amount of leaf nodes **/ - std::size_t leafCount_; + std::size_t leaf_count_; /** \brief Amount of branch nodes **/ - std::size_t branchCount_; + std::size_t branch_count_; /** \brief Pointer to root branch node of octree **/ - BranchNode* rootNode_; + BranchNode* root_node_; /** \brief Depth mask based on octree depth **/ - unsigned int depthMask_; + unsigned int depth_mask_; /** \brief key range */ - OctreeKey maxKey_; + OctreeKey max_key_; /** \brief Currently active octree buffer **/ - unsigned char bufferSelector_; + unsigned char buffer_selector_; // flags indicating if unused branches and leafs might exist in previous buffer - bool treeDirtyFlag_; + bool tree_dirty_flag_; /** \brief Octree depth */ - unsigned int octreeDepth_; + unsigned int octree_depth_; /** \brief Enable dynamic_depth * \note Note that this parameter is ignored in octree2buf! */ diff --git a/octree/include/pcl/octree/octree_base.h b/octree/include/pcl/octree/octree_base.h index 00d7b0ada39..21c3d163d2f 100644 --- a/octree/include/pcl/octree/octree_base.h +++ b/octree/include/pcl/octree/octree_base.h @@ -47,9 +47,6 @@ #include "octree_key.h" #include "octree_iterator.h" -// maximum depth of octree as we are using "unsigned int" octree keys / bit masks -#define OCT_MAXTREEDEPTH ( sizeof(size_t) * 8 ) - namespace pcl { namespace octree @@ -85,25 +82,25 @@ namespace pcl // Octree default iterators typedef OctreeDepthFirstIterator Iterator; typedef const OctreeDepthFirstIterator ConstIterator; - Iterator begin(unsigned int maxDepth_arg = 0) {return Iterator(this, maxDepth_arg);}; + Iterator begin(unsigned int max_depth_arg = 0) {return Iterator(this, max_depth_arg);}; const Iterator end() {return Iterator();}; // Octree leaf node iterators typedef OctreeLeafNodeIterator LeafNodeIterator; typedef const OctreeLeafNodeIterator ConstLeafNodeIterator; - LeafNodeIterator leaf_begin(unsigned int maxDepth_arg = 0) {return LeafNodeIterator(this, maxDepth_arg);}; + LeafNodeIterator leaf_begin(unsigned int max_depth_arg = 0) {return LeafNodeIterator(this, max_depth_arg);}; const LeafNodeIterator leaf_end() {return LeafNodeIterator();}; // Octree depth-first iterators typedef OctreeDepthFirstIterator DepthFirstIterator; typedef const OctreeDepthFirstIterator ConstDepthFirstIterator; - DepthFirstIterator depth_begin(unsigned int maxDepth_arg = 0) {return DepthFirstIterator(this, maxDepth_arg);}; + DepthFirstIterator depth_begin(unsigned int max_depth_arg = 0) {return DepthFirstIterator(this, max_depth_arg);}; const DepthFirstIterator depth_end() {return DepthFirstIterator();}; // Octree breadth-first iterators typedef OctreeBreadthFirstIterator BreadthFirstIterator; typedef const OctreeBreadthFirstIterator ConstBreadthFirstIterator; - BreadthFirstIterator breadth_begin(unsigned int maxDepth_arg = 0) {return BreadthFirstIterator(this, maxDepth_arg);}; + BreadthFirstIterator breadth_begin(unsigned int max_depth_arg = 0) {return BreadthFirstIterator(this, max_depth_arg);}; const BreadthFirstIterator breadth_end() {return BreadthFirstIterator();}; @@ -116,13 +113,13 @@ namespace pcl /** \brief Copy constructor. */ OctreeBase (const OctreeBase& source) : - leafCount_ (source.leafCount_), - branchCount_ (source.branchCount_), - rootNode_ (new (BranchNode) (*(source.rootNode_))), - depthMask_ (source.depthMask_), - octreeDepth_ (source.octreeDepth_), + leaf_count_ (source.leaf_count_), + branch_count_ (source.branch_count_), + root_node_ (new (BranchNode) (*(source.root_node_))), + depth_mask_ (source.depth_mask_), + octree_depth_ (source.octree_depth_), dynamic_depth_enabled_(source.dynamic_depth_enabled_), - maxKey_ (source.maxKey_) + max_key_ (source.max_key_) { } @@ -130,26 +127,26 @@ namespace pcl OctreeBase& operator = (const OctreeBase &source) { - leafCount_ = source.leafCount_; - branchCount_ = source.branchCount_; - rootNode_ = new (BranchNode) (*(source.rootNode_)); - depthMask_ = source.depthMask_; - maxKey_ = source.maxKey_; - octreeDepth_ = source.octreeDepth_; + leaf_count_ = source.leaf_count_; + branch_count_ = source.branch_count_; + root_node_ = new (BranchNode) (*(source.root_node_)); + depth_mask_ = source.depth_mask_; + max_key_ = source.max_key_; + octree_depth_ = source.octree_depth_; return (*this); } /** \brief Set the maximum amount of voxels per dimension. - * \param[in] maxVoxelIndex_arg maximum amount of voxels per dimension + * \param[in] max_voxel_index_arg maximum amount of voxels per dimension */ void - setMaxVoxelIndex (unsigned int maxVoxelIndex_arg); + setMaxVoxelIndex (unsigned int max_voxel_index_arg); /** \brief Set the maximum depth of the octree. - * \param depth_arg: maximum depth of octree + * \param max_depth_arg: maximum depth of octree * */ void - setTreeDepth (unsigned int depth_arg); + setTreeDepth (unsigned int max_depth_arg); /** \brief Get the maximum depth of the octree. * \return depth_arg: maximum depth of octree @@ -157,45 +154,45 @@ namespace pcl unsigned int getTreeDepth () const { - return this->octreeDepth_; + return this->octree_depth_; } - /** \brief Create new leaf node at (idxX, idxY, idxZ). + /** \brief Create new leaf node at (idx_x_arg, idx_y_arg, idx_z_arg). * \note If leaf node already exist, this method returns the existing node - * \param idxX_arg: index of leaf node in the X axis. - * \param idxY_arg: index of leaf node in the Y axis. - * \param idxZ_arg: index of leaf node in the Z axis. + * \param idx_x_arg: index of leaf node in the X axis. + * \param idx_y_arg: index of leaf node in the Y axis. + * \param idx_z_arg: index of leaf node in the Z axis. * \return pointer to new leaf node container. * */ LeafContainerT* - createLeaf (unsigned int idxX_arg, unsigned int idxY_arg, unsigned int idxZ_arg); + createLeaf (unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg); - /** \brief Find leaf node at (idxX, idxY, idxZ). + /** \brief Find leaf node at (idx_x_arg, idx_y_arg, idx_z_arg). * \note If leaf node already exist, this method returns the existing node - * \param idxX_arg: index of leaf node in the X axis. - * \param idxY_arg: index of leaf node in the Y axis. - * \param idxZ_arg: index of leaf node in the Z axis. + * \param idx_x_arg: index of leaf node in the X axis. + * \param idx_y_arg: index of leaf node in the Y axis. + * \param idx_z_arg: index of leaf node in the Z axis. * \return pointer to leaf node container if found, null pointer otherwise. * */ LeafContainerT* - findLeaf (unsigned int idxX_arg, unsigned int idxY_arg, unsigned int idxZ_arg); + findLeaf (unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg); - /** \brief Check for the existence of leaf node at (idxX, idxY, idxZ). - * \param idxX_arg: index of leaf node in the X axis. - * \param idxY_arg: index of leaf node in the Y axis. - * \param idxZ_arg: index of leaf node in the Z axis. + /** \brief idx_x_arg for the existence of leaf node at (idx_x_arg, idx_y_arg, idx_z_arg). + * \param idx_x_arg: index of leaf node in the X axis. + * \param idx_y_arg: index of leaf node in the Y axis. + * \param idx_z_arg: index of leaf node in the Z axis. * \return "true" if leaf node search is successful, otherwise it returns "false". * */ bool - existLeaf (unsigned int idxX_arg, unsigned int idxY_arg, unsigned int idxZ_arg) const ; + existLeaf (unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg) const ; - /** \brief Remove leaf node at (idxX_arg, idxY_arg, idxZ_arg). - * \param idxX_arg: index of leaf node in the X axis. - * \param idxY_arg: index of leaf node in the Y axis. - * \param idxZ_arg: index of leaf node in the Z axis. + /** \brief Remove leaf node at (idx_x_arg, idx_y_arg, idx_z_arg). + * \param idx_x_arg: index of leaf node in the X axis. + * \param idx_y_arg: index of leaf node in the Y axis. + * \param idx_z_arg: index of leaf node in the Z axis. * */ void - removeLeaf (unsigned int idxX_arg, unsigned int idxY_arg, unsigned int idxZ_arg); + removeLeaf (unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg); /** \brief Return the amount of existing leafs in the octree. * \return amount of registered leaf nodes. @@ -203,16 +200,16 @@ namespace pcl std::size_t getLeafCount () const { - return leafCount_; + return leaf_count_; } - /** \brief Return the amount of existing branches in the octree. + /** \brief Return the amount of existing branch nodes in the octree. * \return amount of branch nodes. * */ std::size_t getBranchCount () const { - return branchCount_; + return branch_count_; } /** \brief Delete the octree structure and its leaf nodes. @@ -221,36 +218,36 @@ namespace pcl deleteTree ( ); /** \brief Serialize octree into a binary output vector describing its branch node structure. - * \param binaryTreeOut_arg: reference to output vector for writing binary tree structure. + * \param binary_tree_out_arg: reference to output vector for writing binary tree structure. * */ void - serializeTree (std::vector& binaryTreeOut_arg); + serializeTree (std::vector& binary_tree_out_arg); /** \brief Serialize octree into a binary output vector describing its branch node structure and push all LeafContainerT elements stored in the octree to a vector. - * \param binaryTreeOut_arg: reference to output vector for writing binary tree structure. - * \param dataVector_arg: pointer to all LeafContainerT objects in the octree + * \param binary_tree_out_arg: reference to output vector for writing binary tree structure. + * \param leaf_container_vector_arg: pointer to all LeafContainerT objects in the octree * */ void - serializeTree (std::vector& binaryTreeOut_arg, std::vector& dataVector_arg); + serializeTree (std::vector& binary_tree_out_arg, std::vector& leaf_container_vector_arg); /** \brief Outputs a vector of all LeafContainerT elements that are stored within the octree leaf nodes. - * \param dataVector_arg: pointers to LeafContainerT vector that receives a copy of all LeafContainerT objects in the octree. + * \param leaf_container_vector_arg: pointers to LeafContainerT vector that receives a copy of all LeafContainerT objects in the octree. * */ void - serializeLeafs (std::vector& dataVector_arg); + serializeLeafs (std::vector& leaf_container_vector_arg); /** \brief Deserialize a binary octree description vector and create a corresponding octree structure. Leaf nodes are initialized with getDataTByKey(..). - * \param binaryTreeIn_arg: reference to input vector for reading binary tree structure. + * \param binary_tree_input_arg: reference to input vector for reading binary tree structure. * */ void - deserializeTree (std::vector& binaryTreeIn_arg); + deserializeTree (std::vector& binary_tree_input_arg); /** \brief Deserialize a binary octree description and create a corresponding octree structure. Leaf nodes are initialized with LeafContainerT elements from the dataVector. - * \param binaryTreeIn_arg: reference to input vector for reading binary tree structure. - * \param dataVector_arg: pointer to container vector. + * \param binary_tree_input_arg: reference to input vector for reading binary tree structure. + * \param leaf_container_vector_arg: pointer to container vector. * */ void - deserializeTree (std::vector& binaryTreeIn_arg, std::vector& dataVector_arg); + deserializeTree (std::vector& binary_tree_input_arg, std::vector& leaf_container_vector_arg); protected: @@ -269,7 +266,7 @@ namespace pcl LeafNode* leaf_node; BranchNode* leaf_node_parent; - createLeafRecursive (key_arg, depthMask_ ,rootNode_, leaf_node, leaf_node_parent); + createLeafRecursive (key_arg, depth_mask_ ,root_node_, leaf_node, leaf_node_parent); LeafContainerT* ret = leaf_node->getContainerPtr(); @@ -284,7 +281,7 @@ namespace pcl findLeaf (const OctreeKey& key_arg) const { LeafContainerT* result = 0; - findLeafRecursive (key_arg, depthMask_, rootNode_, result); + findLeafRecursive (key_arg, depth_mask_, root_node_, result); return result; } @@ -304,54 +301,56 @@ namespace pcl void removeLeaf (const OctreeKey& key_arg) { - if (key_arg <= maxKey_) - deleteLeafRecursive (key_arg, depthMask_, rootNode_); + if (key_arg <= max_key_) + deleteLeafRecursive (key_arg, depth_mask_, root_node_); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - // Branch node accessor functions + // Branch node access functions ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** \brief Retrieve root node */ OctreeNode* getRootNode () const { - return this->rootNode_; + return this->root_node_; } /** \brief Check if branch is pointing to a particular child node * \param branch_arg: reference to octree branch class - * \param childIdx_arg: index to child node + * \param child_idx_arg: index to child node * \return "true" if pointer to child node exists; "false" otherwise * */ bool - branchHasChild (const BranchNode& branch_arg, unsigned char childIdx_arg) const + branchHasChild (const BranchNode& branch_arg, + unsigned char child_idx_arg) const { // test occupancyByte for child existence - return (branch_arg.getChildPtr(childIdx_arg) != 0); + return (branch_arg.getChildPtr(child_idx_arg) != 0); } - /** \brief Retrieve a child node pointer for child node at childIdx. + /** \brief Retrieve a child node pointer for child node at child_idx. * \param branch_arg: reference to octree branch class - * \param childIdx_arg: index to child node + * \param child_idx_arg: index to child node * \return pointer to octree child node class */ OctreeNode* getBranchChildPtr (const BranchNode& branch_arg, - unsigned char childIdx_arg) const + unsigned char child_idx_arg) const { - return branch_arg.getChildPtr(childIdx_arg); + return branch_arg.getChildPtr(child_idx_arg); } /** \brief Assign new child node to branch * \param branch_arg: reference to octree branch class - * \param childIdx_arg: index to child node - * \param newChild_arg: pointer to new child node + * \param child_idx_arg: index to child node + * \param new_child_arg: pointer to new child node * */ void setBranchChildPtr (BranchNode& branch_arg, - unsigned char childIdx_arg, OctreeNode* newChild_arg) + unsigned char child_idx_arg, + OctreeNode* new_child_arg) { - branch_arg[childIdx_arg] = newChild_arg; + branch_arg[child_idx_arg] = new_child_arg; } /** \brief Generate bit pattern reflecting the existence of child node pointers @@ -362,44 +361,44 @@ namespace pcl getBranchBitPattern (const BranchNode& branch_arg) const { unsigned char i; - char nodeBits; + char node_bits; // create bit pattern - nodeBits = 0; + node_bits = 0; for (i = 0; i < 8; i++) { const OctreeNode* child = branch_arg.getChildPtr(i); - nodeBits |= static_cast ((!!child) << i); + node_bits |= static_cast ((!!child) << i); } - return (nodeBits); + return (node_bits); } /** \brief Delete child node and all its subchilds from octree * \param branch_arg: reference to octree branch class - * \param childIdx_arg: index to child node + * \param child_idx_arg: index to child node * */ void - deleteBranchChild (BranchNode& branch_arg, unsigned char childIdx_arg) + deleteBranchChild (BranchNode& branch_arg, unsigned char child_idx_arg) { - if (branch_arg.hasChild(childIdx_arg)) + if (branch_arg.hasChild(child_idx_arg)) { - OctreeNode* branchChild = branch_arg[childIdx_arg]; + OctreeNode* branch_child = branch_arg[child_idx_arg]; - switch (branchChild->getNodeType ()) + switch (branch_child->getNodeType ()) { case BRANCH_NODE: { // free child branch recursively - deleteBranch (*static_cast (branchChild)); + deleteBranch (*static_cast (branch_child)); // delete branch node - delete branchChild; + delete branch_child; } break; case LEAF_NODE: { // delete leaf node - delete branchChild; + delete branch_child; break; } default: @@ -407,7 +406,7 @@ namespace pcl } // set branch child pointer to 0 - branch_arg[childIdx_arg] = 0; + branch_arg[child_idx_arg] = 0; } } @@ -426,32 +425,30 @@ namespace pcl /** \brief Create and add a new branch child to a branch class * \param branch_arg: reference to octree branch class - * \param childIdx_arg: index to child node - * \return newBranchChild_arg: pointer of new branch child to this reference + * \param child_idx_arg: index to child node + * \return pointer of new branch child to this reference * */ BranchNode* createBranchChild (BranchNode& branch_arg, - unsigned char childIdx_arg) + unsigned char child_idx_arg) { - BranchNode* newBranchChild = new BranchNode(); - branch_arg[childIdx_arg] = - static_cast (newBranchChild); + BranchNode* new_branch_child = new BranchNode(); + branch_arg[child_idx_arg] = static_cast (new_branch_child); - return newBranchChild; + return new_branch_child; } /** \brief Create and add a new leaf child to a branch class * \param branch_arg: reference to octree branch class - * \param childIdx_arg: index to child node - * \return newLeafChild_arg: pointer of new leaf child to this reference + * \param child_idx_arg: index to child node + * \return pointer of new leaf child to this reference * */ LeafNode* - createLeafChild (BranchNode& branch_arg, unsigned char childIdx_arg) + createLeafChild (BranchNode& branch_arg, unsigned char child_idx_arg) { - LeafNode* newLeafChild = new LeafNode(); - - branch_arg[childIdx_arg] = static_cast (newLeafChild); + LeafNode* new_leaf_child = new LeafNode(); + branch_arg[child_idx_arg] = static_cast (new_leaf_child); - return newLeafChild; + return new_leaf_child; } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -460,62 +457,70 @@ namespace pcl /** \brief Create a leaf node at octree key. If leaf node does already exist, it is returned. * \param key_arg: reference to an octree key - * \param depthMask_arg: depth mask used for octree key analysis and for branch depth indicator - * \param data_arg data to be added + * \param depth_mask_arg: depth mask used for octree key analysis and for branch depth indicator * \param branch_arg: current branch node - * \param returnLeaf_arg: return pointer to leaf node - * \param leafParent_arg: return pointer to parent of leaf node + * \param return_leaf_arg: return pointer to leaf node + * \param parent_of_leaf_arg: return pointer to parent of leaf node * \return depth mask at which leaf node was created **/ unsigned int - createLeafRecursive (const OctreeKey& key_arg, unsigned int depthMask_arg, BranchNode* branch_arg, LeafNode*& returnLeaf_arg, BranchNode*& leafParent_arg); + createLeafRecursive (const OctreeKey& key_arg, + unsigned int depth_mask_arg, + BranchNode* branch_arg, + LeafNode*& return_leaf_arg, + BranchNode*& parent_of_leaf_arg); /** \brief Recursively search for a given leaf node and return a pointer. * \note If leaf node does not exist, a 0 pointer is returned. * \param key_arg: reference to an octree key - * \param depthMask_arg: depth mask used for octree key analysis and for branch depth indicator + * \param depth_mask_arg: depth mask used for octree key analysis and for branch depth indicator * \param branch_arg: current branch node * \param result_arg: pointer to leaf node class **/ void - findLeafRecursive (const OctreeKey& key_arg, unsigned int depthMask_arg, BranchNode* branch_arg, LeafContainerT*& result_arg) const; + findLeafRecursive (const OctreeKey& key_arg, + unsigned int depth_mask_arg, + BranchNode* branch_arg, + LeafContainerT*& result_arg) const; /** \brief Recursively search and delete leaf node * \param key_arg: reference to an octree key - * \param depthMask_arg: depth mask used for octree key analysis and branch depth indicator + * \param depth_mask_arg: depth mask used for octree key analysis and branch depth indicator * \param branch_arg: current branch node * \return "true" if branch does not contain any childs; "false" otherwise. This indicates if current branch can be deleted, too. **/ bool - deleteLeafRecursive (const OctreeKey& key_arg, unsigned int depthMask_arg, BranchNode* branch_arg); + deleteLeafRecursive (const OctreeKey& key_arg, + unsigned int depth_mask_arg, + BranchNode* branch_arg); /** \brief Recursively explore the octree and output binary octree description together with a vector of leaf node LeafContainerTs. - * \param binaryTreeOut_arg: binary output vector - * \param branch_arg: current branch node - * \param key_arg: reference to an octree key - * \param dataVector_arg: writes LeafContainerT pointers to this LeafContainerT* vector. + * \param branch_arg: current branch node + * \param key_arg: reference to an octree key + * \param binary_tree_out_arg: binary output vector + * \param leaf_container_vector_arg: writes LeafContainerT pointers to this LeafContainerT* vector. **/ void - serializeTreeRecursive (const BranchNode* branch_arg, OctreeKey& key_arg, - std::vector* binaryTreeOut_arg, - typename std::vector* dataVector_arg) const; + serializeTreeRecursive (const BranchNode* branch_arg, + OctreeKey& key_arg, + std::vector* binary_tree_out_arg, + typename std::vector* leaf_container_vector_arg) const; - /** \brief Rebuild an octree based on binary XOR octree description and LeafContainerT objects for leaf node initialization. - * \param binaryTreeIn_arg: iterator to input vector + /** \brief Recursive method for deserializing octree structure * \param branch_arg: current branch node - * \param depthMask_arg: depth mask used for octree key analysis and branch depth indicator + * \param depth_mask_arg: depth mask used for octree key analysis and branch depth indicator * \param key_arg: reference to an octree key - * \param dataVectorIterator_arg: iterator pointing to current LeafContainerT object to be added to a leaf node - * \param dataVectorEndIterator_arg: iterator pointing to last object in LeafContainerT input vector. - **/ - - void - deserializeTreeRecursive (BranchNode* branch_arg, - unsigned int depthMask_arg, OctreeKey& key_arg, - typename std::vector::const_iterator& binaryTreeIT_arg, - typename std::vector::const_iterator& binaryTreeIT_End_arg, - typename std::vector::const_iterator* dataVectorIterator_arg, - typename std::vector::const_iterator* dataVectorEndIterator_arg); + * \param binary_tree_input_it_arg: iterator to binary input vector + * \param binary_tree_input_it_end_arg: end iterator of binary input vector + * \param leaf_container_vector_it_arg: iterator pointing to current LeafContainerT object to be added to a leaf node + * \param leaf_container_vector_it_end_arg: iterator pointing to last object in LeafContainerT input vector. + **/ + void + deserializeTreeRecursive (BranchNode* branch_arg, unsigned int depth_mask_arg, OctreeKey& key_arg, + typename std::vector::const_iterator& binary_tree_input_it_arg, + typename std::vector::const_iterator& binary_tree_input_it_end_arg, + typename std::vector::const_iterator* leaf_container_vector_it_arg, + typename std::vector::const_iterator* leaf_container_vector_it_end_arg); ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -524,15 +529,16 @@ namespace pcl /** \brief Callback executed for every leaf node during serialization **/ - virtual void serializeTreeCallback (LeafContainerT&, - const OctreeKey &) const + virtual void + serializeTreeCallback (LeafContainerT&, const OctreeKey &) const { } /** \brief Callback executed for every leaf node during deserialization **/ - virtual void deserializeTreeCallback (LeafContainerT&, const OctreeKey&) + virtual void + deserializeTreeCallback (LeafContainerT&, const OctreeKey&) { } @@ -565,27 +571,25 @@ namespace pcl ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** \brief Amount of leaf nodes **/ - std::size_t leafCount_; + std::size_t leaf_count_; /** \brief Amount of branch nodes **/ - std::size_t branchCount_; + std::size_t branch_count_; /** \brief Pointer to root branch node of octree **/ - BranchNode* rootNode_; + BranchNode* root_node_; /** \brief Depth mask based on octree depth **/ - unsigned int depthMask_; + unsigned int depth_mask_; /** \brief Octree depth */ - unsigned int octreeDepth_; - + unsigned int octree_depth_; - /** \brief Enable dynamic_depth - * \note Leaf nodes are kept as close as possible to the root node */ + /** \brief Enable dynamic_depth **/ bool dynamic_depth_enabled_; /** \brief key range */ - OctreeKey maxKey_; + OctreeKey max_key_; }; } } diff --git a/octree/include/pcl/octree/octree_container.h b/octree/include/pcl/octree/octree_container.h index dc58f478c90..aa5c20c5dc8 100644 --- a/octree/include/pcl/octree/octree_container.h +++ b/octree/include/pcl/octree/octree_container.h @@ -73,7 +73,6 @@ namespace pcl } /** \brief Equal comparison operator - * \param[in] OctreeContainerBase to compare with */ virtual bool operator== (const OctreeContainerBase&) const @@ -82,7 +81,7 @@ namespace pcl } /** \brief Inequal comparison operator - * \param[in] OctreeContainerBase to compare with + * \param[in] other OctreeContainerBase to compare with */ bool operator!= (const OctreeContainerBase& other) const @@ -236,7 +235,7 @@ namespace pcl } /** \brief Equal comparison operator - * \param[in] OctreeContainerBase to compare with + * \param[in] other OctreeContainerBase to compare with */ virtual bool operator== (const OctreeContainerBase& other) const @@ -247,7 +246,7 @@ namespace pcl } /** \brief Add point index to container memory. This container stores a only a single point index. - * \param[in] index to be stored within leaf node. + * \param[in] data_arg index to be stored within leaf node. */ void addPointIndex (int data_arg) @@ -265,13 +264,13 @@ namespace pcl } /** \brief Retrieve point indices from container. This container stores only a single point index - * \param[out] vector of point indices to be stored within data vector + * \param[out] data_vector_arg vector of point indices to be stored within data vector */ void - getPointIndices (std::vector& dataVector_arg) const + getPointIndices (std::vector& data_vector_arg) const { if (data_>=0) - dataVector_arg.push_back (data_); + data_vector_arg.push_back (data_); } /** \brief Get size of container (number of DataT objects) @@ -328,7 +327,7 @@ namespace pcl } /** \brief Equal comparison operator - * \param[in] OctreeContainerDataTVector to compare with + * \param[in] other OctreeContainerDataTVector to compare with */ virtual bool operator== (const OctreeContainerBase& other) const @@ -339,7 +338,7 @@ namespace pcl } /** \brief Add point index to container memory. This container stores a vector of point indices. - * \param[in] index to be stored within leaf node. + * \param[in] data_arg index to be stored within leaf node. */ void addPointIndex (int data_arg) @@ -357,12 +356,12 @@ namespace pcl } /** \brief Retrieve point indices from container. This container stores a vector of point indices. - * \param[out] vector of point indices to be stored within data vector + * \param[out] data_vector_arg vector of point indices to be stored within data vector */ void - getPointIndices (std::vector& dataVector_arg) const + getPointIndices (std::vector& data_vector_arg) const { - dataVector_arg.insert (dataVector_arg.end (), leafDataTVector_.begin (), leafDataTVector_.end ()); + data_vector_arg.insert (data_vector_arg.end (), leafDataTVector_.begin (), leafDataTVector_.end ()); } /** \brief Retrieve reference to point indices vector. This container stores a vector of point indices. diff --git a/octree/include/pcl/octree/octree_iterator.h b/octree/include/pcl/octree/octree_iterator.h index 81402e5d575..48b71f6715b 100644 --- a/octree/include/pcl/octree/octree_iterator.h +++ b/octree/include/pcl/octree/octree_iterator.h @@ -89,27 +89,29 @@ namespace pcl /** \brief Empty constructor. */ explicit - OctreeIteratorBase (unsigned int maxDepth_arg = 0) : - octree_ (0), currentState_(0), maxOctreeDepth_(maxDepth_arg) + OctreeIteratorBase (unsigned int max_depth_arg = 0) : + octree_ (0), current_state_(0), max_octree_depth_(max_depth_arg) { this->reset (); } /** \brief Constructor. * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its root node. + * \param[in] max_depth_arg Depth limitation during traversal */ explicit - OctreeIteratorBase (OctreeT* octree_arg, unsigned int maxDepth_arg = 0) : - octree_ (octree_arg), currentState_(0), maxOctreeDepth_(maxDepth_arg) + OctreeIteratorBase (OctreeT* octree_arg, unsigned int max_depth_arg = 0) : + octree_ (octree_arg), current_state_(0), max_octree_depth_(max_depth_arg) { this->reset (); } /** \brief Copy constructor. * \param[in] src the iterator to copy into this + * \param[in] max_depth_arg Depth limitation during traversal */ - OctreeIteratorBase (const OctreeIteratorBase& src, unsigned int maxDepth_arg = 0) : - octree_ (src.octree_), currentState_(0), maxOctreeDepth_(maxDepth_arg) + OctreeIteratorBase (const OctreeIteratorBase& src, unsigned int max_depth_arg = 0) : + octree_ (src.octree_), current_state_(0), max_octree_depth_(max_depth_arg) { this->reset (); } @@ -121,8 +123,8 @@ namespace pcl operator = (const OctreeIteratorBase& src) { octree_ = src.octree_; - currentState_ = src.currentState_; - maxOctreeDepth_ = src.maxOctreeDepth_; + current_state_ = src.current_state_; + max_octree_depth_ = src.max_octree_depth_; return (*this); } @@ -138,8 +140,8 @@ namespace pcl bool operator==(const OctreeIteratorBase& other) const { return (( octree_ ==other.octree_) && - ( currentState_ == other.currentState_) && - ( maxOctreeDepth_ == other.maxOctreeDepth_) ); + ( current_state_ == other.current_state_) && + ( max_octree_depth_ == other.max_octree_depth_) ); } /** \brief Inequal comparison operator @@ -148,17 +150,17 @@ namespace pcl bool operator!=(const OctreeIteratorBase& other) const { return (( octree_ !=other.octree_) && - ( currentState_ != other.currentState_) && - ( maxOctreeDepth_ != other.maxOctreeDepth_) ); + ( current_state_ != other.current_state_) && + ( max_octree_depth_ != other.max_octree_depth_) ); } /** \brief Reset iterator */ inline void reset () { - currentState_ = 0; - if (octree_ && (!maxOctreeDepth_)) + current_state_ = 0; + if (octree_ && (!max_octree_depth_)) { - maxOctreeDepth_ = octree_->getTreeDepth(); + max_octree_depth_ = octree_->getTreeDepth(); } } @@ -169,9 +171,9 @@ namespace pcl getCurrentOctreeKey () const { assert(octree_!=0); - assert(currentState_!=0); + assert(current_state_!=0); - return (currentState_->key_); + return (current_state_->key_); } /** \brief Get the current depth level of octree @@ -181,9 +183,9 @@ namespace pcl getCurrentOctreeDepth () const { assert(octree_!=0); - assert(currentState_!=0); + assert(current_state_!=0); - return (currentState_->depth_); + return (current_state_->depth_); } /** \brief Get the current octree node @@ -193,9 +195,9 @@ namespace pcl getCurrentOctreeNode () const { assert(octree_!=0); - assert(currentState_!=0); + assert(current_state_!=0); - return (currentState_->node_); + return (current_state_->node_); } @@ -206,9 +208,9 @@ namespace pcl isBranchNode () const { assert(octree_!=0); - assert(currentState_!=0); + assert(current_state_!=0); - return (currentState_->node_->getNodeType () == BRANCH_NODE); + return (current_state_->node_->getNodeType () == BRANCH_NODE); } /** \brief check if current node is a branch node @@ -218,9 +220,9 @@ namespace pcl isLeafNode () const { assert(octree_!=0); - assert(currentState_!=0); + assert(current_state_!=0); - return (currentState_->node_->getNodeType () == LEAF_NODE); + return (current_state_->node_->getNodeType () == LEAF_NODE); } /** \brief *operator. @@ -229,9 +231,9 @@ namespace pcl inline OctreeNode* operator* () const { // return designated object - if (octree_ && currentState_) + if (octree_ && current_state_) { - return (currentState_->node_); + return (current_state_->node_); } else { return 0; @@ -247,16 +249,16 @@ namespace pcl char ret = 0; assert(octree_!=0); - assert(currentState_!=0); + assert(current_state_!=0); if (isBranchNode ()) { // current node is a branch node - const BranchNode* currentBranch = static_cast (currentState_->node_); + const BranchNode* current_branch = static_cast (current_state_->node_); // get child configuration bit pattern - ret = octree_->getBranchBitPattern (*currentBranch); + ret = octree_->getBranchBitPattern (*current_branch); } @@ -270,10 +272,10 @@ namespace pcl getLeafContainer () const { assert(octree_!=0); - assert(currentState_!=0); + assert(current_state_!=0); assert(this->isLeafNode()); - LeafNode* leaf_node = static_cast(currentState_->node_); + LeafNode* leaf_node = static_cast(current_state_->node_); return leaf_node->getContainer(); } @@ -285,10 +287,10 @@ namespace pcl getLeafContainer () { assert(octree_!=0); - assert(currentState_!=0); + assert(current_state_!=0); assert(this->isLeafNode()); - LeafNode* leaf_node = static_cast(currentState_->node_); + LeafNode* leaf_node = static_cast(current_state_->node_); return leaf_node->getContainer(); } @@ -300,10 +302,10 @@ namespace pcl getBranchContainer () const { assert(octree_!=0); - assert(currentState_!=0); + assert(current_state_!=0); assert(this->isBranchNode()); - BranchNode* branch_node = static_cast(currentState_->node_); + BranchNode* branch_node = static_cast(current_state_->node_); return branch_node->getContainer(); } @@ -315,10 +317,10 @@ namespace pcl getBranchContainer () { assert(octree_!=0); - assert(currentState_!=0); + assert(current_state_!=0); assert(this->isBranchNode()); - BranchNode* branch_node = static_cast(currentState_->node_); + BranchNode* branch_node = static_cast(current_state_->node_); return branch_node->getContainer(); } @@ -332,9 +334,9 @@ namespace pcl unsigned long id = 0; assert(octree_!=0); - assert(currentState_!=0); + assert(current_state_!=0); - if (currentState_) + if (current_state_) { const OctreeKey& key = getCurrentOctreeKey(); // calculate integer id with respect to octree key @@ -350,10 +352,10 @@ namespace pcl OctreeT* octree_; /** \brief Pointer to current iterator state. */ - IteratorState* currentState_; + IteratorState* current_state_; /** \brief Maximum octree depth */ - unsigned int maxOctreeDepth_; + unsigned int max_octree_depth_; }; ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -372,15 +374,17 @@ namespace pcl typedef typename OctreeIteratorBase::BranchNode BranchNode; /** \brief Empty constructor. + * \param[in] max_depth_arg Depth limitation during traversal */ explicit - OctreeDepthFirstIterator (unsigned int maxDepth_arg = 0); + OctreeDepthFirstIterator (unsigned int max_depth_arg = 0); /** \brief Constructor. * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its root node. + * \param[in] max_depth_arg Depth limitation during traversal */ explicit - OctreeDepthFirstIterator (OctreeT* octree_arg, unsigned int maxDepth_arg = 0); + OctreeDepthFirstIterator (OctreeT* octree_arg, unsigned int max_depth_arg = 0); /** \brief Empty deconstructor. */ virtual @@ -399,10 +403,10 @@ namespace pcl if (stack_.size()) { - this->currentState_ = &stack_.back(); + this->current_state_ = &stack_.back(); } else { - this->currentState_ = 0; + this->current_state_ = 0; } return (*this); @@ -456,15 +460,17 @@ namespace pcl public: /** \brief Empty constructor. + * \param[in] max_depth_arg Depth limitation during traversal */ explicit - OctreeBreadthFirstIterator (unsigned int maxDepth_arg = 0); + OctreeBreadthFirstIterator (unsigned int max_depth_arg = 0); /** \brief Constructor. * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its root node. + * \param[in] max_depth_arg Depth limitation during traversal */ explicit - OctreeBreadthFirstIterator (OctreeT* octree_arg, unsigned int maxDepth_arg = 0); + OctreeBreadthFirstIterator (OctreeT* octree_arg, unsigned int max_depth_arg = 0); /** \brief Empty deconstructor. */ virtual @@ -483,10 +489,10 @@ namespace pcl if (FIFO_.size()) { - this->currentState_ = &FIFO_.front(); + this->current_state_ = &FIFO_.front(); } else { - this->currentState_ = 0; + this->current_state_ = 0; } return (*this); @@ -534,20 +540,22 @@ namespace pcl public: /** \brief Empty constructor. + * \param[in] max_depth_arg Depth limitation during traversal */ explicit - OctreeLeafNodeIterator (unsigned int maxDepth_arg = 0) : - OctreeDepthFirstIterator (maxDepth_arg) + OctreeLeafNodeIterator (unsigned int max_depth_arg = 0) : + OctreeDepthFirstIterator (max_depth_arg) { reset (); } /** \brief Constructor. * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its root node. + * \param[in] max_depth_arg Depth limitation during traversal */ explicit - OctreeLeafNodeIterator (OctreeT* octree_arg, unsigned int maxDepth_arg = 0) : - OctreeDepthFirstIterator (octree_arg, maxDepth_arg) + OctreeLeafNodeIterator (OctreeT* octree_arg, unsigned int max_depth_arg = 0) : + OctreeDepthFirstIterator (octree_arg, max_depth_arg) { reset (); } @@ -576,7 +584,7 @@ namespace pcl do { OctreeDepthFirstIterator::operator++ (); - } while ((this->currentState_) && (this->currentState_->node_->getNodeType () != LEAF_NODE)); + } while ((this->current_state_) && (this->current_state_->node_->getNodeType () != LEAF_NODE)); return (*this); } @@ -601,8 +609,8 @@ namespace pcl // return designated object OctreeNode* ret = 0; - if (this->currentState_ && (this->currentState_->node_->getNodeType () == LEAF_NODE)) - ret = this->currentState_->node_; + if (this->current_state_ && (this->current_state_->node_->getNodeType () == LEAF_NODE)) + ret = this->current_state_->node_; return (ret); } }; diff --git a/octree/include/pcl/octree/octree_key.h b/octree/include/pcl/octree/octree_key.h index 2d48e6dbcf5..704323c3581 100644 --- a/octree/include/pcl/octree/octree_key.h +++ b/octree/include/pcl/octree/octree_key.h @@ -38,6 +38,8 @@ #ifndef PCL_OCTREE_KEY_H #define PCL_OCTREE_KEY_H +#include + namespace pcl { namespace octree @@ -65,9 +67,9 @@ namespace pcl } /** \brief Copy constructor. */ - OctreeKey (const OctreeKey& source) : - x (source.x), y (source.y), z (source.z) + OctreeKey (const OctreeKey& source) { + memcpy(key_, source.key_, sizeof(key_)); } /** \brief Operator== for comparing octree keys with each other. @@ -134,9 +136,18 @@ namespace pcl static const unsigned char maxDepth = static_cast(sizeof(uint32_t)*8); // Indices addressing a voxel at (X, Y, Z) - uint32_t x; - uint32_t y; - uint32_t z; + + union + { + struct + { + uint32_t x; + uint32_t y; + uint32_t z; + }; + uint32_t key_[3]; + }; + }; } diff --git a/octree/include/pcl/octree/octree_node_pool.h b/octree/include/pcl/octree/octree_node_pool.h index 777f5d5d4f6..62568c82826 100644 --- a/octree/include/pcl/octree/octree_node_pool.h +++ b/octree/include/pcl/octree/octree_node_pool.h @@ -70,7 +70,7 @@ namespace pcl } /** \brief Push node to pool - * \param childIdx_arg: pointer of noe + * \param node_arg: add this node to the pool * */ inline void diff --git a/octree/include/pcl/octree/octree_nodes.h b/octree/include/pcl/octree/octree_nodes.h index ff38514cc02..13a48c82b18 100644 --- a/octree/include/pcl/octree/octree_nodes.h +++ b/octree/include/pcl/octree/octree_nodes.h @@ -207,7 +207,7 @@ namespace pcl OctreeNode() { // reset pointer to child node vectors - memset (childNodeArray_, 0, sizeof(childNodeArray_)); + memset (child_node_array_, 0, sizeof(child_node_array_)); } /** \brief Empty constructor. */ @@ -216,11 +216,11 @@ namespace pcl { unsigned char i; - memset (childNodeArray_, 0, sizeof(childNodeArray_)); + memset (child_node_array_, 0, sizeof(child_node_array_)); for (i = 0; i < 8; ++i) - if (source.childNodeArray_[i]) - childNodeArray_[i] = source.childNodeArray_[i]->deepCopy (); + if (source.child_node_array_[i]) + child_node_array_[i] = source.child_node_array_[i]->deepCopy (); } /** \brief Copy operator. */ @@ -229,11 +229,11 @@ namespace pcl { unsigned char i; - memset (childNodeArray_, 0, sizeof(childNodeArray_)); + memset (child_node_array_, 0, sizeof(child_node_array_)); for (i = 0; i < 8; ++i) - if (source.childNodeArray_[i]) - childNodeArray_[i] = source.childNodeArray_[i]->deepCopy (); + if (source.child_node_array_[i]) + child_node_array_[i] = source.child_node_array_[i]->deepCopy (); return (*this); } @@ -251,25 +251,25 @@ namespace pcl } /** \brief Access operator. - * \param childIdx_arg: index to child node + * \param child_idx_arg: index to child node * \return OctreeNode pointer * */ inline OctreeNode*& - operator[] (unsigned char childIdx_arg) + operator[] (unsigned char child_idx_arg) { - assert(childIdx_arg < 8); - return childNodeArray_[childIdx_arg]; + assert(child_idx_arg < 8); + return child_node_array_[child_idx_arg]; } /** \brief Get pointer to child - * \param childIdx_arg: index to child node + * \param child_idx_arg: index to child node * \return OctreeNode pointer * */ inline OctreeNode* - getChildPtr (unsigned char childIdx_arg) const + getChildPtr (unsigned char child_idx_arg) const { - assert(childIdx_arg < 8); - return childNodeArray_[childIdx_arg]; + assert(child_idx_arg < 8); + return child_node_array_[child_idx_arg]; } /** \brief Get pointer to child @@ -278,17 +278,17 @@ namespace pcl inline void setChildPtr (OctreeNode* child, unsigned char index) { assert(index < 8); - childNodeArray_[index] = child; + child_node_array_[index] = child; } /** \brief Check if branch is pointing to a particular child node - * \param childIdx_arg: index to child node + * \param child_idx_arg: index to child node * \return "true" if pointer to child node exists; "false" otherwise * */ - inline bool hasChild (unsigned char childIdx_arg) const + inline bool hasChild (unsigned char child_idx_arg) const { - return (childNodeArray_[childIdx_arg] != 0); + return (child_node_array_[child_idx_arg] != 0); } @@ -298,14 +298,14 @@ namespace pcl * */ /* inline bool isPrunable () const { - const OctreeNode* firstChild = childNodeArray_[0]; + const OctreeNode* firstChild = child_node_array_[0]; if (!firstChild || firstChild->getNodeType()==BRANCH_NODE) return false; bool prunable = true; for (unsigned char i = 1; i < 8 && prunable; ++i) { - const OctreeNode* child = childNodeArray_[i]; + const OctreeNode* child = child_node_array_[i]; if ( (!child) || (child->getNodeType()==BRANCH_NODE) || ((*static_cast(child)) == (*static_cast(child)) ) ) @@ -325,7 +325,7 @@ namespace pcl // reset node void reset() { - memset(childNodeArray_, 0, sizeof(childNodeArray_)); + memset(child_node_array_, 0, sizeof(child_node_array_)); container_.reset(); } @@ -387,7 +387,7 @@ namespace pcl } protected: - OctreeNode* childNodeArray_[8]; + OctreeNode* child_node_array_[8]; ContainerT container_; }; diff --git a/octree/include/pcl/octree/octree_pointcloud.h b/octree/include/pcl/octree/octree_pointcloud.h index 90cdab4253c..6f424b44342 100644 --- a/octree/include/pcl/octree/octree_pointcloud.h +++ b/octree/include/pcl/octree/octree_pointcloud.h @@ -180,7 +180,7 @@ namespace pcl inline void setResolution (double resolution_arg) { // octree needs to be empty to change its resolution - assert( this->leafCount_ == 0); + assert( this->leaf_count_ == 0); resolution_ = resolution_arg; @@ -200,7 +200,7 @@ namespace pcl * */ inline unsigned int getTreeDepth () const { - return this->octreeDepth_; + return this->octree_depth_; } /** \brief Add points from input point cloud to octree. */ @@ -208,11 +208,11 @@ namespace pcl addPointsFromInputCloud (); /** \brief Add point at given index from input point cloud to octree. Index will be also added to indices vector. - * \param[in] pointIdx_arg index of point to be added + * \param[in] point_idx_arg index of point to be added * \param[in] indices_arg pointer to indices vector of the dataset (given by \a setInputCloud) */ void - addPointFromCloud (const int pointIdx_arg, IndicesPtr indices_arg); + addPointFromCloud (const int point_idx_arg, IndicesPtr indices_arg); /** \brief Add point simultaneously to octree and input point cloud. * \param[in] point_arg point to be added @@ -227,8 +227,7 @@ namespace pcl * \param[in] indices_arg pointer to indices vector of the dataset (given by \a setInputCloud) */ void - addPointToCloud (const PointT& point_arg, PointCloudPtr cloud_arg, - IndicesPtr indices_arg); + addPointToCloud (const PointT& point_arg, PointCloudPtr cloud_arg, IndicesPtr indices_arg); /** \brief Check if voxel at given point exist. * \param[in] point_arg point to be checked @@ -238,41 +237,38 @@ namespace pcl isVoxelOccupiedAtPoint (const PointT& point_arg) const; /** \brief Delete the octree structure and its leaf nodes. - * \param freeMemory_arg: if "true", allocated octree nodes are deleted, otherwise they are pushed to the octree node pool * */ void deleteTree () { // reset bounding box - minX_ = minY_ = maxY_ = minZ_ = maxZ_ = 0; - this->boundingBoxDefined_ = false; + min_x_ = min_y_ = max_y_ = min_z_ = max_z_ = 0; + this->bounding_box_defined_ = false; OctreeT::deleteTree (); } /** \brief Check if voxel at given point coordinates exist. - * \param[in] pointX_arg X coordinate of point to be checked - * \param[in] pointY_arg Y coordinate of point to be checked - * \param[in] pointZ_arg Z coordinate of point to be checked + * \param[in] point_x_arg X coordinate of point to be checked + * \param[in] point_y_arg Y coordinate of point to be checked + * \param[in] point_z_arg Z coordinate of point to be checked * \return "true" if voxel exist; "false" otherwise */ bool - isVoxelOccupiedAtPoint (const double pointX_arg, - const double pointY_arg, const double pointZ_arg) const; + isVoxelOccupiedAtPoint (const double point_x_arg, const double point_y_arg, const double point_z_arg) const; /** \brief Check if voxel at given point from input cloud exist. - * \param[in] pointIdx_arg point to be checked + * \param[in] point_idx_arg point to be checked * \return "true" if voxel exist; "false" otherwise */ bool - isVoxelOccupiedAtPoint (const int& pointIdx_arg) const; + isVoxelOccupiedAtPoint (const int& point_idx_arg) const; /** \brief Get a PointT vector of centers of all occupied voxels. - * \param[out] voxelCenterList_arg results are written to this vector of PointT elements + * \param[out] voxel_center_list_arg results are written to this vector of PointT elements * \return number of occupied voxels */ int - getOccupiedVoxelCenters ( - AlignedPointTVector &voxelCenterList_arg) const; + getOccupiedVoxelCenters (AlignedPointTVector &voxel_center_list_arg) const; /** \brief Get a PointT vector of centers of voxels intersected by a line segment. * This returns a approximation of the actual intersected voxels by walking @@ -296,10 +292,10 @@ namespace pcl deleteVoxelAtPoint (const PointT& point_arg); /** \brief Delete leaf node / voxel at given point from input cloud - * \param[in] pointIdx_arg index of point addressing the voxel to be deleted. + * \param[in] point_idx_arg index of point addressing the voxel to be deleted. */ void - deleteVoxelAtPoint (const int& pointIdx_arg); + deleteVoxelAtPoint (const int& point_idx_arg); ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Bounding box methods @@ -311,28 +307,26 @@ namespace pcl /** \brief Define bounding box for octree * \note Bounding box cannot be changed once the octree contains elements. - * \param[in] minX_arg X coordinate of lower bounding box corner - * \param[in] minY_arg Y coordinate of lower bounding box corner - * \param[in] minZ_arg Z coordinate of lower bounding box corner - * \param[in] maxX_arg X coordinate of upper bounding box corner - * \param[in] maxY_arg Y coordinate of upper bounding box corner - * \param[in] maxZ_arg Z coordinate of upper bounding box corner + * \param[in] min_x_arg X coordinate of lower bounding box corner + * \param[in] min_y_arg Y coordinate of lower bounding box corner + * \param[in] min_z_arg Z coordinate of lower bounding box corner + * \param[in] max_x_arg X coordinate of upper bounding box corner + * \param[in] max_y_arg Y coordinate of upper bounding box corner + * \param[in] max_z_arg Z coordinate of upper bounding box corner */ void - defineBoundingBox (const double minX_arg, const double minY_arg, - const double minZ_arg, const double maxX_arg, const double maxY_arg, - const double maxZ_arg); + defineBoundingBox (const double min_x_arg, const double min_y_arg, const double min_z_arg, + const double max_x_arg, const double max_y_arg, const double max_z_arg); /** \brief Define bounding box for octree * \note Lower bounding box point is set to (0, 0, 0) * \note Bounding box cannot be changed once the octree contains elements. - * \param[in] maxX_arg X coordinate of upper bounding box corner - * \param[in] maxY_arg Y coordinate of upper bounding box corner - * \param[in] maxZ_arg Z coordinate of upper bounding box corner + * \param[in] max_x_arg X coordinate of upper bounding box corner + * \param[in] max_y_arg Y coordinate of upper bounding box corner + * \param[in] max_z_arg Z coordinate of upper bounding box corner */ void - defineBoundingBox (const double maxX_arg, const double maxY_arg, - const double maxZ_arg); + defineBoundingBox (const double max_x_arg, const double max_y_arg, const double max_z_arg); /** \brief Define bounding box cube for octree * \note Lower bounding box corner is set to (0, 0, 0) @@ -344,45 +338,46 @@ namespace pcl /** \brief Get bounding box for octree * \note Bounding box cannot be changed once the octree contains elements. - * \param[in] minX_arg X coordinate of lower bounding box corner - * \param[in] minY_arg Y coordinate of lower bounding box corner - * \param[in] minZ_arg Z coordinate of lower bounding box corner - * \param[in] maxX_arg X coordinate of upper bounding box corner - * \param[in] maxY_arg Y coordinate of upper bounding box corner - * \param[in] maxZ_arg Z coordinate of upper bounding box corner + * \param[in] min_x_arg X coordinate of lower bounding box corner + * \param[in] min_y_arg Y coordinate of lower bounding box corner + * \param[in] min_z_arg Z coordinate of lower bounding box corner + * \param[in] max_x_arg X coordinate of upper bounding box corner + * \param[in] max_y_arg Y coordinate of upper bounding box corner + * \param[in] max_z_arg Z coordinate of upper bounding box corner */ void - getBoundingBox (double& minX_arg, double& minY_arg, double& minZ_arg, - double& maxX_arg, double& maxY_arg, double& maxZ_arg) const; + getBoundingBox (double& min_x_arg, double& min_y_arg, double& min_z_arg, + double& max_x_arg, double& max_y_arg, double& max_z_arg) const; /** \brief Calculates the squared diameter of a voxel at given tree depth - * \param[in] treeDepth_arg depth/level in octree + * \param[in] tree_depth_arg depth/level in octree * \return squared diameter */ double - getVoxelSquaredDiameter (unsigned int treeDepth_arg) const; + getVoxelSquaredDiameter (unsigned int tree_depth_arg) const; /** \brief Calculates the squared diameter of a voxel at leaf depth * \return squared diameter */ - inline double getVoxelSquaredDiameter () const + inline double + getVoxelSquaredDiameter () const { - return getVoxelSquaredDiameter (this->octreeDepth_); + return getVoxelSquaredDiameter (this->octree_depth_); } /** \brief Calculates the squared voxel cube side length at given tree depth - * \param[in] treeDepth_arg depth/level in octree + * \param[in] tree_depth_arg depth/level in octree * \return squared voxel cube side length */ double - getVoxelSquaredSideLen (unsigned int treeDepth_arg) const; + getVoxelSquaredSideLen (unsigned int tree_depth_arg) const; /** \brief Calculates the squared voxel cube side length at leaf level * \return squared voxel cube side length */ inline double getVoxelSquaredSideLen () const { - return getVoxelSquaredSideLen (this->octreeDepth_); + return getVoxelSquaredSideLen (this->octree_depth_); } /** \brief Generate bounds of the current voxel of an octree iterator @@ -390,8 +385,8 @@ namespace pcl * \param[out] min_pt lower bound of voxel * \param[out] max_pt upper bound of voxel */ - inline void getVoxelBounds (OctreeIteratorBase& iterator, - Eigen::Vector3f &min_pt, Eigen::Vector3f &max_pt) + inline void + getVoxelBounds (OctreeIteratorBase& iterator, Eigen::Vector3f &min_pt, Eigen::Vector3f &max_pt) { this->genVoxelBoundsFromOctreeKey (iterator.getCurrentOctreeKey (), iterator.getCurrentOctreeDepth (), min_pt, max_pt); @@ -404,20 +399,20 @@ namespace pcl inline void enableDynamicDepth ( size_t maxObjsPerLeaf ) { - assert(this->leafCount_==0); - maxObjsPerLeaf_ = maxObjsPerLeaf; + assert(this->leaf_count_==0); + max_objs_per_leaf_ = maxObjsPerLeaf; - this->dynamic_depth_enabled_ = static_cast (maxObjsPerLeaf_>0); + this->dynamic_depth_enabled_ = static_cast (max_objs_per_leaf_>0); } protected: /** \brief Add point at index from input pointcloud dataset to octree - * \param[in] pointIdx_arg the index representing the point in the dataset given by \a setInputCloud to be added + * \param[in] point_idx_arg the index representing the point in the dataset given by \a setInputCloud to be added */ virtual void - addPointIdx (const int pointIdx_arg); + addPointIdx (const int point_idx_arg); /** \brief Add point at index from input pointcloud dataset to octree * \param[in] leaf_node to be expanded @@ -459,20 +454,20 @@ namespace pcl getKeyBitSize (); /** \brief Grow the bounding box/octree until point fits - * \param[in] pointIdx_arg point that should be within bounding box; + * \param[in] point_idx_arg point that should be within bounding box; */ void - adoptBoundingBoxToPoint (const PointT& pointIdx_arg); + adoptBoundingBoxToPoint (const PointT& point_idx_arg); /** \brief Checks if given point is within the bounding box of the octree - * \param[in] pointIdx_arg point to be checked for bounding box violations + * \param[in] point_idx_arg point to be checked for bounding box violations * \return "true" - no bound violation */ - inline bool isPointWithinBoundingBox (const PointT& pointIdx_arg) const + inline bool isPointWithinBoundingBox (const PointT& point_idx_arg) const { - return (! ( (pointIdx_arg.x < minX_) || (pointIdx_arg.y < minY_) - || (pointIdx_arg.z < minZ_) || (pointIdx_arg.x >= maxX_) - || (pointIdx_arg.y >= maxY_) || (pointIdx_arg.z >= maxZ_))); + return (! ( (point_idx_arg.x < min_x_) || (point_idx_arg.y < min_y_) + || (point_idx_arg.z < min_z_) || (point_idx_arg.x >= max_x_) + || (point_idx_arg.y >= max_y_) || (point_idx_arg.z >= max_z_))); } /** \brief Generate octree key for voxel at a given point @@ -484,14 +479,14 @@ namespace pcl OctreeKey &key_arg) const; /** \brief Generate octree key for voxel at a given point - * \param[in] pointX_arg X coordinate of point addressing a voxel - * \param[in] pointY_arg Y coordinate of point addressing a voxel - * \param[in] pointZ_arg Z coordinate of point addressing a voxel + * \param[in] point_x_arg X coordinate of point addressing a voxel + * \param[in] point_y_arg Y coordinate of point addressing a voxel + * \param[in] point_z_arg Z coordinate of point addressing a voxel * \param[out] key_arg write octree key to this reference */ void - genOctreeKeyforPoint (const double pointX_arg, const double pointY_arg, - const double pointZ_arg, OctreeKey & key_arg) const; + genOctreeKeyforPoint (const double point_x_arg, const double point_y_arg, const double point_z_arg, + OctreeKey & key_arg) const; /** \brief Virtual method for generating octree key for a given point index. * \note This method enables to assign indices to leaf nodes during octree deserialization. @@ -512,34 +507,34 @@ namespace pcl /** \brief Generate a point at center of octree voxel at given tree level * \param[in] key_arg octree key addressing an octree node. - * \param[in] treeDepth_arg octree depth of query voxel + * \param[in] tree_depth_arg octree depth of query voxel * \param[out] point_arg write leaf node center point to this reference */ void genVoxelCenterFromOctreeKey (const OctreeKey & key_arg, - unsigned int treeDepth_arg, PointT& point_arg) const; + unsigned int tree_depth_arg, PointT& point_arg) const; /** \brief Generate bounds of an octree voxel using octree key and tree depth arguments * \param[in] key_arg octree key addressing an octree node. - * \param[in] treeDepth_arg octree depth of query voxel + * \param[in] tree_depth_arg octree depth of query voxel * \param[out] min_pt lower bound of voxel * \param[out] max_pt upper bound of voxel */ void genVoxelBoundsFromOctreeKey (const OctreeKey & key_arg, - unsigned int treeDepth_arg, Eigen::Vector3f &min_pt, + unsigned int tree_depth_arg, Eigen::Vector3f &min_pt, Eigen::Vector3f &max_pt) const; /** \brief Recursively search the tree for all leaf nodes and return a vector of voxel centers. * \param[in] node_arg current octree node to be explored * \param[in] key_arg octree key addressing a leaf node. - * \param[out] voxelCenterList_arg results are written to this vector of PointT elements + * \param[out] voxel_center_list_arg results are written to this vector of PointT elements * \return number of voxels found */ int getOccupiedVoxelCentersRecursive (const BranchNode* node_arg, const OctreeKey& key_arg, - AlignedPointTVector &voxelCenterList_arg) const; + AlignedPointTVector &voxel_center_list_arg) const; ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Globals @@ -557,22 +552,22 @@ namespace pcl double resolution_; // Octree bounding box coordinates - double minX_; - double maxX_; + double min_x_; + double max_x_; - double minY_; - double maxY_; + double min_y_; + double max_y_; - double minZ_; - double maxZ_; + double min_z_; + double max_z_; /** \brief Flag indicating if octree has defined bounding box. */ - bool boundingBoxDefined_; + bool bounding_box_defined_; /** \brief Amount of DataT objects per leafNode before expanding branch * \note zero indicates a fixed/maximum depth octree structure * **/ - std::size_t maxObjsPerLeaf_; + std::size_t max_objs_per_leaf_; }; } diff --git a/octree/include/pcl/octree/octree_pointcloud_density.h b/octree/include/pcl/octree/octree_pointcloud_density.h index 155d415dabf..667011eb880 100644 --- a/octree/include/pcl/octree/octree_pointcloud_density.h +++ b/octree/include/pcl/octree/octree_pointcloud_density.h @@ -53,7 +53,7 @@ namespace pcl { public: /** \brief Class initialization. */ - OctreePointCloudDensityContainer () : pointCounter_ (0) + OctreePointCloudDensityContainer () : point_counter_ (0) { } @@ -70,14 +70,14 @@ namespace pcl } /** \brief Equal comparison operator - * \param[in] OctreePointCloudDensityContainer to compare with + * \param[in] other OctreePointCloudDensityContainer to compare with */ virtual bool operator==(const OctreeContainerBase& other) const { const OctreePointCloudDensityContainer* otherContainer = dynamic_cast(&other); - return (this->pointCounter_==otherContainer->pointCounter_); + return (this->point_counter_==otherContainer->point_counter_); } /** \brief Read input data. Only an internal counter is increased. @@ -85,7 +85,7 @@ namespace pcl void addPointIndex (int) { - pointCounter_++; + point_counter_++; } /** \brief Return point counter. @@ -94,18 +94,18 @@ namespace pcl unsigned int getPointCounter () { - return (pointCounter_); + return (point_counter_); } /** \brief Reset leaf node. */ virtual void reset () { - pointCounter_ = 0; + point_counter_ = 0; } private: - unsigned int pointCounter_; + unsigned int point_counter_; }; @@ -143,14 +143,14 @@ namespace pcl unsigned int getVoxelDensityAtPoint (const PointT& point_arg) const { - unsigned int pointCount = 0; + unsigned int point_count = 0; OctreePointCloudDensityContainer* leaf = this->findLeafAtPoint (point_arg); if (leaf) - pointCount = leaf->getPointCounter (); + point_count = leaf->getPointCounter (); - return (pointCount); + return (point_count); } }; } diff --git a/octree/include/pcl/octree/octree_pointcloud_supervoxel.h b/octree/include/pcl/octree/octree_pointcloud_supervoxel.h index 87a5223e8c6..8dd6fe73fa9 100644 --- a/octree/include/pcl/octree/octree_pointcloud_supervoxel.h +++ b/octree/include/pcl/octree/octree_pointcloud_supervoxel.h @@ -395,7 +395,7 @@ namespace pcl LeafNode* new_leaf = 0; BranchNode* leaf_parent = 0; - this->createLeafRecursive (key, this->depthMask_, this->rootNode_, new_leaf, leaf_parent); + this->createLeafRecursive (key, this->depth_mask_, this->root_node_, new_leaf, leaf_parent); if (new_leaf) { diff --git a/octree/include/pcl/octree/octree_pointcloud_voxelcentroid.h b/octree/include/pcl/octree/octree_pointcloud_voxelcentroid.h index 83d03a25c0d..e90d3fb84d3 100644 --- a/octree/include/pcl/octree/octree_pointcloud_voxelcentroid.h +++ b/octree/include/pcl/octree/octree_pointcloud_voxelcentroid.h @@ -220,10 +220,9 @@ namespace pcl getVoxelCentroids (typename OctreePointCloud::AlignedPointTVector &voxel_centroid_list_arg) const; /** \brief Recursively explore the octree and output a PointT vector of centroids for all occupied voxels. - ** \param[in] binaryTreeOut_arg: binary output vector * \param[in] branch_arg: current branch node + * \param[in] key_arg: current key * \param[out] voxel_centroid_list_arg results are written to this vector of PointT elements - * \return number of occupied voxels */ void getVoxelCentroidsRecursive (const BranchNode* branch_arg, diff --git a/octree/include/pcl/octree/octree_search.h b/octree/include/pcl/octree/octree_search.h index 7714a6faa9a..5d1ce079b92 100644 --- a/octree/include/pcl/octree/octree_search.h +++ b/octree/include/pcl/octree/octree_search.h @@ -93,19 +93,19 @@ namespace pcl /** \brief Search for neighbors within a voxel at given point * \param[in] point point addressing a leaf node voxel - * \param[out] pointIdx_data the resultant indices of the neighboring voxel points + * \param[out] point_idx_data the resultant indices of the neighboring voxel points * \return "true" if leaf node exist; "false" otherwise */ bool - voxelSearch (const PointT& point, std::vector& pointIdx_data); + voxelSearch (const PointT& point, std::vector& point_idx_data); /** \brief Search for neighbors within a voxel at given point referenced by a point index * \param[in] index the index in input cloud defining the query point - * \param[out] pointIdx_data the resultant indices of the neighboring voxel points + * \param[out] point_idx_data the resultant indices of the neighboring voxel points * \return "true" if leaf node exist; "false" otherwise */ bool - voxelSearch (const int index, std::vector& pointIdx_data); + voxelSearch (const int index, std::vector& point_idx_data); /** \brief Search for k-nearest neighbors at the query point. * \param[in] cloud the point cloud data @@ -141,11 +141,10 @@ namespace pcl * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!) * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k * a priori!) - * \return number of neighbors found - */ + * \return number of neighbors found + */ int - nearestKSearch (int index, int k, std::vector &k_indices, - std::vector &k_sqr_distances); + nearestKSearch (int index, int k, std::vector &k_indices, std::vector &k_sqr_distances); /** \brief Search for approx. nearest neighbor at the query point. * \param[in] cloud the point cloud data @@ -155,8 +154,7 @@ namespace pcl * \return number of neighbors found */ inline void - approxNearestSearch (const PointCloud &cloud, int query_index, int &result_index, - float &sqr_distance) + approxNearestSearch (const PointCloud &cloud, int query_index, int &result_index, float &sqr_distance) { return (approxNearestSearch (cloud.points[query_index], result_index, sqr_distance)); } @@ -189,9 +187,8 @@ namespace pcl * \return number of neighbors found in radius */ int - radiusSearch (const PointCloud &cloud, int index, double radius, - std::vector &k_indices, std::vector &k_sqr_distances, - unsigned int max_nn = 0) + radiusSearch (const PointCloud &cloud, int index, double radius, std::vector &k_indices, + std::vector &k_sqr_distances, unsigned int max_nn = 0) { return (radiusSearch (cloud.points[index], radius, k_indices, k_sqr_distances, max_nn)); } @@ -224,26 +221,25 @@ namespace pcl /** \brief Get a PointT vector of centers of all voxels that intersected by a ray (origin, direction). * \param[in] origin ray origin * \param[in] direction ray direction vector - * \param[out] voxelCenterList results are written to this vector of PointT elements - * \param[in] maxVoxelCount stop raycasting when this many voxels intersected (0: disable) + * \param[out] voxel_center_list results are written to this vector of PointT elements + * \param[in] max_voxel_count stop raycasting when this many voxels intersected (0: disable) * \return number of intersected voxels - */ + */ int getIntersectedVoxelCenters (Eigen::Vector3f origin, Eigen::Vector3f direction, - AlignedPointTVector &voxelCenterList, - int maxVoxelCount = 0) const; + AlignedPointTVector &voxel_center_list, int max_voxel_count = 0) const; /** \brief Get indices of all voxels that are intersected by a ray (origin, direction). * \param[in] origin ray origin * \param[in] direction ray direction vector * \param[out] k_indices resulting point indices from intersected voxels - * \param[in] maxVoxelCount stop raycasting when this many voxels intersected (0: disable) - * \return number of intersected voxels - */ + * \param[in] max_voxel_count stop raycasting when this many voxels intersected (0: disable) + * \return number of intersected voxels + */ int getIntersectedVoxelIndices (Eigen::Vector3f origin, Eigen::Vector3f direction, std::vector &k_indices, - int maxVoxelCount = 0) const; + int max_voxel_count = 0) const; /** \brief Search for points within rectangular search area @@ -265,88 +261,89 @@ namespace pcl */ class prioBranchQueueEntry { - public: - /** \brief Empty constructor */ - prioBranchQueueEntry () : node (), pointDistance (0), key () - { - } + public: + /** \brief Empty constructor */ + prioBranchQueueEntry () : + node (), point_distance (0), key () + { + } - /** \brief Constructor for initializing priority queue entry. - * \param _node pointer to octree node - * \param _key octree key addressing voxel in octree structure - * \param[in] _point_distance distance of query point to voxel center - */ - prioBranchQueueEntry (OctreeNode* _node, OctreeKey& _key, float _point_distance) : - node (_node), pointDistance (_point_distance), key (_key) - { - } + /** \brief Constructor for initializing priority queue entry. + * \param _node pointer to octree node + * \param _key octree key addressing voxel in octree structure + * \param[in] _point_distance distance of query point to voxel center + */ + prioBranchQueueEntry (OctreeNode* _node, OctreeKey& _key, float _point_distance) : + node (_node), point_distance (_point_distance), key (_key) + { + } - /** \brief Operator< for comparing priority queue entries with each other. - * \param[in] rhs the priority queue to compare this against - */ - bool - operator < (const prioBranchQueueEntry rhs) const - { - return (this->pointDistance > rhs.pointDistance); - } + /** \brief Operator< for comparing priority queue entries with each other. + * \param[in] rhs the priority queue to compare this against + */ + bool + operator < (const prioBranchQueueEntry rhs) const + { + return (this->point_distance > rhs.point_distance); + } - /** \brief Pointer to octree node. */ - const OctreeNode* node; + /** \brief Pointer to octree node. */ + const OctreeNode* node; - /** \brief Distance to query point. */ - float pointDistance; + /** \brief Distance to query point. */ + float point_distance; - /** \brief Octree key. */ - OctreeKey key; + /** \brief Octree key. */ + OctreeKey key; }; ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** \brief @b Priority queue entry for point candidates * \note This class defines priority queue entries for the nearest neighbor point candidates. * \author Julius Kammerl (julius@kammerl.de) - */ + */ class prioPointQueueEntry { - public: + public: - /** \brief Empty constructor */ - prioPointQueueEntry () : - pointIdx_ (0), pointDistance_ (0) - { - } + /** \brief Empty constructor */ + prioPointQueueEntry () : + point_idx_ (0), point_distance_ (0) + { + } - /** \brief Constructor for initializing priority queue entry. - * \param[in] pointIdx an index representing a point in the dataset given by \a setInputCloud - * \param[in] pointDistance distance of query point to voxel center - */ - prioPointQueueEntry (unsigned int& pointIdx, float pointDistance) : - pointIdx_ (pointIdx), pointDistance_ (pointDistance) - { - } + /** \brief Constructor for initializing priority queue entry. + * \param[in] point_idx an index representing a point in the dataset given by \a setInputCloud + * \param[in] point_distance distance of query point to voxel center + */ + prioPointQueueEntry (unsigned int& point_idx, float point_distance) : + point_idx_ (point_idx), point_distance_ (point_distance) + { + } - /** \brief Operator< for comparing priority queue entries with each other. - * \param[in] rhs priority queue to compare this against - */ - bool - operator< (const prioPointQueueEntry& rhs) const - { - return (this->pointDistance_ < rhs.pointDistance_); - } + /** \brief Operator< for comparing priority queue entries with each other. + * \param[in] rhs priority queue to compare this against + */ + bool + operator< (const prioPointQueueEntry& rhs) const + { + return (this->point_distance_ < rhs.point_distance_); + } - /** \brief Index representing a point in the dataset given by \a setInputCloud. */ - int pointIdx_; + /** \brief Index representing a point in the dataset given by \a setInputCloud. */ + int point_idx_; - /** \brief Distance to query point. */ - float pointDistance_; + /** \brief Distance to query point. */ + float point_distance_; }; /** \brief Helper function to calculate the squared distance between two points - * \param[in] pointA point A - * \param[in] pointB point B + * \param[in] point_a point A + * \param[in] point_b point B * \return squared distance between point A and point B */ float - pointSquaredDist (const PointT& pointA, const PointT& pointB) const; + pointSquaredDist (const PointT& point_a, const PointT& point_b) const; ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Recursive search routine methods @@ -357,7 +354,7 @@ namespace pcl * \param[in] radiusSquared squared search radius * \param[in] node current octree node to be explored * \param[in] key octree key addressing a leaf node. - * \param[in] treeDepth current depth/level in the octree + * \param[in] tree_depth current depth/level in the octree * \param[out] k_indices vector of indices found to be neighbors of query point * \param[out] k_sqr_distances squared distances of neighbors to query point * \param[in] max_nn maximum of neighbors to be found @@ -365,7 +362,7 @@ namespace pcl void getNeighborsWithinRadiusRecursive (const PointT& point, const double radiusSquared, const BranchNode* node, const OctreeKey& key, - unsigned int treeDepth, std::vector& k_indices, + unsigned int tree_depth, std::vector& k_indices, std::vector& k_sqr_distances, unsigned int max_nn) const; /** \brief Recursive search method that explores the octree and finds the K nearest neighbors @@ -373,50 +370,50 @@ namespace pcl * \param[in] K amount of nearest neighbors to be found * \param[in] node current octree node to be explored * \param[in] key octree key addressing a leaf node. - * \param[in] treeDepth current depth/level in the octree - * \param[in] squaredSearchRadius squared search radius distance - * \param[out] pointCandidates priority queue of nearest neigbor point candidates + * \param[in] tree_depth current depth/level in the octree + * \param[in] squared_search_radius squared search radius distance + * \param[out] point_candidates priority queue of nearest neigbor point candidates * \return squared search radius based on current point candidate set found */ double getKNearestNeighborRecursive (const PointT& point, unsigned int K, const BranchNode* node, - const OctreeKey& key, unsigned int treeDepth, - const double squaredSearchRadius, - std::vector& pointCandidates) const; + const OctreeKey& key, unsigned int tree_depth, + const double squared_search_radius, + std::vector& point_candidates) const; /** \brief Recursive search method that explores the octree and finds the approximate nearest neighbor * \param[in] point query point * \param[in] node current octree node to be explored * \param[in] key octree key addressing a leaf node. - * \param[in] treeDepth current depth/level in the octree + * \param[in] tree_depth current depth/level in the octree * \param[out] result_index result index is written to this reference * \param[out] sqr_distance squared distance to search */ void approxNearestSearchRecursive (const PointT& point, const BranchNode* node, const OctreeKey& key, - unsigned int treeDepth, int& result_index, float& sqr_distance); + unsigned int tree_depth, int& result_index, float& sqr_distance); /** \brief Recursively search the tree for all intersected leaf nodes and return a vector of voxel centers. * This algorithm is based off the paper An Efficient Parametric Algorithm for Octree Traversal: * http://wscg.zcu.cz/wscg2000/Papers_2000/X31.pdf - * \param[in] minX octree nodes X coordinate of lower bounding box corner - * \param[in] minY octree nodes Y coordinate of lower bounding box corner - * \param[in] minZ octree nodes Z coordinate of lower bounding box corner - * \param[in] maxX octree nodes X coordinate of upper bounding box corner - * \param[in] maxY octree nodes Y coordinate of upper bounding box corner - * \param[in] maxZ octree nodes Z coordinate of upper bounding box corner + * \param[in] min_x octree nodes X coordinate of lower bounding box corner + * \param[in] min_y octree nodes Y coordinate of lower bounding box corner + * \param[in] min_z octree nodes Z coordinate of lower bounding box corner + * \param[in] max_x octree nodes X coordinate of upper bounding box corner + * \param[in] max_y octree nodes Y coordinate of upper bounding box corner + * \param[in] max_z octree nodes Z coordinate of upper bounding box corner * \param[in] a * \param[in] node current octree node to be explored * \param[in] key octree key addressing a leaf node. - * \param[out] voxelCenterList results are written to this vector of PointT elements - * \param[in] maxVoxelCount stop raycasting when this many voxels intersected (0: disable) + * \param[out] voxel_center_list results are written to this vector of PointT elements + * \param[in] max_voxel_count stop raycasting when this many voxels intersected (0: disable) * \return number of voxels found */ int - getIntersectedVoxelCentersRecursive (double minX, double minY, double minZ, double maxX, double maxY, - double maxZ, unsigned char a, const OctreeNode* node, - const OctreeKey& key, AlignedPointTVector &voxelCenterList, - int maxVoxelCount) const; + getIntersectedVoxelCentersRecursive (double min_x, double min_y, double min_z, double max_x, double max_y, + double max_z, unsigned char a, const OctreeNode* node, + const OctreeKey& key, AlignedPointTVector &voxel_center_list, + int max_voxel_count) const; /** \brief Recursive search method that explores the octree and finds points within a rectangular search area @@ -424,51 +421,51 @@ namespace pcl * \param[in] max_pt upper corner of search area * \param[in] node current octree node to be explored * \param[in] key octree key addressing a leaf node. - * \param[in] treeDepth current depth/level in the octree + * \param[in] tree_depth current depth/level in the octree * \param[out] k_indices the resultant point indices */ void boxSearchRecursive (const Eigen::Vector3f &min_pt, const Eigen::Vector3f &max_pt, const BranchNode* node, - const OctreeKey& key, unsigned int treeDepth, std::vector& k_indices) const; + const OctreeKey& key, unsigned int tree_depth, std::vector& k_indices) const; /** \brief Recursively search the tree for all intersected leaf nodes and return a vector of indices. * This algorithm is based off the paper An Efficient Parametric Algorithm for Octree Traversal: * http://wscg.zcu.cz/wscg2000/Papers_2000/X31.pdf - * \param[in] minX octree nodes X coordinate of lower bounding box corner - * \param[in] minY octree nodes Y coordinate of lower bounding box corner - * \param[in] minZ octree nodes Z coordinate of lower bounding box corner - * \param[in] maxX octree nodes X coordinate of upper bounding box corner - * \param[in] maxY octree nodes Y coordinate of upper bounding box corner - * \param[in] maxZ octree nodes Z coordinate of upper bounding box corner + * \param[in] min_x octree nodes X coordinate of lower bounding box corner + * \param[in] min_y octree nodes Y coordinate of lower bounding box corner + * \param[in] min_z octree nodes Z coordinate of lower bounding box corner + * \param[in] max_x octree nodes X coordinate of upper bounding box corner + * \param[in] max_y octree nodes Y coordinate of upper bounding box corner + * \param[in] max_z octree nodes Z coordinate of upper bounding box corner * \param[in] a * \param[in] node current octree node to be explored * \param[in] key octree key addressing a leaf node. * \param[out] k_indices resulting indices - * \param[in] maxVoxelCount stop raycasting when this many voxels intersected (0: disable) + * \param[in] max_voxel_count stop raycasting when this many voxels intersected (0: disable) * \return number of voxels found */ int - getIntersectedVoxelIndicesRecursive (double minX, double minY, double minZ, - double maxX, double maxY, double maxZ, + getIntersectedVoxelIndicesRecursive (double min_x, double min_y, double min_z, + double max_x, double max_y, double max_z, unsigned char a, const OctreeNode* node, const OctreeKey& key, std::vector &k_indices, - int maxVoxelCount) const; + int max_voxel_count) const; /** \brief Initialize raytracing algorithm * \param origin * \param direction - * \param[in] minX octree nodes X coordinate of lower bounding box corner - * \param[in] minY octree nodes Y coordinate of lower bounding box corner - * \param[in] minZ octree nodes Z coordinate of lower bounding box corner - * \param[in] maxX octree nodes X coordinate of upper bounding box corner - * \param[in] maxY octree nodes Y coordinate of upper bounding box corner - * \param[in] maxZ octree nodes Z coordinate of upper bounding box corner + * \param[in] min_x octree nodes X coordinate of lower bounding box corner + * \param[in] min_y octree nodes Y coordinate of lower bounding box corner + * \param[in] min_z octree nodes Z coordinate of lower bounding box corner + * \param[in] max_x octree nodes X coordinate of upper bounding box corner + * \param[in] max_y octree nodes Y coordinate of upper bounding box corner + * \param[in] max_z octree nodes Z coordinate of upper bounding box corner * \param a */ inline void initIntersectedVoxel (Eigen::Vector3f &origin, Eigen::Vector3f &direction, - double &minX, double &minY, double &minZ, - double &maxX, double &maxY, double &maxZ, + double &min_x, double &min_y, double &min_z, + double &max_x, double &max_y, double &max_z, unsigned char &a) const { // Account for division by zero when direction vector is 0.0 @@ -486,79 +483,79 @@ namespace pcl // Handle negative axis direction vector if (direction.x () < 0.0) { - origin.x () = static_cast (this->minX_) + static_cast (this->maxX_) - origin.x (); + origin.x () = static_cast (this->min_x_) + static_cast (this->max_x_) - origin.x (); direction.x () = -direction.x (); a |= 4; } if (direction.y () < 0.0) { - origin.y () = static_cast (this->minY_) + static_cast (this->maxY_) - origin.y (); + origin.y () = static_cast (this->min_y_) + static_cast (this->max_y_) - origin.y (); direction.y () = -direction.y (); a |= 2; } if (direction.z () < 0.0) { - origin.z () = static_cast (this->minZ_) + static_cast (this->maxZ_) - origin.z (); + origin.z () = static_cast (this->min_z_) + static_cast (this->max_z_) - origin.z (); direction.z () = -direction.z (); a |= 1; } - minX = (this->minX_ - origin.x ()) / direction.x (); - maxX = (this->maxX_ - origin.x ()) / direction.x (); - minY = (this->minY_ - origin.y ()) / direction.y (); - maxY = (this->maxY_ - origin.y ()) / direction.y (); - minZ = (this->minZ_ - origin.z ()) / direction.z (); - maxZ = (this->maxZ_ - origin.z ()) / direction.z (); + min_x = (this->min_x_ - origin.x ()) / direction.x (); + max_x = (this->max_x_ - origin.x ()) / direction.x (); + min_y = (this->min_y_ - origin.y ()) / direction.y (); + max_y = (this->max_y_ - origin.y ()) / direction.y (); + min_z = (this->min_z_ - origin.z ()) / direction.z (); + max_z = (this->max_z_ - origin.z ()) / direction.z (); } /** \brief Find first child node ray will enter - * \param[in] minX octree nodes X coordinate of lower bounding box corner - * \param[in] minY octree nodes Y coordinate of lower bounding box corner - * \param[in] minZ octree nodes Z coordinate of lower bounding box corner - * \param[in] midX octree nodes X coordinate of bounding box mid line - * \param[in] midY octree nodes Y coordinate of bounding box mid line - * \param[in] midZ octree nodes Z coordinate of bounding box mid line + * \param[in] min_x octree nodes X coordinate of lower bounding box corner + * \param[in] min_y octree nodes Y coordinate of lower bounding box corner + * \param[in] min_z octree nodes Z coordinate of lower bounding box corner + * \param[in] mid_x octree nodes X coordinate of bounding box mid line + * \param[in] mid_y octree nodes Y coordinate of bounding box mid line + * \param[in] mid_z octree nodes Z coordinate of bounding box mid line * \return the first child node ray will enter */ inline int - getFirstIntersectedNode (double minX, double minY, double minZ, double midX, double midY, double midZ) const + getFirstIntersectedNode (double min_x, double min_y, double min_z, double mid_x, double mid_y, double mid_z) const { int currNode = 0; - if (minX > minY) + if (min_x > min_y) { - if (minX > minZ) + if (min_x > min_z) { - // max(minX, minY, minZ) is minX. Entry plane is YZ. - if (midY < minX) + // max(min_x, min_y, min_z) is min_x. Entry plane is YZ. + if (mid_y < min_x) currNode |= 2; - if (midZ < minX) + if (mid_z < min_x) currNode |= 1; } else { - // max(minX, minY, minZ) is minZ. Entry plane is XY. - if (midX < minZ) + // max(min_x, min_y, min_z) is min_z. Entry plane is XY. + if (mid_x < min_z) currNode |= 4; - if (midY < minZ) + if (mid_y < min_z) currNode |= 2; } } else { - if (minY > minZ) + if (min_y > min_z) { - // max(minX, minY, minZ) is minY. Entry plane is XZ. - if (midX < minY) + // max(min_x, min_y, min_z) is min_y. Entry plane is XZ. + if (mid_x < min_y) currNode |= 4; - if (midZ < minY) + if (mid_z < min_y) currNode |= 1; } else { - // max(minX, minY, minZ) is minZ. Entry plane is XY. - if (midX < minZ) + // max(min_x, min_y, min_z) is min_z. Entry plane is XY. + if (mid_x < min_z) currNode |= 4; - if (midY < minZ) + if (mid_y < min_z) currNode |= 2; } } diff --git a/outofcore/include/pcl/outofcore/octree_base_node.h b/outofcore/include/pcl/outofcore/octree_base_node.h index 3241a44ed2f..0ead7f79161 100644 --- a/outofcore/include/pcl/outofcore/octree_base_node.h +++ b/outofcore/include/pcl/outofcore/octree_base_node.h @@ -419,6 +419,7 @@ namespace pcl /** \brief Tests whether \ref point falls within the input bounding box * \param[in] min_bb The minimum corner of the input bounding box * \param[in] max_bb The maximum corner of the input bounding box + * \param[in] point The test point */ bool pointInBoundingBox (const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, const Eigen::Vector3d &point); @@ -426,6 +427,7 @@ namespace pcl /** \brief Tests whether \ref p falls within the input bounding box * \param[in] min_bb The minimum corner of the input bounding box * \param[in] max_bb The maximum corner of the input bounding box + * \param[in] p The point to be tested **/ static bool pointInBoundingBox (const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, const PointT &p); @@ -445,7 +447,7 @@ namespace pcl * \param[in] idx Index (0-7) of the child node */ void - createChild (const size_t idx); + createChild (const std::size_t idx); /** \brief Write JSON metadata for this node to file */ void diff --git a/registration/include/pcl/registration/gicp.h b/registration/include/pcl/registration/gicp.h index 06b45c617f2..b1e6997f44b 100644 --- a/registration/include/pcl/registration/gicp.h +++ b/registration/include/pcl/registration/gicp.h @@ -229,19 +229,19 @@ namespace pcl protected: /** \brief The number of neighbors used for covariances computation. - * \default 20 + * default: 20 */ int k_correspondences_; /** \brief The epsilon constant for gicp paper; this is NOT the convergence * tolerence - * \default 0.001 + * default: 0.001 */ double gicp_epsilon_; /** The epsilon constant for rotation error. (In GICP the transformation epsilon * is split in rotation part and translation part). - * \default 2e-3 + * default: 2e-3 */ double rotation_epsilon_; diff --git a/surface/include/pcl/surface/impl/concave_hull.hpp b/surface/include/pcl/surface/impl/concave_hull.hpp index 1bc98308d36..24b558ad5f6 100644 --- a/surface/include/pcl/surface/impl/concave_hull.hpp +++ b/surface/include/pcl/surface/impl/concave_hull.hpp @@ -56,6 +56,9 @@ #include ////////////////////////////////////////////////////////////////////////// +/** \brief Get dimension of concave hull + * \return dimension + */ template int pcl::ConcaveHull::getDim () const { From ece0a4cc8e948c929bceefe4c7b654b5b37fe30b Mon Sep 17 00:00:00 2001 From: Julius Kammerl Date: Mon, 25 Mar 2013 22:38:50 -0700 Subject: [PATCH 28/32] fixing compiler warning --- common/include/pcl/point_types_conversion.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/common/include/pcl/point_types_conversion.h b/common/include/pcl/point_types_conversion.h index dbd73efcb24..fe867f53f37 100644 --- a/common/include/pcl/point_types_conversion.h +++ b/common/include/pcl/point_types_conversion.h @@ -91,7 +91,7 @@ namespace pcl PointRGBtoI (RGB& in, Intensity32u& out) { - out.intensity = static_cast(std::numeric_limits::max() * 0.299f * static_cast (in.r) + out.intensity = static_cast(static_cast(std::numeric_limits::max()) * 0.299f * static_cast (in.r) + 0.587f * static_cast (in.g) + 0.114f * static_cast (in.b)); } From 8d696c6857dc789e34f3bdb7e45ad963409d513b Mon Sep 17 00:00:00 2001 From: "Radu B. Rusu" Date: Tue, 26 Mar 2013 00:04:44 -0700 Subject: [PATCH 29/32] fixed compilation error on GCC 4.7.2 --- octree/include/pcl/octree/impl/octree_pointcloud.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/octree/include/pcl/octree/impl/octree_pointcloud.hpp b/octree/include/pcl/octree/impl/octree_pointcloud.hpp index dfc9beaff48..4aa9680ddf2 100644 --- a/octree/include/pcl/octree/impl/octree_pointcloud.hpp +++ b/octree/include/pcl/octree/impl/octree_pointcloud.hpp @@ -571,7 +571,7 @@ pcl::octree::OctreePointCloud LeafNode* leaf_node; BranchNode* parent_branch_of_leaf_node; - unsigned int depth_mask = createLeafRecursive (key, this->depth_mask_ ,this->root_node_, leaf_node, parent_branch_of_leaf_node); + unsigned int depth_mask = this->createLeafRecursive (key, this->depth_mask_ ,this->root_node_, leaf_node, parent_branch_of_leaf_node); if (this->dynamic_depth_enabled_ && depth_mask) { @@ -588,7 +588,7 @@ pcl::octree::OctreePointCloud child_idx, depth_mask); - depth_mask = createLeafRecursive (key, this->depth_mask_ ,this->root_node_, leaf_node, parent_branch_of_leaf_node); + depth_mask = this->createLeafRecursive (key, this->depth_mask_ ,this->root_node_, leaf_node, parent_branch_of_leaf_node); leaf_obj_count = (*leaf_node)->getSize (); } From cab716517496e3f4690e188de5059de6abddda28 Mon Sep 17 00:00:00 2001 From: "Radu B. Rusu" Date: Tue, 26 Mar 2013 00:05:18 -0700 Subject: [PATCH 30/32] added a fix to deal with oscillations in SAC-based model refinement --- .../include/pcl/sample_consensus/sac.h | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/sample_consensus/include/pcl/sample_consensus/sac.h b/sample_consensus/include/pcl/sample_consensus/sac.h index 50f86ddcda4..76e4c66d10a 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac.h +++ b/sample_consensus/include/pcl/sample_consensus/sac.h @@ -176,7 +176,7 @@ namespace pcl * \param[in] max_iterations the maxim number of iterations to try to refine in case the inliers keep on changing */ virtual bool - refineModel (const double sigma = 3.0, const unsigned int max_iterations = 10000) + refineModel (const double sigma = 3.0, const unsigned int max_iterations = 1000) { if (!sac_model_) { @@ -190,10 +190,12 @@ namespace pcl unsigned int refine_iterations = 0; bool inlier_changed = false; std::vector new_inliers; + std::vector inliers_sizes; do { // Optimize the model coefficients sac_model_->optimizeModelCoefficients (inliers_, model_coefficients_, model_coefficients_); + inliers_sizes.push_back (inliers_.size ()); // Select the new inliers based on the optimized coefficients and new threshold sac_model_->selectWithinDistance (model_coefficients_, error_threshold, new_inliers); @@ -212,6 +214,16 @@ namespace pcl // If the number of inliers changed, then we are still optimizing if (new_inliers.size () != inliers_.size ()) { + // Check if the number of inliers is oscillating in between two values + if (inliers_sizes.size () >= 4) + { + if (inliers_sizes[inliers_sizes.size () - 1] == inliers_sizes[inliers_sizes.size () - 3] && + inliers_sizes[inliers_sizes.size () - 2] == inliers_sizes[inliers_sizes.size () - 4]) + { + inlier_changed = false; + break; + } + } inlier_changed = true; continue; } From 3d527faa871e34be6ca662565ebfe083d03d6c7c Mon Sep 17 00:00:00 2001 From: Julius Kammerl Date: Tue, 26 Mar 2013 00:16:46 -0700 Subject: [PATCH 31/32] fixing octree unit test --- octree/include/pcl/octree/impl/octree2buf_base.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/octree/include/pcl/octree/impl/octree2buf_base.hpp b/octree/include/pcl/octree/impl/octree2buf_base.hpp index b23cc58905e..522ddfe42af 100644 --- a/octree/include/pcl/octree/impl/octree2buf_base.hpp +++ b/octree/include/pcl/octree/impl/octree2buf_base.hpp @@ -548,7 +548,8 @@ namespace pcl // XOR of current and previous occupancy bit patterns node_XOR_bit_pattern = branch_bit_pattern_curr_buffer ^ branch_bit_pattern_prev_buffer; - if (do_XOR_encoding_arg) + + if (binary_tree_out_arg) { if (do_XOR_encoding_arg) { From 7103fbb644232efbddc7082ffeff786b407efa4c Mon Sep 17 00:00:00 2001 From: "Radu B. Rusu" Date: Tue, 26 Mar 2013 13:37:09 -0700 Subject: [PATCH 32/32] improved the SAC model refinement and fixed a bug where if the number of inliers refined would go to 0, the original number of inliers would be overwritten --- ...pondence_rejection_sample_consensus_2d.hpp | 2 +- .../include/pcl/sample_consensus/sac.h | 50 ++++++++++++++----- 2 files changed, 38 insertions(+), 14 deletions(-) diff --git a/registration/include/pcl/registration/impl/correspondence_rejection_sample_consensus_2d.hpp b/registration/include/pcl/registration/impl/correspondence_rejection_sample_consensus_2d.hpp index f576bd8e59b..97fb2c96e7d 100644 --- a/registration/include/pcl/registration/impl/correspondence_rejection_sample_consensus_2d.hpp +++ b/registration/include/pcl/registration/impl/correspondence_rejection_sample_consensus_2d.hpp @@ -101,7 +101,7 @@ pcl::registration::CorrespondenceRejectorSampleConsensus2D::getRemaining else { if (refine_ && !sac.refineModel (2.0)) - PCL_WARN ("[pcl::registration::%s::getRemainingCorrespondences] Error refining model! Leaving correspondences as they are...\n", getClassName ().c_str ()); + PCL_WARN ("[pcl::registration::%s::getRemainingCorrespondences] Error refining model!\n", getClassName ().c_str ()); std::vector inliers; sac.getInliers (inliers); diff --git a/sample_consensus/include/pcl/sample_consensus/sac.h b/sample_consensus/include/pcl/sample_consensus/sac.h index 76e4c66d10a..d6742e7ad20 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac.h +++ b/sample_consensus/include/pcl/sample_consensus/sac.h @@ -188,21 +188,28 @@ namespace pcl error_threshold = threshold_; double sigma_sqr = sigma * sigma; unsigned int refine_iterations = 0; - bool inlier_changed = false; - std::vector new_inliers; + bool inlier_changed = false, oscillating = false; + std::vector new_inliers, prev_inliers = inliers_; std::vector inliers_sizes; + Eigen::VectorXf new_model_coefficients = model_coefficients_; do { // Optimize the model coefficients - sac_model_->optimizeModelCoefficients (inliers_, model_coefficients_, model_coefficients_); - inliers_sizes.push_back (inliers_.size ()); + sac_model_->optimizeModelCoefficients (prev_inliers, new_model_coefficients, new_model_coefficients); + inliers_sizes.push_back (prev_inliers.size ()); // Select the new inliers based on the optimized coefficients and new threshold - sac_model_->selectWithinDistance (model_coefficients_, error_threshold, new_inliers); - PCL_DEBUG ("[pcl::SampleConsensus::refineModel] Number of inliers found (before/after): %zu/%zu\n", inliers_.size (), new_inliers.size ()); + sac_model_->selectWithinDistance (new_model_coefficients, error_threshold, new_inliers); + PCL_DEBUG ("[pcl::SampleConsensus::refineModel] Number of inliers found (before/after): %zu/%zu, with an error threshold of %g.\n", prev_inliers.size (), new_inliers.size (), error_threshold); if (new_inliers.empty ()) - return (false); + { + refine_iterations++; + if (refine_iterations >= max_iterations) + break; + continue; + //return (false); + } // Estimate the variance and the new threshold double variance = sac_model_->computeVariance (); @@ -210,9 +217,9 @@ namespace pcl PCL_DEBUG ("[pcl::SampleConsensus::refineModel] New estimated error threshold: %g on iteration %d out of %d.\n", error_threshold, refine_iterations, max_iterations); inlier_changed = false; - std::swap (inliers_, new_inliers); + std::swap (prev_inliers, new_inliers); // If the number of inliers changed, then we are still optimizing - if (new_inliers.size () != inliers_.size ()) + if (new_inliers.size () != prev_inliers.size ()) { // Check if the number of inliers is oscillating in between two values if (inliers_sizes.size () >= 4) @@ -220,7 +227,7 @@ namespace pcl if (inliers_sizes[inliers_sizes.size () - 1] == inliers_sizes[inliers_sizes.size () - 3] && inliers_sizes[inliers_sizes.size () - 2] == inliers_sizes[inliers_sizes.size () - 4]) { - inlier_changed = false; + oscillating = true; break; } } @@ -229,10 +236,10 @@ namespace pcl } // Check the values of the inlier set - for (size_t i = 0; i < inliers_.size (); ++i) + for (size_t i = 0; i < prev_inliers.size (); ++i) { // If the value of the inliers changed, then we are still optimizing - if (inliers_[i] != new_inliers[i]) + if (prev_inliers[i] != new_inliers[i]) { inlier_changed = true; break; @@ -240,10 +247,27 @@ namespace pcl } } while (inlier_changed && ++refine_iterations < max_iterations); - + + // If the new set of inliers is empty, we didn't do a good job refining + if (new_inliers.empty ()) + { + PCL_ERROR ("[pcl::SampleConsensus::refineModel] Refinement failed: got an empty set of inliers!\n"); + return (false); + } + + if (oscillating) + { + PCL_DEBUG ("[pcl::SampleConsensus::refineModel] Detected oscillations in the model refinement.\n"); + return (true); + } + // If no inliers have been changed anymore, then the refinement was successful if (!inlier_changed) + { + std::swap (inliers_, new_inliers); + model_coefficients_ = new_model_coefficients; return (true); + } return (false); }