From: Jochen Sprickerhof Date: Fri, 10 Apr 2020 20:07:59 +0000 (+0200) Subject: New upstream version 1.10.1+dfsg X-Git-Tag: archive/raspbian/1.14.0+dfsg-2+rpi1^2~10^2~6 X-Git-Url: https://dgit.raspbian.org/?a=commitdiff_plain;h=d0a7ef075ae569d3355d4b5fe9b7d9b256ea2fe7;p=pcl.git New upstream version 1.10.1+dfsg --- diff --git a/.ci/azure-pipelines/build-macos.yml b/.ci/azure-pipelines/build-macos.yml index 94a3ec4a..cba91569 100644 --- a/.ci/azure-pipelines/build-macos.yml +++ b/.ci/azure-pipelines/build-macos.yml @@ -1,9 +1,14 @@ jobs: - job: osx - displayName: macOS High Sierra + strategy: + matrix: + macOS Catalina: + VMIMAGE: 'macOS-10.15' + macOS Mojave: + VMIMAGE: 'macOS-10.14' timeoutInMinutes: 0 pool: - vmImage: 'macOS-10.13' + vmImage: '$(VMIMAGE)' variables: BUILD_DIR: '$(Agent.WorkFolder)/build' GOOGLE_TEST_DIR: '$(Agent.WorkFolder)/googletest' @@ -19,6 +24,7 @@ jobs: - script: | mkdir $BUILD_DIR && cd $BUILD_DIR cmake $(Build.SourcesDirectory) \ + -DCMAKE_OSX_SYSROOT="/Applications/Xcode.app/Contents/Developer/Platforms/MacOSX.platform/Developer/SDKs/MacOSX.sdk" \ -DCMAKE_CXX_FLAGS="$CMAKE_CXX_FLAGS" \ -DGTEST_SRC_DIR="$GOOGLE_TEST_DIR/googletest" \ -DGTEST_INCLUDE_DIR="$GOOGLE_TEST_DIR/googletest/include" \ @@ -46,7 +52,7 @@ jobs: cmake --build . -- test_filters test_registration test_registration_api cmake --build . -- -j2 displayName: 'Build Library' - - script: cd $BUILD_DIR/test && ctest -V -T Test + - script: cd $BUILD_DIR && cmake --build . -- tests displayName: 'Run Unit Tests' - task: PublishTestResults@2 inputs: diff --git a/.ci/azure-pipelines/build-ubuntu-16-04.yaml b/.ci/azure-pipelines/build-ubuntu-16-04.yaml index 74d76fcc..837a8eab 100644 --- a/.ci/azure-pipelines/build-ubuntu-16-04.yaml +++ b/.ci/azure-pipelines/build-ubuntu-16-04.yaml @@ -41,8 +41,7 @@ jobs: cmake --build . -- -j2 displayName: 'Build Library' - script: | - cd $BUILD_DIR/test - ctest -V -T Test + cd $BUILD_DIR && cmake --build . -- tests displayName: 'Run Unit Tests' - task: PublishTestResults@2 inputs: diff --git a/.ci/azure-pipelines/build-ubuntu-19-10.yaml b/.ci/azure-pipelines/build-ubuntu-19-10.yaml index bb1b70f3..0dd77e05 100644 --- a/.ci/azure-pipelines/build-ubuntu-19-10.yaml +++ b/.ci/azure-pipelines/build-ubuntu-19-10.yaml @@ -42,8 +42,7 @@ jobs: cmake --build . -- -j2 displayName: 'Build Library' - script: | - cd $BUILD_DIR/test - ctest -V -T Test + cd $BUILD_DIR && cmake --build . -- tests displayName: 'Run Unit Tests' - task: PublishTestResults@2 inputs: diff --git a/.ci/azure-pipelines/build-windows.yml b/.ci/azure-pipelines/build-windows.yml index b55b1214..7bfaa40a 100644 --- a/.ci/azure-pipelines/build-windows.yml +++ b/.ci/azure-pipelines/build-windows.yml @@ -37,7 +37,7 @@ jobs: displayName: 'CMake Configuration' - script: cd %BUILD_DIR% && cmake --build . --config %CONFIGURATION% displayName: 'Build Library' - - script: cd %BUILD_DIR%/test && ctest -C %CONFIGURATION% -V -T Test + - script: cd %BUILD_DIR% && cmake --build . --target tests --config %CONFIGURATION% displayName: 'Run Unit Tests' - task: PublishTestResults@2 inputs: diff --git a/.ci/azure-pipelines/tutorials.yml b/.ci/azure-pipelines/tutorials.yml index afabea93..3b675666 100644 --- a/.ci/azure-pipelines/tutorials.yml +++ b/.ci/azure-pipelines/tutorials.yml @@ -40,5 +40,5 @@ jobs: cmake --build . -- install displayName: 'Install PCL' - script: | - $(Build.SourcesDirectory)/.ci/scripts/build_tutorials.sh -k -s -e $EXCLUDE_TUTORIALS $INSTALL_DIR/share/pcl-1.9 $(Build.SourcesDirectory) $BUILD_DIR/tutorials + $(Build.SourcesDirectory)/.ci/scripts/build_tutorials.sh -k -s -e $EXCLUDE_TUTORIALS $INSTALL_DIR/share/pcl-1.10 $(Build.SourcesDirectory) $BUILD_DIR/tutorials displayName: 'Build Tutorials' diff --git a/.dev/docker/env/Dockerfile b/.dev/docker/env/Dockerfile index c54c97cd..b133937d 100644 --- a/.dev/docker/env/Dockerfile +++ b/.dev/docker/env/Dockerfile @@ -1,5 +1,15 @@ -ARG UBUNTU_DISTRO=16.04 -FROM ubuntu:${UBUNTU_DISTRO} +# For valid combinations, check the following repo: +# https://gitlab.com/nvidia/container-images/cuda/tree/master/dist +# To enable cuda, use "--build-arg USE_CUDA=true" during image build process +ARG USE_CUDA +ARG CUDA_VERSION="9.0" +ARG UBUNTU_DISTRO="16.04" +# Known conflicts: +# * 9.1 is an issue with 16.04 for Eigen +ARG BASE_CUDA_IMAGE=${USE_CUDA:+"nvidia/cuda:${CUDA_VERSION}-devel-ubuntu${UBUNTU_DISTRO}"} +ARG BASE_IMAGE=${BASE_CUDA_IMAGE:-"ubuntu:${UBUNTU_DISTRO}"} + +FROM ${BASE_IMAGE} ARG VTK_VERSION=6 ENV DEBIAN_FRONTEND=noninteractive diff --git a/.dev/format.sh b/.dev/format.sh index 769a2b9c..7b7c1d13 100755 --- a/.dev/format.sh +++ b/.dev/format.sh @@ -8,7 +8,7 @@ format() { # don't use a directory with whitespace - local whitelist="2d ml simulation stereo" + local whitelist="2d ml octree simulation stereo" local PCL_DIR="${2}" local formatter="${1}" diff --git a/.github/ISSUE_TEMPLATE/bug-report.md b/.github/ISSUE_TEMPLATE/bug-report.md new file mode 100644 index 00000000..8bad30c5 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/bug-report.md @@ -0,0 +1,49 @@ +--- +name: Bug report +about: Create a report to help us improve PCL +title: "[module_name] Provide a general summary of the issue" +labels: 'status: triage, kind: bug' +assignees: '' + +--- + + +**Describe the bug** + +A clear and concise description of what the bug is. + +**Context** + +What are you trying to accomplish? Providing context helps us come up with a solution that is most useful in the real world + +**Expected behavior** + +A clear and concise description of what you expected to happen. + +**Current Behavior** + +What happens instead of the expected behavior? + +**To Reproduce** + +Provide a link to a live example, or an unambiguous set of steps to reproduce this bug. A reproducible example helps to provide faster answers. + +**Screenshots/Code snippets** + +In order to help explain your problem, please consider adding +* screenshots of the GUI issues +* code snippets: [syntax for code](https://github.com/adam-p/markdown-here/wiki/Markdown-Cheatsheet#code) with correct language highlights + +**Your Environment (please complete the following information):** + + - OS: [e.g. Ubuntu 16.04] + - Compiler: [:eg GCC 8.1] + - PCL Version [e.g. 1.10, HEAD] + +**Possible Solution** + +Not obligatory, but suggest a fix/reason for the bug. Feel free to create a PR if you feel comfortable. + +**Additional context** + +Add any other context about the problem here. diff --git a/.github/ISSUE_TEMPLATE/compilation-failure.md b/.github/ISSUE_TEMPLATE/compilation-failure.md new file mode 100644 index 00000000..17d00466 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/compilation-failure.md @@ -0,0 +1,57 @@ +--- +name: Compilation failure +about: Help! My code doesn't compile (but it should) +title: '[compile error] "Please add a short description here"' +labels: 'status: triage, kind: compile error' +assignees: '' + +--- + + +**Describe the error** + +Please paste the compilation results/errors. + +**To Reproduce** + +Provide a link to a live example, or an unambiguous set of steps to reproduce this bug. A reproducible example helps to provide faster answers. Custom OS, custom PCL configs or custom build systems make the issue difficult to reproduce. +* Best reproducability: A docker image + code snippets provided here +* Good reproducability: Common Linux OS + default PCL config + code snippets provided here +* Poor reproducability: code snippets + +Remember to reproduce the error in a clean rebuild (removing all build objects and starting build from scratch) + +**Screenshots/Code snippets/Build information** + +In order to help explain your problem, please consider adding +* screenshots of the GUI issues +* code snippets: [syntax for code](https://github.com/adam-p/markdown-here/wiki/Markdown-Cheatsheet#code) with correct language highlights +* debugging information: + * the output of cmake using `cmake --debug` + * the failing build command(s) using `VERBOSE=1 make`, `ninja -v` or similar + * steps taken to narrow down the problem + +**Your Environment (please complete the following information):** + + - OS: [e.g. Ubuntu 16.04] + - Compiler: [:eg GCC 8.1] + - PCL Version [e.g. 1.10, HEAD] (NB: please ensure you don't have multiple versions available) + - PCL Type: [Installed/Compiled from source] + +If PCL was compiled from source or failure in compiling PCL itself: + - GPU, Kinfu, CUDA enabled? Yes/No + - List and Version of dependencies used + - Compilation flags are used + +If compiling against PCL: + - Checked `CMakeLists.txt` for simple errors like missing `find_package` or `target_link_libraries`? + * [CMakeLists.txt for PCL >= 1.9](https://github.com/kunaltyagi/pcl-cmake-minimum/blob/master/CMakeLists.txt) + * [CMakeLists.txt for older versions](https://github.com/PointCloudLibrary/pcl/blob/update-issue-templates/doc/tutorials/content/sources/concatenate_clouds/CMakeLists.txt) + +**Possible Solution** + +Not obligatory, but suggest a fix/reason for the bug. Feel free to create a PR if you feel comfortable. + +**Additional context** + +Add any other context about the problem here. diff --git a/.github/ISSUE_TEMPLATE/feature_request.md b/.github/ISSUE_TEMPLATE/feature_request.md new file mode 100644 index 00000000..da676549 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/feature_request.md @@ -0,0 +1,38 @@ +--- +name: Feature request +about: Suggest an idea for PCL +title: '[module_name] "Please insert a short description of the feature request"' +labels: 'status: triage, kind: request' +assignees: '' + +--- + + +**Is your feature request related to a problem? Please describe.** + +A clear and concise description of what the problem is. Ex. I'm always frustrated when [...]. + +**Context** + +How has the lack of this feature this issue affected you? What are you trying to accomplish? +Providing context helps us come up with a solution that is most useful in the real world + +**Expected behavior** + +Please tell us how the situation should be instead + +**Current Behavior** + +Please explain the difference from current behavior + +**Describe the solution you'd like** + +A clear and concise description of what you want to happen. Not obligatory, but feel free to suggest ideas on how to implement the addition or change + +**Describe alternatives you've considered** + +A clear and concise description of any alternative solutions or features you've considered. + +**Additional context** + +Add any other context or screenshots about the feature request here. diff --git a/.github/ISSUE_TEMPLATE/something-else.md b/.github/ISSUE_TEMPLATE/something-else.md new file mode 100644 index 00000000..10c40f37 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/something-else.md @@ -0,0 +1,10 @@ +--- +name: Something else +about: It's none of the above +title: "[custom] Provide a general summary of the issue" +labels: 'status: triage' +assignees: '' + +--- + + diff --git a/.github/issue_template.md b/.github/issue_template.md deleted file mode 100644 index a07f5952..00000000 --- a/.github/issue_template.md +++ /dev/null @@ -1,29 +0,0 @@ - - - - -## Your Environment - -* Operating System and version: -* Compiler: -* PCL Version: - -## Context - - - -## Expected Behavior - - - -## Current Behavior - - - -## Code to Reproduce - - - -## Possible Solution - - diff --git a/.github/stale.yml b/.github/stale.yml index 595c3ad5..e5004002 100644 --- a/.github/stale.yml +++ b/.github/stale.yml @@ -1,28 +1,52 @@ # Configuration for probot-stale - https://github.com/probot/stale # Number of days of inactivity before an Issue or Pull Request becomes stale -daysUntilStale: 60 -# Number of days of inactivity before a stale Issue or Pull Request is closed -# False means never close -daysUntilClose: false +daysUntilStale: 30 + +# Number of days of inactivity before an Issue or Pull Request with the stale label is closed. +# Set to false to disable. If disabled, issues still need to be closed manually, but will remain marked as stale. +daysUntilClose: 7 + +# Only issues or pull requests with all of these labels are check if stale. Defaults to `[]` (disabled) +onlyLabels: + - "needs: author reply" + - "needs: more work" + # Issues or Pull Requests with these labels will never be considered stale. Set to `[]` to disable -exemptLabels: - - "status: needs review" - - "status: needs testing" - - "status: needs decision" - - "status: ready to merge" +exemptLabels: [] + +# Set to true to ignore issues in a project (defaults to false) +exemptProjects: false + +# Set to true to ignore issues in a milestone (defaults to false) +exemptMilestones: false + +# Set to true to ignore issues with an assignee (defaults to false) +exemptAssignees: false + # Label to use when marking as stale staleLabel: "status: stale" + # Comment to post when marking as stale. Set to `false` to disable -markComment: | - This pull request has been automatically marked as stale because it hasn't had - any activity in the past 60 days. Commenting or adding a new commit to the - pull request will revert this. - - Come back whenever you have time. We look forward to your contribution. -# Comment to post when removing the stale label. Set to `false` to disable -unmarkComment: false -# Comment to post when closing a stale Issue or Pull Request. Set to `false` to disable -closeComment: false +markComment: > + This issue has been automatically marked as stale because it has not had any activity in the past month. It will be closed if no further activity occurs. + +# Comment to post when removing the stale label. +# unmarkComment: > +# Your comment here. + +# Comment to post when closing a stale Issue or Pull Request. +# closeComment: > +# Your comment here. + +# Limit the number of actions per hour, from 1-30. Default is 30 +limitPerRun: 30 + # Limit to only `issues` or `pulls` -only: pulls +# only: issues + +# Configuration settings that are specific to just 'pulls': +pulls: + daysUntilClose: false + markComment: > + This pull request has been automatically marked as stale because it has not had any activity in the past month. Commenting or adding a new commit to the pull request will revert this. diff --git a/CHANGES.md b/CHANGES.md index 1700eee6..af5ec2aa 100644 --- a/CHANGES.md +++ b/CHANGES.md @@ -1,5 +1,94 @@ # ChangeList +## *= 1.10.1 (18.03.2020) =* + +### Notable changes + +**Deprecation** *of public APIs, scheduled to be removed after two minor releases* + +* **[common]** Deprecate several `PointWithViewpoint` ctors; make ctors more uniform in PCL point types [[#3597](https://github.com/PointCloudLibrary/pcl/pull/3597)] + +**Removal** *of the public APIs deprecated in previous releases* + +* **[common]** Remove deprecated checks for `USE_ROS` macro [[#3690](https://github.com/PointCloudLibrary/pcl/pull/3690)] + +**Behavior changes** *in classes, apps, or tools* + +* **[tools]** Continue on PCD load failure in `pcl_train_linemod_template` [[#3652](https://github.com/PointCloudLibrary/pcl/pull/3652)] + +### Changes grouped by module + +#### CMake: + +* Fix CMake grouping of tools targets [[#3709](https://github.com/PointCloudLibrary/pcl/pull/3709)] +* Enable `PCL_ONLY_CORE_POINT_TYPES` in mingw builds [[#3694](https://github.com/PointCloudLibrary/pcl/pull/3694)] +* Downgrade RSSDK2 warning message to status [[#3683](https://github.com/PointCloudLibrary/pcl/pull/3683)] +* Fix test targets arguments for MSVC [[#3636](https://github.com/PointCloudLibrary/pcl/pull/3636)] +* Remove duplicate `/bigobj` for MSVC [[#3604](https://github.com/PointCloudLibrary/pcl/pull/3604)] + +#### libpcl_common: + +* Better PointType ctor and reduced warnings in `register_point_struct.h` [[#3732](https://github.com/PointCloudLibrary/pcl/pull/3732)] +* **[removal]** Remove deprecated checks for `USE_ROS` macro [[#3690](https://github.com/PointCloudLibrary/pcl/pull/3690)] +* Replace `dirent` with `boost::filesystem` [[#3676](https://github.com/PointCloudLibrary/pcl/pull/3676)] +* Fix code accidentally casting away const-ness [[#3648](https://github.com/PointCloudLibrary/pcl/pull/3648)] +* Fix compilation of CUDA code on Windows [[#3634](https://github.com/PointCloudLibrary/pcl/pull/3634)] +* Remove undefined behavior and add stricter checks in console arg parsing [[#3613](https://github.com/PointCloudLibrary/pcl/pull/3613)] +* **[deprecation]** Deprecate several `PointWithViewpoint` ctors; make ctors more uniform in PCL point types [[#3597](https://github.com/PointCloudLibrary/pcl/pull/3597)] + +#### libpcl_cuda: + +* Fix memory leaks in CUDA apps [[#3587](https://github.com/PointCloudLibrary/pcl/pull/3587)] +* Fix compilation of CUDA/GPU modules [[#3576](https://github.com/PointCloudLibrary/pcl/pull/3576)] + +#### libpcl_features: + +* Add precompiled `computeApproximateCovariances`; fix compilation error for the same [[#3711](https://github.com/PointCloudLibrary/pcl/pull/3711)] +* Fix vector initialization in `NormalEstimationOMP` [[#3614](https://github.com/PointCloudLibrary/pcl/pull/3614)] +* Fix indexing bug in `IntegralImageNormalEstimation` [[#3574](https://github.com/PointCloudLibrary/pcl/pull/3574)] + +#### libpcl_filters: + +* Set `is_dense` based on actual cloud contents in `removeNaNNormalsFromPointCloud()` [[#3685](https://github.com/PointCloudLibrary/pcl/pull/3685)] + +#### libpcl_gpu: + +* Fix compile error in KinFuLS `initRegistration` [[#3737](https://github.com/PointCloudLibrary/pcl/pull/3737)] +* Fix illegal memory acces in CUDA Octree builder [[#3627](https://github.com/PointCloudLibrary/pcl/pull/3627)] +* Fix compile issue in `people_app` [[#3618](https://github.com/PointCloudLibrary/pcl/pull/3618)] +* Fix compilation of CUDA/GPU modules [[#3576](https://github.com/PointCloudLibrary/pcl/pull/3576)] + +#### libpcl_io: + +* Fix `if/ifdef` WIN32 issues [[#3668](https://github.com/PointCloudLibrary/pcl/pull/3668)] +* Add `Grabber::toggle()` method [[#3615](https://github.com/PointCloudLibrary/pcl/pull/3615)] +* Close the correct file in `pcl::io::savePLYFileBinary` [[#3601](https://github.com/PointCloudLibrary/pcl/pull/3601)] +* Fix entropy range encoding in octree-based pointcloud compression [[#3579](https://github.com/PointCloudLibrary/pcl/pull/3579)] + +#### libpcl_surface: + +* Add default initialization of grid resolution in `MarchingCubes` [[#3718](https://github.com/PointCloudLibrary/pcl/pull/3718)] + +#### PCL Apps: + +* Fix `if/ifdef` WIN32 issues [[#3668](https://github.com/PointCloudLibrary/pcl/pull/3668)] + +#### PCL Docs: + +* Fix missing standard includes, reduce warnings in doxygen-enabled builds [[#3755](https://github.com/PointCloudLibrary/pcl/pull/3755)] + +#### PCL Tutorials: + +* Fix documentation for point cloud stream compression executable name [[#3693](https://github.com/PointCloudLibrary/pcl/pull/3693)] +* Fix segfault in NARF keypoint extraction tutorial [[#3623](https://github.com/PointCloudLibrary/pcl/pull/3623)] + +#### PCL Tools: + +* Use linemod member method to save templates [[#3691](https://github.com/PointCloudLibrary/pcl/pull/3691)] +* **[behavior change]** Continue on PCD load failure in `pcl_train_linemod_template` [[#3652](https://github.com/PointCloudLibrary/pcl/pull/3652)] +* Warn user about unorganized PCDs in `pcl_train_linemod_template` [[#3644](https://github.com/PointCloudLibrary/pcl/pull/3644)] + + ## *= 1.10.0 (19.01.2020) =* Starting with PCL 1.10, to ensure compatibility with future PCL releases, please diff --git a/CMakeLists.txt b/CMakeLists.txt index 93a64487..f0bc2396 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -26,7 +26,7 @@ if("${CMAKE_BUILD_TYPE}" STREQUAL "") set(CMAKE_BUILD_TYPE RelWithDebInfo CACHE STRING "build type default to RelWithDebInfo, set to Release to improve performance" FORCE) endif() -project(PCL VERSION 1.10.0) +project(PCL VERSION 1.10.1) string(TOLOWER ${PROJECT_NAME} PROJECT_NAME_LOWER) ### ---[ Find universal dependencies @@ -35,10 +35,6 @@ set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake/Modules/" ${CMAKE_MODUL # ---[ Include pkgconfig include(FindPkgConfig) -# ---[ Release/Debug specific flags -if(CMAKE_BUILD_TYPE STREQUAL "Release" OR CMAKE_BUILD_TYPE STREQUAL "RelWithDebInfo") - add_definitions("-DBOOST_DISABLE_ASSERTS -DEIGEN_NO_DEBUG") -endif() if(WIN32 AND NOT MINGW) set(CMAKE_DEBUG_POSTFIX "d" CACHE STRING "Add postfix to target for Debug build.") set(CMAKE_RELEASE_POSTFIX "" CACHE STRING "Add postfix to target for Release build.") @@ -80,6 +76,8 @@ elseif(__COMPILER_PATHSCALE) set(CMAKE_COMPILER_IS_PATHSCALE 1) elseif(MSVC) set(CMAKE_COMPILER_IS_MSVC 1) +elseif(MINGW) + set(CMAKE_COMPILER_IS_MINGW 1) endif() # Create a variable with expected default CXX flags @@ -144,10 +142,12 @@ if(CMAKE_COMPILER_IS_GNUCXX) endif() if(CMAKE_COMPILER_IS_MSVC) - add_definitions("-DBOOST_ALL_NO_LIB -D_SCL_SECURE_NO_WARNINGS -D_CRT_SECURE_NO_WARNINGS -DNOMINMAX -DPCL_ONLY_CORE_POINT_TYPES /bigobj ${SSE_DEFINITIONS}") - + add_definitions("-DBOOST_ALL_NO_LIB -D_SCL_SECURE_NO_WARNINGS -D_CRT_SECURE_NO_WARNINGS -DNOMINMAX -DPCL_ONLY_CORE_POINT_TYPES ${SSE_DEFINITIONS}") + + add_compile_options(/bigobj) + if("${CMAKE_CXX_FLAGS}" STREQUAL "${CMAKE_CXX_FLAGS_DEFAULT}") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /bigobj /fp:precise /wd4800 /wd4521 /wd4251 /wd4275 /wd4305 /wd4355 ${SSE_FLAGS_STR}") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /fp:precise /wd4800 /wd4521 /wd4251 /wd4275 /wd4305 /wd4355 ${SSE_FLAGS_STR}") # Add extra code generation/link optimizations if(CMAKE_MSVC_CODE_LINK_OPTIMIZATION) @@ -213,6 +213,10 @@ if(CMAKE_COMPILER_IS_CLANG) set(CLANG_LIBRARIES "stdc++") endif() +if(CMAKE_COMPILER_IS_MINGW) + add_definitions(-DPCL_ONLY_CORE_POINT_TYPES) +endif() + include("${PCL_SOURCE_DIR}/cmake/pcl_utils.cmake") DISSECT_VERSION() GET_OS_INFO() @@ -435,17 +439,14 @@ if(WITH_VTK AND NOT ANDROID) elseif(${VTK_RENDERING_BACKEND} STREQUAL "OpenGL2") set(VTK_RENDERING_BACKEND_OPENGL_VERSION "2") endif() - set(HAVE_VTK ON) else() set(VTK_FOUND OFF) - set(HAVE_VTK OFF) message("Warning: You are to build PCL in STATIC but VTK is SHARED!") message("Warning: VTK disabled!") endif() endif() else() set(VTK_FOUND OFF) - set(HAVE_VTK OFF) endif() diff --git a/README.md b/README.md index 55e75e19..54b0cec4 100644 --- a/README.md +++ b/README.md @@ -5,7 +5,7 @@ [![Release][release-image]][releases] [![License][license-image]][license] -[release-image]: https://img.shields.io/badge/release-1.10.0-green.svg?style=flat +[release-image]: https://img.shields.io/badge/release-1.10.1-green.svg?style=flat [releases]: https://github.com/PointCloudLibrary/pcl/releases [license-image]: https://img.shields.io/badge/license-BSD-green.svg?style=flat @@ -63,7 +63,7 @@ Please refer to the platform specific tutorials: Documentation ------------- - [Tutorials](http://www.pointclouds.org/documentation/tutorials/) -- [PCL trunk documentation](http://docs.pointclouds.org/trunk/) (updated daily) +- [PCL trunk documentation](https://pointcloudlibrary.github.io/documentation/) (updated nightly) Contributing ------------ diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/impl/global_nn_classifier.hpp b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/impl/global_nn_classifier.hpp index ba45fdec..c5002167 100644 --- a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/impl/global_nn_classifier.hpp +++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/impl/global_nn_classifier.hpp @@ -106,14 +106,12 @@ template class Distance, typename PointInT, typename FeatureT> nearestKSearch (flann_index_, histogram, NN_, indices, distances); //gather NN-search results - double score = 0; for (int i = 0; i < NN_; ++i) { - score = distances[0][i]; index_score is; is.idx_models_ = indices[0][i]; is.idx_input_ = static_cast (idx); - is.score_ = score; + is.score_ = distances[0][i]; indices_scores.push_back (is); } } diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/impl/global_nn_recognizer_crh.hpp b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/impl/global_nn_recognizer_crh.hpp index 867e2031..f44ea72b 100644 --- a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/impl/global_nn_recognizer_crh.hpp +++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/impl/global_nn_recognizer_crh.hpp @@ -203,14 +203,12 @@ template class Distance, typename PointInT, typename FeatureT> nearestKSearch (flann_index_, histogram, NN_, indices, distances); //gather NN-search results - double score = 0; for (int i = 0; i < NN_; ++i) { - score = distances[0][i]; index_score is; is.idx_models_ = indices[0][i]; is.idx_input_ = static_cast (idx); - is.score_ = score; + is.score_ = distances[0][i]; indices_scores.push_back (is); } } diff --git a/apps/cloud_composer/src/cloud_browser.cpp b/apps/cloud_composer/src/cloud_browser.cpp index a5ac2de9..c11a5311 100644 --- a/apps/cloud_composer/src/cloud_browser.cpp +++ b/apps/cloud_composer/src/cloud_browser.cpp @@ -32,7 +32,6 @@ pcl::cloud_composer::BackgroundDelegate::paint (QPainter *painter, const QStyleO if (text_color_variant.canConvert ()) { QColor text_color = text_color_variant.value (); - QPalette palette = option.palette; QStyleOptionViewItem option_copy = option; option_copy.palette.setColor (QPalette::HighlightedText, text_color); QStyledItemDelegate::paint (painter, option_copy, index); diff --git a/apps/cloud_composer/src/cloud_composer.cpp b/apps/cloud_composer/src/cloud_composer.cpp index 5b16af83..8529a667 100644 --- a/apps/cloud_composer/src/cloud_composer.cpp +++ b/apps/cloud_composer/src/cloud_composer.cpp @@ -210,13 +210,13 @@ pcl::cloud_composer::ComposerMainWindow::initializePlugins () { QDir plugin_dir = QCoreApplication::applicationDirPath (); qDebug() << plugin_dir.path ()<< " "<invisibleRootItem(); else parent_item = model_->itemFromIndex (parent); - QString project_name = model_->getName (); for (int row = start; row <= end; ++row) { QStandardItem* new_item = parent_item->child(row); @@ -129,7 +128,6 @@ pcl::cloud_composer::CloudView::rowsAboutToBeRemoved (const QModelIndex& parent, parent_item = model_->invisibleRootItem(); else parent_item = model_->itemFromIndex (parent); - QString project_name = model_->getName (); //qDebug () << "Rows about to be removed, parent = "<text ()<<" start="< Debugging -> Command arguments\"") + SET (ArgumentWarningShown TRUE PARENT_SCOPE) + else() + #Only add if there are arguments to test + if(PCL_ADD_TEST_ARGUMENTS) + string (REPLACE ";" " " PCL_ADD_TEST_ARGUMENTS_STR "${PCL_ADD_TEST_ARGUMENTS}") + set_target_properties(${_exename} PROPERTIES VS_DEBUGGER_COMMAND_ARGUMENTS ${PCL_ADD_TEST_ARGUMENTS_STR}) + endif() + endif() + endif() + set_target_properties(${_exename} PROPERTIES FOLDER "Tests") add_test(NAME ${_name} COMMAND ${_exename} ${PCL_ADD_TEST_ARGUMENTS}) diff --git a/common/include/pcl/PCLHeader.h b/common/include/pcl/PCLHeader.h index 20e28ed9..8a692d77 100644 --- a/common/include/pcl/PCLHeader.h +++ b/common/include/pcl/PCLHeader.h @@ -1,14 +1,9 @@ #pragma once -#ifdef USE_ROS - #error USE_ROS setup requires PCL to compile against ROS message headers, which is now deprecated -#endif - -#include -#include -#include -#include -#include +#include // for string +#include // for ostream + +#include // for shared_ptr namespace pcl { diff --git a/common/include/pcl/PCLImage.h b/common/include/pcl/PCLImage.h index a5d128c0..1e8886ca 100644 --- a/common/include/pcl/PCLImage.h +++ b/common/include/pcl/PCLImage.h @@ -1,15 +1,10 @@ #pragma once -#include -#include -#include +#include // for string +#include // for vector +#include // for ostream -#ifdef USE_ROS - #error USE_ROS setup requires PCL to compile against ROS message headers, which is now deprecated -#endif - -// Include the correct Header path here -#include +#include // for PCLHeader namespace pcl { diff --git a/common/include/pcl/PCLPointCloud2.h b/common/include/pcl/PCLPointCloud2.h index 85ab2c1b..f27ad687 100644 --- a/common/include/pcl/PCLPointCloud2.h +++ b/common/include/pcl/PCLPointCloud2.h @@ -1,15 +1,10 @@ #pragma once -#ifdef USE_ROS - #error USE_ROS setup requires PCL to compile against ROS message headers, which is now deprecated -#endif - #include #include #include -// Include the correct Header path here #include #include @@ -122,7 +117,7 @@ namespace pcl } s << "is_dense: "; s << " " << v.is_dense << std::endl; - + return (s); } diff --git a/common/include/pcl/PCLPointField.h b/common/include/pcl/PCLPointField.h index b50d20cc..58f0895b 100644 --- a/common/include/pcl/PCLPointField.h +++ b/common/include/pcl/PCLPointField.h @@ -1,13 +1,9 @@ #pragma once -#ifdef USE_ROS - #error USE_ROS setup requires PCL to compile against ROS message headers, which is now deprecated -#endif - -#include -#include -#include -#include +#include // for string +#include // for ostream + +#include // for shared_ptr namespace pcl { diff --git a/common/include/pcl/common/bivariate_polynomial.h b/common/include/pcl/common/bivariate_polynomial.h index 1b0d5e13..615320b4 100644 --- a/common/include/pcl/common/bivariate_polynomial.h +++ b/common/include/pcl/common/bivariate_polynomial.h @@ -41,15 +41,16 @@ #include #include +#include -namespace pcl +namespace pcl { /** \brief This represents a bivariate polynomial and provides some functionality for it - * \author Bastian Steder + * \author Bastian Steder * \ingroup common */ template - class BivariatePolynomialT + class BivariatePolynomialT { public: //-----CONSTRUCTOR&DESTRUCTOR----- @@ -76,7 +77,7 @@ namespace pcl /** Calculate the value of the polynomial at the given point */ real - getValue (real x, real y) const; + getValue (real x, real y) const; /** Calculate the gradient of this polynomial * If forceRecalc is false, it will do nothing when the gradient already exists */ @@ -91,7 +92,7 @@ namespace pcl * !!Currently only implemented for degree 2!! */ void findCriticalPoints (std::vector& x_values, std::vector& y_values, std::vector& types) const; - + /** write as binary to a stream */ void writeBinary (std::ostream& os) const; @@ -107,7 +108,7 @@ namespace pcl /** read binary from a file */ void readBinary (const char* filename); - + /** How many parameters has a bivariate polynomial of the given degree */ static unsigned int getNoOfParametersFromDegree (int n) { return ((n+2)* (n+1))/2;} @@ -116,7 +117,7 @@ namespace pcl int degree; real* parameters; BivariatePolynomialT* gradient_x, * gradient_y; - + protected: //-----METHODS----- /** Delete all members */ diff --git a/common/include/pcl/common/file_io.h b/common/include/pcl/common/file_io.h index f4fa71c9..fe413bb5 100644 --- a/common/include/pcl/common/file_io.h +++ b/common/include/pcl/common/file_io.h @@ -39,9 +39,6 @@ #pragma once #include -#ifndef _WIN32 - #include -#endif #include #include diff --git a/common/include/pcl/common/impl/bivariate_polynomial.hpp b/common/include/pcl/common/impl/bivariate_polynomial.hpp index de1db4ec..381a7c28 100644 --- a/common/include/pcl/common/impl/bivariate_polynomial.hpp +++ b/common/include/pcl/common/impl/bivariate_polynomial.hpp @@ -40,6 +40,10 @@ #define BIVARIATE_POLYNOMIAL_HPP #include +#include +#include +#include +#include /////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template @@ -99,18 +103,18 @@ template void pcl::BivariatePolynomialT::deepCopy (const pcl::BivariatePolynomialT& other) { if (this == &other) return; - if (degree != other.degree) + if (degree != other.degree) { memoryCleanUp (); degree = other.degree; parameters = new real[getNoOfParameters ()]; } - if (other.gradient_x == NULL) + if (other.gradient_x == NULL) { delete gradient_x; gradient_x=NULL; delete gradient_y; gradient_y=NULL; } - else if (gradient_x==NULL) + else if (gradient_x==NULL) { gradient_x = new pcl::BivariatePolynomialT (); gradient_y = new pcl::BivariatePolynomialT (); @@ -118,7 +122,7 @@ pcl::BivariatePolynomialT::deepCopy (const pcl::BivariatePolynomialT std::copy_n(other.parameters, getNoOfParameters (), parameters); - if (other.gradient_x != NULL) + if (other.gradient_x != NULL) { gradient_x->deepCopy (*other.gradient_x); gradient_y->deepCopy (*other.gradient_y); @@ -130,23 +134,23 @@ template void pcl::BivariatePolynomialT::calculateGradient (bool forceRecalc) { if (gradient_x!=NULL && !forceRecalc) return; - + if (gradient_x == NULL) gradient_x = new pcl::BivariatePolynomialT (degree-1); if (gradient_y == NULL) gradient_y = new pcl::BivariatePolynomialT (degree-1); - + unsigned int parameterPosDx=0, parameterPosDy=0; - for (int xDegree=degree; xDegree>=0; xDegree--) + for (int xDegree=degree; xDegree>=0; xDegree--) { - for (int yDegree=degree-xDegree; yDegree>=0; yDegree--) + for (int yDegree=degree-xDegree; yDegree>=0; yDegree--) { - if (xDegree > 0) + if (xDegree > 0) { gradient_x->parameters[parameterPosDx] = xDegree * parameters[parameterPosDx]; parameterPosDx++; } - if (yDegree > 0) + if (yDegree > 0) { gradient_y->parameters[parameterPosDy] = yDegree * parameters[ ( (degree+2-xDegree)* (degree+1-xDegree))/2 - yDegree - 1]; @@ -163,7 +167,7 @@ pcl::BivariatePolynomialT::getValue (real x, real y) const unsigned int parametersSize = getNoOfParameters (); real* tmpParameter = ¶meters[parametersSize-1]; real tmpX=1.0, tmpY, ret=0; - for (int xDegree=0; xDegree<=degree; xDegree++) + for (int xDegree=0; xDegree<=degree; xDegree++) { tmpY = 1.0; for (int yDegree=0; yDegree<=degree-xDegree; yDegree++) @@ -194,16 +198,16 @@ pcl::BivariatePolynomialT::findCriticalPoints (std::vector& x_values x_values.clear (); y_values.clear (); types.clear (); - + if (degree == 2) { real x = (real(2)*parameters[2]*parameters[3] - parameters[1]*parameters[4]) / (parameters[1]*parameters[1] - real(4)*parameters[0]*parameters[3]), y = (real(-2)*parameters[0]*x - parameters[2]) / parameters[1]; - + if (!std::isfinite(x) || !std::isfinite(y)) return; - + int type = 2; real det_H = real(4)*parameters[0]*parameters[3] - parameters[1]*parameters[1]; //std::cout << "det(H) = "<& p) real* tmpParameter = p.parameters; bool first = true; real currentParameter; - for (int xDegree=p.degree; xDegree>=0; xDegree--) + for (int xDegree=p.degree; xDegree>=0; xDegree--) { - for (int yDegree=p.degree-xDegree; yDegree>=0; yDegree--) + for (int yDegree=p.degree-xDegree; yDegree>=0; yDegree--) { currentParameter = *tmpParameter; - if (!first) + if (!first) { os << (currentParameter<0.0?" - ":" + "); currentParameter = std::abs (currentParameter); } os << currentParameter; - if (xDegree>0) + if (xDegree>0) { os << "x"; if (xDegree>1) os<<"^"<0) + if (yDegree>0) { os << "y"; if (yDegree>1) os<<"^"<& p) template void pcl::BivariatePolynomialT::writeBinary (std::ostream& os) const { - os.write (reinterpret_cast (°ree), sizeof (int)); + os.write (reinterpret_cast (°ree), sizeof (int)); unsigned int paramCnt = getNoOfParametersFromDegree (this->degree); - os.write (reinterpret_cast (this->parameters), paramCnt * sizeof (real)); + os.write (reinterpret_cast (this->parameters), paramCnt * sizeof (real)); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -299,4 +303,3 @@ pcl::BivariatePolynomialT::readBinary (const char* filename) } #endif - diff --git a/common/include/pcl/common/impl/common.hpp b/common/include/pcl/common/impl/common.hpp index 8dace825..ecf7f5a1 100644 --- a/common/include/pcl/common/impl/common.hpp +++ b/common/include/pcl/common/impl/common.hpp @@ -422,13 +422,12 @@ pcl::calculatePolygonArea (const pcl::PointCloud &polygon) { float area = 0.0f; int num_points = polygon.size (); - int j = 0; Eigen::Vector3f va,vb,res; res(0) = res(1) = res(2) = 0.0f; for (int i = 0; i < num_points; ++i) { - j = (i + 1) % num_points; + int j = (i + 1) % num_points; va = polygon[i].getVector3fMap (); vb = polygon[j].getVector3fMap (); res += va.cross (vb); diff --git a/common/include/pcl/common/impl/eigen.hpp b/common/include/pcl/common/impl/eigen.hpp index a0e59e85..d41f2055 100644 --- a/common/include/pcl/common/impl/eigen.hpp +++ b/common/include/pcl/common/impl/eigen.hpp @@ -41,7 +41,7 @@ #include #include - +#include #include ////////////////////////////////////////////////////////////////////////////////////////// @@ -493,15 +493,15 @@ pcl::determinant3x3Matrix (const Matrix& matrix) } ////////////////////////////////////////////////////////////////////////////////////////// -void -pcl::getTransFromUnitVectorsZY (const Eigen::Vector3f& z_axis, - const Eigen::Vector3f& y_direction, +void +pcl::getTransFromUnitVectorsZY (const Eigen::Vector3f& z_axis, + const Eigen::Vector3f& y_direction, Eigen::Affine3f& transformation) { Eigen::Vector3f tmp0 = (y_direction.cross(z_axis)).normalized(); Eigen::Vector3f tmp1 = (z_axis.cross(tmp0)).normalized(); Eigen::Vector3f tmp2 = z_axis.normalized(); - + transformation(0,0)=tmp0[0]; transformation(0,1)=tmp0[1]; transformation(0,2)=tmp0[2]; transformation(0,3)=0.0f; transformation(1,0)=tmp1[0]; transformation(1,1)=tmp1[1]; transformation(1,2)=tmp1[2]; transformation(1,3)=0.0f; transformation(2,0)=tmp2[0]; transformation(2,1)=tmp2[1]; transformation(2,2)=tmp2[2]; transformation(2,3)=0.0f; @@ -509,8 +509,8 @@ pcl::getTransFromUnitVectorsZY (const Eigen::Vector3f& z_axis, } ////////////////////////////////////////////////////////////////////////////////////////// -Eigen::Affine3f -pcl::getTransFromUnitVectorsZY (const Eigen::Vector3f& z_axis, +Eigen::Affine3f +pcl::getTransFromUnitVectorsZY (const Eigen::Vector3f& z_axis, const Eigen::Vector3f& y_direction) { Eigen::Affine3f transformation; @@ -519,15 +519,15 @@ pcl::getTransFromUnitVectorsZY (const Eigen::Vector3f& z_axis, } ////////////////////////////////////////////////////////////////////////////////////////// -void -pcl::getTransFromUnitVectorsXY (const Eigen::Vector3f& x_axis, - const Eigen::Vector3f& y_direction, +void +pcl::getTransFromUnitVectorsXY (const Eigen::Vector3f& x_axis, + const Eigen::Vector3f& y_direction, Eigen::Affine3f& transformation) { Eigen::Vector3f tmp2 = (x_axis.cross(y_direction)).normalized(); Eigen::Vector3f tmp1 = (tmp2.cross(x_axis)).normalized(); Eigen::Vector3f tmp0 = x_axis.normalized(); - + transformation(0,0)=tmp0[0]; transformation(0,1)=tmp0[1]; transformation(0,2)=tmp0[2]; transformation(0,3)=0.0f; transformation(1,0)=tmp1[0]; transformation(1,1)=tmp1[1]; transformation(1,2)=tmp1[2]; transformation(1,3)=0.0f; transformation(2,0)=tmp2[0]; transformation(2,1)=tmp2[1]; transformation(2,2)=tmp2[2]; transformation(2,3)=0.0f; @@ -535,8 +535,8 @@ pcl::getTransFromUnitVectorsXY (const Eigen::Vector3f& x_axis, } ////////////////////////////////////////////////////////////////////////////////////////// -Eigen::Affine3f -pcl::getTransFromUnitVectorsXY (const Eigen::Vector3f& x_axis, +Eigen::Affine3f +pcl::getTransFromUnitVectorsXY (const Eigen::Vector3f& x_axis, const Eigen::Vector3f& y_direction) { Eigen::Affine3f transformation; @@ -545,17 +545,17 @@ pcl::getTransFromUnitVectorsXY (const Eigen::Vector3f& x_axis, } ////////////////////////////////////////////////////////////////////////////////////////// -void -pcl::getTransformationFromTwoUnitVectors (const Eigen::Vector3f& y_direction, - const Eigen::Vector3f& z_axis, +void +pcl::getTransformationFromTwoUnitVectors (const Eigen::Vector3f& y_direction, + const Eigen::Vector3f& z_axis, Eigen::Affine3f& transformation) { getTransFromUnitVectorsZY (z_axis, y_direction, transformation); } ////////////////////////////////////////////////////////////////////////////////////////// -Eigen::Affine3f -pcl::getTransformationFromTwoUnitVectors (const Eigen::Vector3f& y_direction, +Eigen::Affine3f +pcl::getTransformationFromTwoUnitVectors (const Eigen::Vector3f& y_direction, const Eigen::Vector3f& z_axis) { Eigen::Affine3f transformation; @@ -563,10 +563,10 @@ pcl::getTransformationFromTwoUnitVectors (const Eigen::Vector3f& y_direction, return (transformation); } -void -pcl::getTransformationFromTwoUnitVectorsAndOrigin (const Eigen::Vector3f& y_direction, +void +pcl::getTransformationFromTwoUnitVectorsAndOrigin (const Eigen::Vector3f& y_direction, const Eigen::Vector3f& z_axis, - const Eigen::Vector3f& origin, + const Eigen::Vector3f& origin, Eigen::Affine3f& transformation) { getTransformationFromTwoUnitVectors(y_direction, z_axis, transformation); @@ -598,9 +598,9 @@ pcl::getTranslationAndEulerAngles (const Eigen::Transform void -pcl::getTransformation (Scalar x, Scalar y, Scalar z, - Scalar roll, Scalar pitch, Scalar yaw, +template void +pcl::getTransformation (Scalar x, Scalar y, Scalar z, + Scalar roll, Scalar pitch, Scalar yaw, Eigen::Transform &t) { Scalar A = std::cos (yaw), B = sin (yaw), C = std::cos (pitch), D = sin (pitch), @@ -613,7 +613,7 @@ pcl::getTransformation (Scalar x, Scalar y, Scalar z, } ////////////////////////////////////////////////////////////////////////////////////////// -template void +template void pcl::saveBinary (const Eigen::MatrixBase& matrix, std::ostream& file) { std::uint32_t rows = static_cast (matrix.rows ()), cols = static_cast (matrix.cols ()); @@ -628,7 +628,7 @@ pcl::saveBinary (const Eigen::MatrixBase& matrix, std::ostream& file) } ////////////////////////////////////////////////////////////////////////////////////////// -template void +template void pcl::loadBinary (Eigen::MatrixBase const & matrix_, std::istream& file) { Eigen::MatrixBase &matrix = const_cast &> (matrix_); @@ -638,7 +638,7 @@ pcl::loadBinary (Eigen::MatrixBase const & matrix_, std::istream& file) file.read (reinterpret_cast (&cols), sizeof (cols)); if (matrix.rows () != static_cast(rows) || matrix.cols () != static_cast(cols)) matrix.derived().resize(rows, cols); - + for (std::uint32_t i = 0; i < rows; ++i) for (std::uint32_t j = 0; j < cols; ++j) { @@ -649,7 +649,7 @@ pcl::loadBinary (Eigen::MatrixBase const & matrix_, std::istream& file) } ////////////////////////////////////////////////////////////////////////////////////////// -template +template typename Eigen::internal::umeyama_transform_matrix_type::type pcl::umeyama (const Eigen::MatrixBase& src, const Eigen::MatrixBase& dst, bool with_scaling) { @@ -902,4 +902,3 @@ pcl::transformBetween2CoordinateSystems (const Eigen::Matrix +#include + +#include +#include +#include +#include +#include + namespace pcl { -#ifndef _WIN32 - void getAllPcdFilesInDirectory(const std::string& directory, std::vector& file_names) +void getAllPcdFilesInDirectory(const std::string& directory, std::vector& file_names) +{ + boost::filesystem::path p(directory); + if(boost::filesystem::is_directory(p)) { - DIR *dp; - struct dirent *dirp; - if((dp = opendir(directory.c_str())) == nullptr) { - std::cerr << "Could not open directory.\n"; - return; - } - while ((dirp = readdir(dp)) != nullptr) { - if (dirp->d_type == DT_REG) // Only regular files + for(const auto& entry : boost::make_iterator_range(boost::filesystem::directory_iterator(p), {})) + { + if (boost::filesystem::is_regular_file(entry)) { - std::string file_name = dirp->d_name; - if (file_name.substr(file_name.size()-4, 4)==".pcd") - file_names.emplace_back(dirp->d_name); + if (entry.path().extension() == ".pcd") + file_names.emplace_back(entry.path().filename().string()); } } - closedir(dp); - std::sort(file_names.begin(), file_names.end()); - //for (unsigned int i=0; i +#include +// #include + namespace pcl { diff --git a/common/include/pcl/common/impl/polynomial_calculations.hpp b/common/include/pcl/common/impl/polynomial_calculations.hpp index 9e7198c3..6bdf0ab3 100644 --- a/common/include/pcl/common/impl/polynomial_calculations.hpp +++ b/common/include/pcl/common/impl/polynomial_calculations.hpp @@ -267,8 +267,7 @@ inline void return; } - double root1, root2, root3, root4, - a2 = a*a, + double a2 = a*a, a3 = a2*a, a4 = a2*a2, b2 = b*b, @@ -297,7 +296,7 @@ inline void } else if (quadraticRoot > 0.0) { - root1 = sqrt (quadraticRoot); + double root1 = sqrt (quadraticRoot); roots.push_back (root1 - resubValue); roots.push_back (-root1 - resubValue); } @@ -346,28 +345,28 @@ inline void if (tmp1 > 0) { tmp1 = sqrt (tmp1); - root1 = - (b/ (4.0*a)) + 0.5* (w+tmp1); - root2 = - (b/ (4.0*a)) + 0.5* (w-tmp1); + double root1 = - (b/ (4.0*a)) + 0.5* (w+tmp1); + double root2 = - (b/ (4.0*a)) + 0.5* (w-tmp1); roots.push_back (root1); roots.push_back (root2); } else if (isNearlyZero (tmp1)) { - root1 = - (b/ (4.0*a)) + 0.5*w; + double root1 = - (b/ (4.0*a)) + 0.5*w; roots.push_back (root1); } if (tmp2 > 0) { tmp2 = sqrt (tmp2); - root3 = - (b/ (4.0*a)) + 0.5* (-w+tmp2); - root4 = - (b/ (4.0*a)) + 0.5* (-w-tmp2); + double root3 = - (b/ (4.0*a)) + 0.5* (-w+tmp2); + double root4 = - (b/ (4.0*a)) + 0.5* (-w-tmp2); roots.push_back (root3); roots.push_back (root4); } else if (isNearlyZero (tmp2)) { - root3 = - (b/ (4.0*a)) - 0.5*w; + double root3 = - (b/ (4.0*a)) - 0.5*w; roots.push_back (root3); } diff --git a/common/include/pcl/common/impl/transforms.hpp b/common/include/pcl/common/impl/transforms.hpp index 2168e048..eff1bdd9 100644 --- a/common/include/pcl/common/impl/transforms.hpp +++ b/common/include/pcl/common/impl/transforms.hpp @@ -45,6 +45,11 @@ #include #endif +#include +#include +#include +#include + namespace pcl { @@ -212,7 +217,7 @@ namespace pcl /////////////////////////////////////////////////////////////////////////////////////////// template void -pcl::transformPointCloud (const pcl::PointCloud &cloud_in, +pcl::transformPointCloud (const pcl::PointCloud &cloud_in, pcl::PointCloud &cloud_out, const Eigen::Transform &transform, bool copy_all_fields) @@ -245,8 +250,8 @@ pcl::transformPointCloud (const pcl::PointCloud &cloud_in, // otherwise we get errors during the multiplication (?) for (std::size_t i = 0; i < cloud_out.points.size (); ++i) { - if (!std::isfinite (cloud_in.points[i].x) || - !std::isfinite (cloud_in.points[i].y) || + if (!std::isfinite (cloud_in.points[i].x) || + !std::isfinite (cloud_in.points[i].y) || !std::isfinite (cloud_in.points[i].z)) continue; tf.se3 (cloud_in[i].data, cloud_out[i].data); @@ -256,8 +261,8 @@ pcl::transformPointCloud (const pcl::PointCloud &cloud_in, /////////////////////////////////////////////////////////////////////////////////////////// template void -pcl::transformPointCloud (const pcl::PointCloud &cloud_in, - const std::vector &indices, +pcl::transformPointCloud (const pcl::PointCloud &cloud_in, + const std::vector &indices, pcl::PointCloud &cloud_out, const Eigen::Transform &transform, bool copy_all_fields) @@ -292,8 +297,8 @@ pcl::transformPointCloud (const pcl::PointCloud &cloud_in, { if (copy_all_fields) cloud_out.points[i] = cloud_in.points[indices[i]]; - if (!std::isfinite (cloud_in.points[indices[i]].x) || - !std::isfinite (cloud_in.points[indices[i]].y) || + if (!std::isfinite (cloud_in.points[indices[i]].x) || + !std::isfinite (cloud_in.points[indices[i]].y) || !std::isfinite (cloud_in.points[indices[i]].z)) continue; tf.se3 (cloud_in[indices[i]].data, cloud_out[i].data); @@ -303,7 +308,7 @@ pcl::transformPointCloud (const pcl::PointCloud &cloud_in, /////////////////////////////////////////////////////////////////////////////////////////// template void -pcl::transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, +pcl::transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, pcl::PointCloud &cloud_out, const Eigen::Transform &transform, bool copy_all_fields) @@ -339,8 +344,8 @@ pcl::transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, { for (std::size_t i = 0; i < cloud_out.points.size (); ++i) { - if (!std::isfinite (cloud_in.points[i].x) || - !std::isfinite (cloud_in.points[i].y) || + if (!std::isfinite (cloud_in.points[i].x) || + !std::isfinite (cloud_in.points[i].y) || !std::isfinite (cloud_in.points[i].z)) continue; tf.se3 (cloud_in[i].data, cloud_out[i].data); @@ -351,8 +356,8 @@ pcl::transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, /////////////////////////////////////////////////////////////////////////////////////////// template void -pcl::transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, - const std::vector &indices, +pcl::transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, + const std::vector &indices, pcl::PointCloud &cloud_out, const Eigen::Transform &transform, bool copy_all_fields) @@ -389,8 +394,8 @@ pcl::transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, if (copy_all_fields) cloud_out.points[i] = cloud_in.points[indices[i]]; - if (!std::isfinite (cloud_in.points[indices[i]].x) || - !std::isfinite (cloud_in.points[indices[i]].y) || + if (!std::isfinite (cloud_in.points[indices[i]].x) || + !std::isfinite (cloud_in.points[indices[i]].y) || !std::isfinite (cloud_in.points[indices[i]].z)) continue; @@ -402,9 +407,9 @@ pcl::transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, /////////////////////////////////////////////////////////////////////////////////////////// template inline void -pcl::transformPointCloud (const pcl::PointCloud &cloud_in, +pcl::transformPointCloud (const pcl::PointCloud &cloud_in, pcl::PointCloud &cloud_out, - const Eigen::Matrix &offset, + const Eigen::Matrix &offset, const Eigen::Quaternion &rotation, bool copy_all_fields) { @@ -416,9 +421,9 @@ pcl::transformPointCloud (const pcl::PointCloud &cloud_in, /////////////////////////////////////////////////////////////////////////////////////////// template inline void -pcl::transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, +pcl::transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, pcl::PointCloud &cloud_out, - const Eigen::Matrix &offset, + const Eigen::Matrix &offset, const Eigen::Quaternion &rotation, bool copy_all_fields) { @@ -430,7 +435,7 @@ pcl::transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, /////////////////////////////////////////////////////////////////////////////////////////// template inline PointT -pcl::transformPoint (const PointT &point, +pcl::transformPoint (const PointT &point, const Eigen::Transform &transform) { PointT ret = point; @@ -441,7 +446,7 @@ pcl::transformPoint (const PointT &point, /////////////////////////////////////////////////////////////////////////////////////////// template inline PointT -pcl::transformPointWithNormal (const PointT &point, +pcl::transformPointWithNormal (const PointT &point, const Eigen::Transform &transform) { PointT ret = point; @@ -453,12 +458,12 @@ pcl::transformPointWithNormal (const PointT &point, /////////////////////////////////////////////////////////////////////////////////////////// template double -pcl::getPrincipalTransformation (const pcl::PointCloud &cloud, +pcl::getPrincipalTransformation (const pcl::PointCloud &cloud, Eigen::Transform &transform) { EIGEN_ALIGN16 Eigen::Matrix covariance_matrix; Eigen::Matrix centroid; - + pcl::computeMeanAndCovarianceMatrix (cloud, covariance_matrix, centroid); EIGEN_ALIGN16 Eigen::Matrix eigen_vects; @@ -467,10 +472,10 @@ pcl::getPrincipalTransformation (const pcl::PointCloud &cloud, double rel1 = eigen_vals.coeff (0) / eigen_vals.coeff (1); double rel2 = eigen_vals.coeff (1) / eigen_vals.coeff (2); - + transform.translation () = centroid.head (3); transform.linear () = eigen_vects; - + return (std::min (rel1, rel2)); } diff --git a/common/include/pcl/common/io.h b/common/include/pcl/common/io.h index 2670407e..eba6eaff 100644 --- a/common/include/pcl/common/io.h +++ b/common/include/pcl/common/io.h @@ -47,6 +47,7 @@ #include #include #include +#include #include #include @@ -75,7 +76,7 @@ namespace pcl * \ingroup common */ template - [[deprecated("use getFieldIndex (field_name, fields) instead")]] + PCL_DEPRECATED("use getFieldIndex (field_name, fields) instead") inline int getFieldIndex (const pcl::PointCloud &cloud, const std::string &field_name, std::vector &fields); @@ -105,7 +106,7 @@ namespace pcl * \ingroup common */ template - [[deprecated("use getFields () with return value instead")]] + PCL_DEPRECATED("use getFields () with return value instead") inline void getFields (const pcl::PointCloud &cloud, std::vector &fields); @@ -115,7 +116,7 @@ namespace pcl * \ingroup common */ template - [[deprecated("use getFields () with return value instead")]] + PCL_DEPRECATED("use getFields () with return value instead") inline void getFields (std::vector &fields); @@ -327,7 +328,7 @@ namespace pcl * \return true if successful, false otherwise (e.g., name/number of fields differs) * \ingroup common */ - [[deprecated("use pcl::concatenate() instead, but beware of subtle difference in behavior (see documentation)")]] + PCL_DEPRECATED("use pcl::concatenate() instead, but beware of subtle difference in behavior (see documentation)") PCL_EXPORTS bool concatenatePointCloud (const pcl::PCLPointCloud2 &cloud1, const pcl::PCLPointCloud2 &cloud2, diff --git a/common/include/pcl/common/point_tests.h b/common/include/pcl/common/point_tests.h index 66543978..abd58c35 100644 --- a/common/include/pcl/common/point_tests.h +++ b/common/include/pcl/common/point_tests.h @@ -67,7 +67,7 @@ namespace pcl template<> inline bool isFinite(const pcl::Axis&) { return (true); } template<> inline bool isFinite(const pcl::BRISKSignature512&) { return (true); } - template<> inline bool isFinite(const pcl::BorderDescription &p) { return true; } + template<> inline bool isFinite(const pcl::BorderDescription &) { return true; } template<> inline bool isFinite(const pcl::Boundary&) { return (true); } template<> inline bool isFinite(const pcl::ESFSignature640&) { return (true); } template<> inline bool isFinite(const pcl::FPFHSignature33&) { return (true); } diff --git a/common/include/pcl/common/synchronizer.h b/common/include/pcl/common/synchronizer.h index daee8fb9..8fbb516e 100644 --- a/common/include/pcl/common/synchronizer.h +++ b/common/include/pcl/common/synchronizer.h @@ -35,8 +35,11 @@ #pragma once +#include #include +#include #include +#include namespace pcl { diff --git a/common/include/pcl/common/utils.h b/common/include/pcl/common/utils.h index 1cd6b279..6c91b001 100644 --- a/common/include/pcl/common/utils.h +++ b/common/include/pcl/common/utils.h @@ -38,6 +38,7 @@ #pragma once +#include #include namespace pcl @@ -50,7 +51,7 @@ namespace pcl * \param[in] eps epsilon * \return true if val1 is equal to val2, false otherwise. */ - template bool + template bool equal (T val1, T val2, T eps = std::numeric_limits::min ()) { return (std::fabs (val1 - val2) < eps); diff --git a/common/include/pcl/console/parse.h b/common/include/pcl/console/parse.h index 79e14f45..04d0e286 100644 --- a/common/include/pcl/console/parse.h +++ b/common/include/pcl/console/parse.h @@ -150,6 +150,16 @@ namespace pcl PCL_EXPORTS int parse_argument (int argc, const char * const * argv, const char * str, unsigned int &val); + /** \brief Parse for a specific given command line argument. + * \param[in] argc the number of command line arguments + * \param[in] argv the command line arguments + * \param[in] str the string value to search for + * \param[out] val the resultant value + * \return index of found argument or -1 if arguments do not appear in list + */ + PCL_EXPORTS int + parse_argument (int argc, const char * const * argv, const char * str, long int &val) noexcept; + /** \brief Parse for a specific given command line argument. * \param[in] argc the number of command line arguments * \param[in] argv the command line arguments diff --git a/common/include/pcl/impl/point_types.hpp b/common/include/pcl/impl/point_types.hpp index a3ce9066..8f5823ca 100644 --- a/common/include/pcl/impl/point_types.hpp +++ b/common/include/pcl/impl/point_types.hpp @@ -39,6 +39,7 @@ #ifndef PCL_IMPL_POINT_TYPES_HPP_ #define PCL_IMPL_POINT_TYPES_HPP_ +#include #if defined __GNUC__ # pragma GCC system_header #endif @@ -290,16 +291,9 @@ namespace pcl */ struct EIGEN_ALIGN16 PointXYZ : public _PointXYZ { - inline PointXYZ (const _PointXYZ &p) - { - x = p.x; y = p.y; z = p.z; data[3] = 1.0f; - } + inline PointXYZ (const _PointXYZ &p): PointXYZ(p.x, p.y, p.z) {} - inline PointXYZ () - { - x = y = z = 0.0f; - data[3] = 1.0f; - } + inline PointXYZ (): PointXYZ(0.f, 0.f, 0.f) {} inline PointXYZ (float _x, float _y, float _z) { @@ -344,20 +338,14 @@ namespace pcl { inline RGB (const _RGB &p) { - rgba = p.rgba; + rgba = p.rgba; } - inline RGB () - { - r = g = b = 0; - a = 255; - } + inline RGB (): RGB(0, 0, 0) {} inline RGB (std::uint8_t _r, std::uint8_t _g, std::uint8_t _b) { - r = _r; - g = _g; - b = _b; + r = _r; g = _g; b = _b; a = 255; } @@ -381,14 +369,14 @@ namespace pcl intensity = p.intensity; } - inline Intensity () + inline Intensity (float _intensity = 0.f) { - intensity = 0.0f; + intensity = _intensity; } - + friend std::ostream& operator << (std::ostream& os, const Intensity& p); }; - + struct _Intensity8u { @@ -407,11 +395,11 @@ namespace pcl intensity = p.intensity; } - inline Intensity8u () + inline Intensity8u (std::uint8_t _intensity = 0) { - intensity = 0; + intensity = _intensity; } - + #if defined(_LIBCPP_VERSION) && _LIBCPP_VERSION <= 1101 operator unsigned char() const { @@ -439,9 +427,9 @@ namespace pcl intensity = p.intensity; } - inline Intensity32u () + inline Intensity32u (std::uint32_t _intensity = 0) { - intensity = 0; + intensity = _intensity; } friend std::ostream& operator << (std::ostream& os, const Intensity32u& p); @@ -473,21 +461,18 @@ namespace pcl intensity = p.intensity; } - inline PointXYZI () - { - x = y = z = 0.0f; - data[3] = 1.0f; - intensity = 0.0f; - } - inline PointXYZI (float _intensity) + inline PointXYZI (float _intensity = 0.f): PointXYZI(0.f, 0.f, 0.f, _intensity) {} + + inline PointXYZI (float _x, float _y, float _z, float _intensity = 0.f) { - x = y = z = 0.0f; + x = _x; y = _y; z = _z; data[3] = 1.0f; intensity = _intensity; } + friend std::ostream& operator << (std::ostream& os, const PointXYZI& p); }; - + struct EIGEN_ALIGN16 _PointXYZL { @@ -505,13 +490,15 @@ namespace pcl label = p.label; } - inline PointXYZL () + inline PointXYZL (std::uint32_t _label = 0): PointXYZL(0.f, 0.f, 0.f, _label) {} + + inline PointXYZL (float _x, float _y, float _z, std::uint32_t _label = 0) { - x = y = z = 0.0f; + x = _x; y = _y; z = _z; data[3] = 1.0f; - label = 0; + label = _label; } - + friend std::ostream& operator << (std::ostream& os, const PointXYZL& p); }; @@ -519,8 +506,10 @@ namespace pcl PCL_EXPORTS std::ostream& operator << (std::ostream& os, const Label& p); struct Label { - std::uint32_t label; - + std::uint32_t label = 0; + + Label (std::uint32_t _label = 0): label(_label) {} + friend std::ostream& operator << (std::ostream& os, const Label& p); }; @@ -561,12 +550,20 @@ namespace pcl rgba = p.rgba; } - inline PointXYZRGBA () + inline PointXYZRGBA (): PointXYZRGBA (0, 0, 0, 0) {} + + inline PointXYZRGBA (std::uint8_t _r, std::uint8_t _g, std::uint8_t _b, std::uint8_t _a): + PointXYZRGBA (0.f, 0.f, 0.f, _r, _g, _b, _a) {} + + inline PointXYZRGBA (float _x, float _y, float _z): + PointXYZRGBA (_x, _y, _z, 0, 0, 0, 0) {} + + inline PointXYZRGBA (float _x, float _y, float _z, std::uint8_t _r, + std::uint8_t _g, std::uint8_t _b, std::uint8_t _a) { - x = y = z = 0.0f; + x = _x; y = _y; z = _z; data[3] = 1.0f; - r = g = b = 0; - a = 255; + r = _r; g = _g; b = _b; a = _a; } friend std::ostream& operator << (std::ostream& os, const PointXYZRGBA& p); @@ -628,20 +625,20 @@ namespace pcl rgb = p.rgb; } - inline PointXYZRGB () - { - x = y = z = 0.0f; - data[3] = 1.0f; - r = g = b = 0; - a = 255; - } - inline PointXYZRGB (std::uint8_t _r, std::uint8_t _g, std::uint8_t _b) + inline PointXYZRGB (): PointXYZRGB (0.f, 0.f, 0.f) {} + + inline PointXYZRGB (std::uint8_t _r, std::uint8_t _g, std::uint8_t _b): + PointXYZRGB (0.f, 0.f, 0.f, _r, _g, _b) {} + + inline PointXYZRGB (float _x, float _y, float _z): + PointXYZRGB (_x, _y, _z, 0, 0, 0) {} + + inline PointXYZRGB (float _x, float _y, float _z, + std::uint8_t _r, std::uint8_t _g, std::uint8_t _b) { - x = y = z = 0.0f; + x = _x; y = _y; z = _z; data[3] = 1.0f; - r = _r; - g = _g; - b = _b; + r = _r; g = _g; b = _b; a = 255; } @@ -660,25 +657,26 @@ namespace pcl label = p.label; } - inline PointXYZRGBL () - { - x = y = z = 0.0f; - data[3] = 1.0f; - r = g = b = 0; - a = 255; - label = 0; - } - inline PointXYZRGBL (std::uint8_t _r, std::uint8_t _g, std::uint8_t _b, std::uint32_t _label) + inline PointXYZRGBL (std::uint32_t _label = 0): + PointXYZRGBL (0.f, 0.f, 0.f, 0, 0, 0, _label) {} + + inline PointXYZRGBL (std::uint8_t _r, std::uint8_t _g, std::uint8_t _b): + PointXYZRGBL (0.f, 0.f, 0.f, _r, _g, _b) {} + + inline PointXYZRGBL (float _x, float _y, float _z): + PointXYZRGBL (_x, _y, _z, 0, 0, 0) {} + + inline PointXYZRGBL (float _x, float _y, float _z, + std::uint8_t _r, std::uint8_t _g, std::uint8_t _b, + std::uint32_t _label = 0) { - x = y = z = 0.0f; + x = _x; y = _y; z = _z; data[3] = 1.0f; - r = _r; - g = _g; - b = _b; + r = _r; g = _g; b = _b; a = 255; label = _label; } - + friend std::ostream& operator << (std::ostream& os, const PointXYZRGBL& p); PCL_MAKE_ALIGNED_OPERATOR_NEW }; @@ -709,20 +707,22 @@ namespace pcl h = p.h; s = p.s; v = p.v; } - inline PointXYZHSV () - { - x = y = z = 0.0f; - data[3] = 1.0f; - h = s = v = data_c[3] = 0; - } - inline PointXYZHSV (float _h, float _v, float _s) + inline PointXYZHSV (): PointXYZHSV (0.f, 0.f, 0.f) {} + + // @TODO: Use strong types?? + // This is a dangerous type, doesn't behave like others + inline PointXYZHSV (float _h, float _s, float _v): + PointXYZHSV (0.f, 0.f, 0.f, _h, _s, _v) {} + + inline PointXYZHSV (float _x, float _y, float _z, + float _h, float _s, float _v) { - x = y = z = 0.0f; + x = _x; y = _y; z = _z; data[3] = 1.0f; - h = _h; v = _v; s = _s; + h = _h; s = _s; v = _v; data_c[3] = 0; } - + friend std::ostream& operator << (std::ostream& os, const PointXYZHSV& p); PCL_MAKE_ALIGNED_OPERATOR_NEW }; @@ -735,9 +735,13 @@ namespace pcl */ struct PointXY { - float x; - float y; - + float x = 0.f; + float y = 0.f; + + inline PointXY() = default; + + inline PointXY(float _x, float _y): x(_x), y(_y) {} + friend std::ostream& operator << (std::ostream& os, const PointXY& p); }; @@ -748,9 +752,13 @@ namespace pcl */ struct PointUV { - float u; - float v; - + float u = 0.f; + float v = 0.f; + + inline PointUV() = default; + + inline PointUV(float _u, float _v): u(_u), v(_v) {} + friend std::ostream& operator << (std::ostream& os, const PointUV& p); }; @@ -758,6 +766,7 @@ namespace pcl /** \brief A point structure representing an interest point with Euclidean xyz coordinates, and an interest value. * \ingroup common */ + // @TODO: inheritance trick like on other PointTypes struct EIGEN_ALIGN16 InterestPoint { PCL_ADD_POINT4D; // This adds the members x,y,z which can also be accessed using the point (which is float[4]) @@ -770,7 +779,7 @@ namespace pcl float data_c[4]; }; PCL_MAKE_ALIGNED_OPERATOR_NEW - + friend std::ostream& operator << (std::ostream& os, const InterestPoint& p); }; @@ -801,17 +810,13 @@ namespace pcl curvature = p.curvature; } - inline Normal () - { - normal_x = normal_y = normal_z = data_n[3] = 0.0f; - curvature = 0; - } + inline Normal (float _curvature = 0.f): Normal (0.f, 0.f, 0.f, _curvature) {} - inline Normal (float n_x, float n_y, float n_z) + inline Normal (float n_x, float n_y, float n_z, float _curvature = 0.f) { normal_x = n_x; normal_y = n_y; normal_z = n_z; - curvature = 0; data_n[3] = 0.0f; + curvature = _curvature; } friend std::ostream& operator << (std::ostream& os, const Normal& p); @@ -837,10 +842,7 @@ namespace pcl data_n[3] = 0.0f; } - inline Axis () - { - normal_x = normal_y = normal_z = data_n[3] = 0.0f; - } + inline Axis (): Axis (0.f, 0.f, 0.f) {} inline Axis (float n_x, float n_y, float n_z) { @@ -881,14 +883,20 @@ namespace pcl curvature = p.curvature; } - inline PointNormal () + inline PointNormal (float _curvature = 0.f): PointNormal (0.f, 0.f, 0.f, 0.f, 0.f, 0.f, _curvature) {} + + inline PointNormal (float _x, float _y, float _z): + PointNormal (_x, _y, _z, 0.f, 0.f, 0.f, 0.f) {} + + inline PointNormal (float _x, float _y, float _z, float n_x, float n_y, float n_z, float _curvature = 0.f) { - x = y = z = 0.0f; + x = _x; y = _y; z = _z; data[3] = 1.0f; - normal_x = normal_y = normal_z = data_n[3] = 0.0f; - curvature = 0.f; + normal_x = n_x; normal_y = n_y; normal_z = n_z; + data_n[3] = 0.0f; + curvature = _curvature; } - + friend std::ostream& operator << (std::ostream& os, const PointNormal& p); }; @@ -950,14 +958,28 @@ namespace pcl rgba = p.rgba; } - inline PointXYZRGBNormal () + inline PointXYZRGBNormal (float _curvature = 0.f): + PointXYZRGBNormal (0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, _curvature) {} + + inline PointXYZRGBNormal (float _x, float _y, float _z): + PointXYZRGBNormal (_x, _y, _z, 0, 0, 0) {} + + inline PointXYZRGBNormal (std::uint8_t _r, std::uint8_t _g, std::uint8_t _b): + PointXYZRGBNormal (0.f, 0.f, 0.f, _r, _g, _b) {} + + inline PointXYZRGBNormal (float _x, float _y, float _z, std::uint8_t _r, std::uint8_t _g, std::uint8_t _b): + PointXYZRGBNormal (_x, _y, _z, _r, _g, _b, 0.f, 0.f, 0.f) {} + + inline PointXYZRGBNormal (float _x, float _y, float _z, std::uint8_t _r, std::uint8_t _g, std::uint8_t _b, + float n_x, float n_y, float n_z, float _curvature = 0.f) { - x = y = z = 0.0f; + x = _x; y = _y; z = _z; data[3] = 1.0f; - r = g = b = 0; + r = _r; g = _g; b = _b; a = 255; - normal_x = normal_y = normal_z = data_n[3] = 0.0f; - curvature = 0; + normal_x = n_x; normal_y = n_y; normal_z = n_z; + data_n[3] = 0.f; + curvature = _curvature; } friend std::ostream& operator << (std::ostream& os, const PointXYZRGBNormal& p); @@ -978,7 +1000,7 @@ namespace pcl }; PCL_MAKE_ALIGNED_OPERATOR_NEW }; - + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const PointXYZINormal& p); /** \brief A point structure representing Euclidean xyz coordinates, intensity, together with normal coordinates and the surface curvature estimate. * \ingroup common @@ -992,16 +1014,23 @@ namespace pcl curvature = p.curvature; intensity = p.intensity; } + + inline PointXYZINormal (float _intensity = 0.f): PointXYZINormal (0.f, 0.f, 0.f, _intensity) {} + + inline PointXYZINormal (float _x, float _y, float _z, float _intensity = 0.f): + PointXYZINormal (_x, _y, _z, _intensity, 0.f, 0.f, 0.f) {} - inline PointXYZINormal () + inline PointXYZINormal (float _x, float _y, float _z, float _intensity, + float n_x, float n_y, float n_z, float _curvature = 0.f) { - x = y = z = 0.0f; + x = _x; y = _y; z = _z; data[3] = 1.0f; - normal_x = normal_y = normal_z = data_n[3] = 0.0f; - intensity = 0.0f; - curvature = 0; + intensity = _intensity; + normal_x = n_x; normal_y = n_y; normal_z = n_z; + data_n[3] = 0.f; + curvature = _curvature; } - + friend std::ostream& operator << (std::ostream& os, const PointXYZINormal& p); }; @@ -1036,13 +1065,20 @@ namespace pcl label = p.label; } - inline PointXYZLNormal () + inline PointXYZLNormal (std::uint32_t _label = 0): PointXYZLNormal (0.f, 0.f, 0.f, _label) {} + + inline PointXYZLNormal (float _x, float _y, float _z, std::uint32_t _label = 0.f): + PointXYZLNormal (_x, _y, _z, _label, 0.f, 0.f, 0.f) {} + + inline PointXYZLNormal (float _x, float _y, float _z, std::uint32_t _label, + float n_x, float n_y, float n_z, float _curvature = 0.f) { - x = y = z = 0.0f; + x = _x; y = _y; z = _z; data[3] = 1.0f; - normal_x = normal_y = normal_z = data_n[3] = 0.0f; - label = 0; - curvature = 0; + label = _label; + normal_x = n_x; normal_y = n_y; normal_z = n_z; + data_n[3] = 0.f; + curvature = _curvature; } friend std::ostream& operator << (std::ostream& os, const PointXYZLNormal& p); @@ -1077,13 +1113,15 @@ namespace pcl range = p.range; } - inline PointWithRange () + inline PointWithRange (float _range = 0.f): PointWithRange (0.f, 0.f, 0.f, _range) {} + + inline PointWithRange (float _x, float _y, float _z, float _range = 0.f) { - x = y = z = 0.0f; + x = _x; y = _y; z = _z; data[3] = 1.0f; - range = 0.0f; + range = _range; } - + friend std::ostream& operator << (std::ostream& os, const PointWithRange& p); }; @@ -1116,14 +1154,25 @@ namespace pcl vp_x = p.vp_x; vp_y = p.vp_y; vp_z = p.vp_z; } - inline PointWithViewpoint (float _x = 0.0f, float _y = 0.0f, float _z = 0.0f, - float _vp_x = 0.0f, float _vp_y = 0.0f, float _vp_z = 0.0f) + inline PointWithViewpoint (): PointWithViewpoint (0.f, 0.f, 0.f) {} + + PCL_DEPRECATED("Use ctor accepting all position (x, y, z) data") + inline PointWithViewpoint (float _x, float _y = 0.f): + PointWithViewpoint (_x, _y, 0.f) {} + + PCL_DEPRECATED("Use ctor accepting all viewpoint (vp_x, vp_y, vp_z) data") + inline PointWithViewpoint (float _x, float _y, float _z, float _vp_x, float _vp_y = 0.f): + PointWithViewpoint (_x, _y, _z, _vp_x, _vp_y, 0.f) {} + + inline PointWithViewpoint (float _x, float _y, float _z): PointWithViewpoint (_x, _y, _z, 0.f, 0.f, 0.f) {} + + inline PointWithViewpoint (float _x, float _y, float _z, float _vp_x, float _vp_y, float _vp_z) { x = _x; y = _y; z = _z; data[3] = 1.0f; vp_x = _vp_x; vp_y = _vp_y; vp_z = _vp_z; } - + friend std::ostream& operator << (std::ostream& os, const PointWithViewpoint& p); }; @@ -1133,8 +1182,12 @@ namespace pcl */ struct MomentInvariants { - float j1, j2, j3; - + float j1 = 0.f, j2 = 0.f, j3 = 0.f; + + inline MomentInvariants () = default; + + inline MomentInvariants (float _j1, float _j2, float _j3): j1 (_j1), j2 (_j2), j3 (_j3) {} + friend std::ostream& operator << (std::ostream& os, const MomentInvariants& p); }; @@ -1144,8 +1197,12 @@ namespace pcl */ struct PrincipalRadiiRSD { - float r_min, r_max; - + float r_min = 0.f, r_max = 0.f; + + inline PrincipalRadiiRSD () = default; + + inline PrincipalRadiiRSD (float _r_min, float _r_max): r_min (_r_min), r_max (_r_max) {} + friend std::ostream& operator << (std::ostream& os, const PrincipalRadiiRSD& p); }; @@ -1155,7 +1212,7 @@ namespace pcl */ struct Boundary { - std::uint8_t boundary_point; + std::uint8_t boundary_point = 0; #if defined(_LIBCPP_VERSION) && _LIBCPP_VERSION <= 1101 operator unsigned char() const @@ -1163,7 +1220,9 @@ namespace pcl return boundary_point; } #endif - + + inline Boundary (std::uint8_t _boundary = 0): boundary_point (_boundary) {} + friend std::ostream& operator << (std::ostream& os, const Boundary& p); }; @@ -1183,9 +1242,18 @@ namespace pcl float principal_curvature_z; }; }; - float pc1; - float pc2; - + float pc1 = 0.f; + float pc2 = 0.f; + + inline PrincipalCurvatures (): PrincipalCurvatures (0.f, 0.f) {} + + inline PrincipalCurvatures (float _pc1, float _pc2): PrincipalCurvatures (0.f, 0.f, 0.f, _pc1, _pc2) {} + + inline PrincipalCurvatures (float _x, float _y, float _z): PrincipalCurvatures (_x, _y, _z, 0.f, 0.f) {} + + inline PrincipalCurvatures (float _x, float _y, float _z, float _pc1, float _pc2): + principal_curvature_x (_x), principal_curvature_y (_y), principal_curvature_z (_z), pc1 (_pc1), pc2 (_pc2) {} + friend std::ostream& operator << (std::ostream& os, const PrincipalCurvatures& p); }; @@ -1195,9 +1263,11 @@ namespace pcl */ struct PFHSignature125 { - float histogram[125]; + float histogram[125] = {0.f}; static int descriptorSize () { return 125; } - + + inline PFHSignature125 () = default; + friend std::ostream& operator << (std::ostream& os, const PFHSignature125& p); }; @@ -1207,9 +1277,11 @@ namespace pcl */ struct PFHRGBSignature250 { - float histogram[250]; + float histogram[250] = {0.f}; static int descriptorSize () { return 250; } + inline PFHRGBSignature250 () = default; + friend std::ostream& operator << (std::ostream& os, const PFHRGBSignature250& p); }; @@ -1219,9 +1291,14 @@ namespace pcl */ struct PPFSignature { - float f1, f2, f3, f4; - float alpha_m; - + float f1 = 0.f, f2 = 0.f, f3 = 0.f, f4 = 0.f; + float alpha_m = 0.f; + + inline PPFSignature (float _alpha = 0.f): PPFSignature (0.f, 0.f, 0.f, 0.f, _alpha) {} + + inline PPFSignature (float _f1, float _f2, float _f3, float _f4, float _alpha = 0.f): + f1 (_f1), f2 (_f2), f3 (_f3), f4 (_f4), alpha_m (_alpha) {} + friend std::ostream& operator << (std::ostream& os, const PPFSignature& p); }; @@ -1233,7 +1310,15 @@ namespace pcl { float f1, f2, f3, f4, f5, f6, f7, f8, f9, f10; float alpha_m; - + + inline CPPFSignature (float _alpha = 0.f): + CPPFSignature (0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, _alpha) {} + + inline CPPFSignature (float _f1, float _f2, float _f3, float _f4, float _f5, float _f6, + float _f7, float _f8, float _f9, float _f10, float _alpha = 0.f): + f1 (_f1), f2 (_f2), f3 (_f3), f4 (_f4), f5 (_f5), f6 (_f6), + f7 (_f7), f8 (_f8), f9 (_f9), f10 (_f10), alpha_m (_alpha) {} + friend std::ostream& operator << (std::ostream& os, const CPPFSignature& p); }; @@ -1243,10 +1328,18 @@ namespace pcl */ struct PPFRGBSignature { - float f1, f2, f3, f4; - float r_ratio, g_ratio, b_ratio; - float alpha_m; - + float f1 = 0.f, f2 = 0.f, f3 = 0.f, f4 = 0.f; + float r_ratio = 0.f, g_ratio = 0.f, b_ratio = 0.f; + float alpha_m = 0.f; + + inline PPFRGBSignature (float _alpha = 0.f): PPFRGBSignature (0.f, 0.f, 0.f, 0.f, _alpha) {} + + inline PPFRGBSignature (float _f1, float _f2, float _f3, float _f4, float _alpha = 0.f): + PPFRGBSignature (_f1, _f2, _f3, _f4, _alpha, 0.f, 0.f, 0.f) {} + + inline PPFRGBSignature (float _f1, float _f2, float _f3, float _f4, float _alpha, float _r, float _g, float _b): + f1 (_f1), f2 (_f2), f3 (_f3), f4 (_f4), r_ratio (_r), g_ratio (_g), b_ratio (_b), alpha_m (_alpha) {} + friend std::ostream& operator << (std::ostream& os, const PPFRGBSignature& p); }; @@ -1257,8 +1350,10 @@ namespace pcl */ struct NormalBasedSignature12 { - float values[12]; - + float values[12] = {0.f}; + + inline NormalBasedSignature12 () = default; + friend std::ostream& operator << (std::ostream& os, const NormalBasedSignature12& p); }; @@ -1268,10 +1363,12 @@ namespace pcl */ struct ShapeContext1980 { - float descriptor[1980]; - float rf[9]; + float descriptor[1980] = {0.f}; + float rf[9] = {0.f}; static int descriptorSize () { return 1980; } + inline ShapeContext1980 () = default; + friend std::ostream& operator << (std::ostream& os, const ShapeContext1980& p); }; @@ -1281,10 +1378,12 @@ namespace pcl */ struct UniqueShapeContext1960 { - float descriptor[1960]; - float rf[9]; + float descriptor[1960] = {0.f}; + float rf[9] = {0.f}; static int descriptorSize () { return 1960; } + inline UniqueShapeContext1960 () = default; + friend std::ostream& operator << (std::ostream& os, const UniqueShapeContext1960& p); }; @@ -1294,10 +1393,12 @@ namespace pcl */ struct SHOT352 { - float descriptor[352]; - float rf[9]; + float descriptor[352] = {0.f}; + float rf[9] = {0.f}; static int descriptorSize () { return 352; } + inline SHOT352 () = default; + friend std::ostream& operator << (std::ostream& os, const SHOT352& p); }; @@ -1308,10 +1409,12 @@ namespace pcl */ struct SHOT1344 { - float descriptor[1344]; - float rf[9]; + float descriptor[1344] = {0.f}; + float rf[9] = {0.f}; static int descriptorSize () { return 1344; } + inline SHOT1344 () = default; + friend std::ostream& operator << (std::ostream& os, const SHOT1344& p); }; @@ -1359,6 +1462,8 @@ namespace pcl std::fill_n(z_axis, 3, 0); } + // @TODO: add other ctors + friend std::ostream& operator << (std::ostream& os, const ReferenceFrame& p); PCL_MAKE_ALIGNED_OPERATOR_NEW }; @@ -1370,9 +1475,11 @@ namespace pcl */ struct FPFHSignature33 { - float histogram[33]; + float histogram[33] = {0.f}; static int descriptorSize () { return 33; } + inline FPFHSignature33 () = default; + friend std::ostream& operator << (std::ostream& os, const FPFHSignature33& p); }; @@ -1382,21 +1489,25 @@ namespace pcl */ struct VFHSignature308 { - float histogram[308]; + float histogram[308] = {0.f}; static int descriptorSize () { return 308; } + inline VFHSignature308 () = default; + friend std::ostream& operator << (std::ostream& os, const VFHSignature308& p); }; - + PCL_EXPORTS std::ostream& operator << (std::ostream& os, const GRSDSignature21& p); /** \brief A point structure representing the Global Radius-based Surface Descriptor (GRSD). * \ingroup common */ struct GRSDSignature21 { - float histogram[21]; + float histogram[21] = {0.f}; static int descriptorSize () { return 21; } + inline GRSDSignature21 () = default; + friend std::ostream& operator << (std::ostream& os, const GRSDSignature21& p); }; @@ -1406,11 +1517,15 @@ namespace pcl */ struct BRISKSignature512 { - float scale; - float orientation; - unsigned char descriptor[64]; + float scale = 0.f; + float orientation = 0.f; + unsigned char descriptor[64] = {0}; static int descriptorSize () { return 64; } + inline BRISKSignature512 () = default; + + inline BRISKSignature512 (float _scale, float _orientation): scale (_scale), orientation (_orientation) {} + friend std::ostream& operator << (std::ostream& os, const BRISKSignature512& p); }; @@ -1420,9 +1535,11 @@ namespace pcl */ struct ESFSignature640 { - float histogram[640]; + float histogram[640] = {0.f}; static int descriptorSize () { return 640; } + inline ESFSignature640 () = default; + friend std::ostream& operator << (std::ostream& os, const ESFSignature640& p); }; @@ -1432,9 +1549,11 @@ namespace pcl */ struct GASDSignature512 { - float histogram[512]; + float histogram[512] = {0.f}; static int descriptorSize() { return 512; } + inline GASDSignature512 () = default; + friend std::ostream& operator << (std::ostream& os, const GASDSignature512& p); }; @@ -1444,9 +1563,11 @@ namespace pcl */ struct GASDSignature984 { - float histogram[984]; + float histogram[984] = {0.f}; static int descriptorSize() { return 984; } + inline GASDSignature984 () = default; + friend std::ostream& operator << (std::ostream& os, const GASDSignature984& p); }; @@ -1456,9 +1577,11 @@ namespace pcl */ struct GASDSignature7992 { - float histogram[7992]; + float histogram[7992] = {0.f}; static int descriptorSize() { return 7992; } + inline GASDSignature7992 () = default; + friend std::ostream& operator << (std::ostream& os, const GASDSignature7992& p); }; @@ -1468,9 +1591,11 @@ namespace pcl */ struct GFPFHSignature16 { - float histogram[16]; + float histogram[16] = {0.f}; static int descriptorSize () { return 16; } - + + inline GFPFHSignature16 () = default; + friend std::ostream& operator << (std::ostream& os, const GFPFHSignature16& p); }; @@ -1480,10 +1605,17 @@ namespace pcl */ struct Narf36 { - float x, y, z, roll, pitch, yaw; - float descriptor[36]; + float x = 0.f, y = 0.f, z = 0.f, roll = 0.f, pitch = 0.f, yaw = 0.f; + float descriptor[36] = {0.f}; static int descriptorSize () { return 36; } + inline Narf36 () = default; + + inline Narf36 (float _x, float _y, float _z): Narf36 (_x, _y, _z, 0.f, 0.f, 0.f) {} + + inline Narf36 (float _x, float _y, float _z, float _roll, float _pitch, float _yaw): + x (_x), y (_y), z (_z), roll (_roll), pitch (_pitch), yaw (_yaw) {} + friend std::ostream& operator << (std::ostream& os, const Narf36& p); }; @@ -1493,10 +1625,14 @@ namespace pcl */ struct BorderDescription { - int x, y; + int x = 0.f, y = 0.f; BorderTraits traits; //std::vector neighbors; - + + inline BorderDescription () = default; + + // TODO: provide other ctors + friend std::ostream& operator << (std::ostream& os, const BorderDescription& p); }; @@ -1517,10 +1653,15 @@ namespace pcl float gradient_z; }; }; - + + inline IntensityGradient (): IntensityGradient (0.f, 0.f, 0.f) {} + + inline IntensityGradient (float _x, float _y, float _z): gradient_x (_x), gradient_y (_y), gradient_z (_z) {} + friend std::ostream& operator << (std::ostream& os, const IntensityGradient& p); }; + // TODO: Maybe make other histogram based structs an alias for this /** \brief A point structure representing an N-D histogram. * \ingroup common */ @@ -1567,40 +1708,19 @@ namespace pcl octave = p.octave; } - inline PointWithScale () - { - x = y = z = 0.0f; - scale = 1.0f; - angle = -1.0f; - response = 0.0f; - octave = 0; - data[3] = 1.0f; - } + inline PointWithScale (): PointWithScale (0.f, 0.f, 0.f) {} - inline PointWithScale (float _x, float _y, float _z, float _scale) + inline PointWithScale (float _x, float _y, float _z, float _scale = 1.f, + float _angle = -1.f, float _response = 0.f, int _octave = 0) { - x = _x; - y = _y; - z = _z; - scale = _scale; - angle = -1.0f; - response = 0.0f; - octave = 0; + x = _x; y = _y; z = _z; data[3] = 1.0f; - } - - inline PointWithScale (float _x, float _y, float _z, float _scale, float _angle, float _response, int _octave) - { - x = _x; - y = _y; - z = _z; scale = _scale; angle = _angle; response = _response; octave = _octave; - data[3] = 1.0f; } - + friend std::ostream& operator << (std::ostream& os, const PointWithScale& p); }; @@ -1648,7 +1768,9 @@ namespace pcl a = 255; radius = confidence = curvature = 0.0f; } - + + // TODO: add other ctor to PointSurfel + friend std::ostream& operator << (std::ostream& os, const PointSurfel& p); }; @@ -1675,11 +1797,18 @@ namespace pcl height_variance = p.height_variance; } - inline PointDEM () + inline PointDEM (): PointDEM (0.f, 0.f, 0.f) {} + + inline PointDEM (float _x, float _y, float _z): PointDEM (_x, _y, _z, 0.f, 0.f, 0.f) {} + + inline PointDEM (float _x, float _y, float _z, float _intensity, + float _intensity_variance, float _height_variance) { - x = y = z = 0.0f; data[3] = 1.0f; - intensity = 0.0f; - intensity_variance = height_variance = 0.0f; + x = _x; y = _y; z = _z; + data[3] = 1.0f; + intensity = _intensity; + intensity_variance = _intensity_variance; + height_variance = _height_variance; } friend std::ostream& operator << (std::ostream& os, const PointDEM& p); diff --git a/common/include/pcl/pcl_macros.h b/common/include/pcl/pcl_macros.h index d06ad4dc..b512dad5 100644 --- a/common/include/pcl/pcl_macros.h +++ b/common/include/pcl/pcl_macros.h @@ -76,6 +76,24 @@ #include +// It seems that __has_cpp_attribute doesn't work correctly +// when compiling with some versions of nvcc so we +// additionally check if nvcc is used before setting the +// PCL_DEPRECATED macro to [[deprecated]]. +#if defined(__has_cpp_attribute) && __has_cpp_attribute(deprecated) && !defined(__CUDACC__) + #define PCL_DEPRECATED(message) [[deprecated(message)]] +#elif defined(__GNUC__) || defined(__clang__) + #define PCL_DEPRECATED(message) __attribute__((deprecated(message))) +#elif defined(_MSC_VER) + // Until Visual Studio 2013 you had to use __declspec(deprecated). + // However, we decided to ignore the deprecation for these version because + // of simplicity reasons. See PR #3634 for the details. + #define PCL_DEPRECATED(message) +#else + #warning "You need to implement PCL_DEPRECATED for this compiler" + #define PCL_DEPRECATED(message) +#endif + namespace pcl { /** @@ -89,15 +107,15 @@ namespace pcl template using shared_ptr = boost::shared_ptr; - using uint8_t [[deprecated("use std::uint8_t instead of pcl::uint8_t")]] = std::uint8_t; - using int8_t [[deprecated("use std::int8_t instead of pcl::int8_t")]] = std::int8_t; - using uint16_t [[deprecated("use std::uint16_t instead of pcl::uint16_t")]] = std::uint16_t; - using int16_t [[deprecated("use std::uint16_t instead of pcl::int16_t")]] = std::int16_t; - using uint32_t [[deprecated("use std::uint32_t instead of pcl::uint32_t")]] = std::uint32_t; - using int32_t [[deprecated("use std::int32_t instead of pcl::int32_t")]] = std::int32_t; - using uint64_t [[deprecated("use std::uint64_t instead of pcl::uint64_t")]] = std::uint64_t; - using int64_t [[deprecated("use std::int64_t instead of pcl::int64_t")]] = std::int64_t; - using int_fast16_t [[deprecated("use std::int_fast16_t instead of pcl::int_fast16_t")]] = std::int_fast16_t; + using uint8_t PCL_DEPRECATED("use std::uint8_t instead of pcl::uint8_t") = std::uint8_t; + using int8_t PCL_DEPRECATED("use std::int8_t instead of pcl::int8_t") = std::int8_t; + using uint16_t PCL_DEPRECATED("use std::uint16_t instead of pcl::uint16_t") = std::uint16_t; + using int16_t PCL_DEPRECATED("use std::uint16_t instead of pcl::int16_t") = std::int16_t; + using uint32_t PCL_DEPRECATED("use std::uint32_t instead of pcl::uint32_t") = std::uint32_t; + using int32_t PCL_DEPRECATED("use std::int32_t instead of pcl::int32_t") = std::int32_t; + using uint64_t PCL_DEPRECATED("use std::uint64_t instead of pcl::uint64_t") = std::uint64_t; + using int64_t PCL_DEPRECATED("use std::int64_t instead of pcl::int64_t") = std::int64_t; + using int_fast16_t PCL_DEPRECATED("use std::int_fast16_t instead of pcl::int_fast16_t") = std::int_fast16_t; } #if defined _WIN32 && defined _MSC_VER @@ -134,15 +152,15 @@ namespace pcl template -[[deprecated("use std::isnan instead of pcl_isnan")]] +PCL_DEPRECATED("use std::isnan instead of pcl_isnan") bool pcl_isnan (T&& x) { return std::isnan (std::forward (x)); } template -[[deprecated("use std::isfinite instead of pcl_isfinite")]] +PCL_DEPRECATED("use std::isfinite instead of pcl_isfinite") bool pcl_isfinite (T&& x) { return std::isfinite (std::forward (x)); } template -[[deprecated("use std::isinf instead of pcl_isinf")]] +PCL_DEPRECATED("use std::isinf instead of pcl_isinf") bool pcl_isinf (T&& x) { return std::isinf (std::forward (x)); } diff --git a/common/include/pcl/point_cloud.h b/common/include/pcl/point_cloud.h index dfbe769f..b6e8266c 100644 --- a/common/include/pcl/point_cloud.h +++ b/common/include/pcl/point_cloud.h @@ -138,7 +138,7 @@ namespace pcl namespace detail { template - [[deprecated("use createMapping() instead")]] + PCL_DEPRECATED("use createMapping() instead") shared_ptr& getMapping (pcl::PointCloud& p); } // namespace detail @@ -606,7 +606,7 @@ namespace pcl protected: // This is motivated by ROS integration. Users should not need to access mapping_. - [[deprecated("rewrite your code to avoid using this protected field")]] shared_ptr mapping_; + PCL_DEPRECATED("rewrite your code to avoid using this protected field") shared_ptr mapping_; friend shared_ptr& detail::getMapping(pcl::PointCloud &p); diff --git a/common/include/pcl/point_representation.h b/common/include/pcl/point_representation.h index b5064a84..0a24ad82 100644 --- a/common/include/pcl/point_representation.h +++ b/common/include/pcl/point_representation.h @@ -40,6 +40,7 @@ #pragma once #include +#include #include #include diff --git a/common/include/pcl/point_traits.h b/common/include/pcl/point_traits.h index add14691..1ff6235e 100644 --- a/common/include/pcl/point_traits.h +++ b/common/include/pcl/point_traits.h @@ -197,7 +197,7 @@ namespace pcl return (field.name == traits::name::value && field.datatype == traits::datatype::value && (field.count == traits::datatype::size || - field.count == 0 && traits::datatype::size == 1 /* see bug #821 */)); + ((field.count == 0) && (traits::datatype::size == 1 /* see bug #821 */)))); } }; diff --git a/common/include/pcl/register_point_struct.h b/common/include/pcl/register_point_struct.h index 4b3f2c2e..32ac7a00 100644 --- a/common/include/pcl/register_point_struct.h +++ b/common/include/pcl/register_point_struct.h @@ -68,7 +68,7 @@ // Must be used in global namespace with name fully qualified #define POINT_CLOUD_REGISTER_POINT_STRUCT(name, fseq) \ POINT_CLOUD_REGISTER_POINT_STRUCT_I(name, \ - BOOST_PP_CAT(POINT_CLOUD_REGISTER_POINT_STRUCT_X fseq, 0)) \ + BOOST_PP_CAT(POINT_CLOUD_REGISTER_POINT_STRUCT_X fseq, 0)) /***/ #define POINT_CLOUD_REGISTER_POINT_WRAPPER(wrapper, pod) \ @@ -77,7 +77,7 @@ namespace traits { \ template<> struct POD { using type = pod; }; \ } \ - } \ + } /***/ // These macros help transform the unusual data structure (type, name, tag)(type, name, tag)... @@ -106,7 +106,7 @@ namespace pcl { using type = std::remove_all_extents_t; static const std::uint32_t count = sizeof (T) / sizeof (type); - for (int i = 0; i < count; ++i) + for (std::uint32_t i = 0; i < count; ++i) l[i] += r[i]; } @@ -123,7 +123,7 @@ namespace pcl { using type = std::remove_all_extents_t; static const std::uint32_t count = sizeof (T1) / sizeof (type); - for (int i = 0; i < count; ++i) + for (std::uint32_t i = 0; i < count; ++i) p[i] += scalar; } @@ -140,7 +140,7 @@ namespace pcl { using type = std::remove_all_extents_t; static const std::uint32_t count = sizeof (T) / sizeof (type); - for (int i = 0; i < count; ++i) + for (std::uint32_t i = 0; i < count; ++i) l[i] -= r[i]; } @@ -157,7 +157,7 @@ namespace pcl { using type = std::remove_all_extents_t; static const std::uint32_t count = sizeof (T1) / sizeof (type); - for (int i = 0; i < count; ++i) + for (std::uint32_t i = 0; i < count; ++i) p[i] -= scalar; } @@ -174,7 +174,7 @@ namespace pcl { using type = std::remove_all_extents_t; static const std::uint32_t count = sizeof (T1) / sizeof (type); - for (int i = 0; i < count; ++i) + for (std::uint32_t i = 0; i < count; ++i) p[i] *= scalar; } @@ -191,7 +191,7 @@ namespace pcl { using type = std::remove_all_extents_t; static const std::uint32_t count = sizeof (T1) / sizeof (type); - for (int i = 0; i < count; ++i) + for (std::uint32_t i = 0; i < count; ++i) p[i] /= scalar; } } @@ -200,34 +200,34 @@ namespace pcl // Point operators #define PCL_PLUSEQ_POINT_TAG(r, data, elem) \ pcl::traits::plus (lhs.BOOST_PP_TUPLE_ELEM(3, 1, elem), \ - rhs.BOOST_PP_TUPLE_ELEM(3, 1, elem)); \ + rhs.BOOST_PP_TUPLE_ELEM(3, 1, elem)); /***/ #define PCL_PLUSEQSC_POINT_TAG(r, data, elem) \ pcl::traits::plusscalar (p.BOOST_PP_TUPLE_ELEM(3, 1, elem), \ - scalar); \ + scalar); /***/ - //p.BOOST_PP_TUPLE_ELEM(3, 1, elem) += scalar; \ + //p.BOOST_PP_TUPLE_ELEM(3, 1, elem) += scalar; #define PCL_MINUSEQ_POINT_TAG(r, data, elem) \ pcl::traits::minus (lhs.BOOST_PP_TUPLE_ELEM(3, 1, elem), \ - rhs.BOOST_PP_TUPLE_ELEM(3, 1, elem)); \ + rhs.BOOST_PP_TUPLE_ELEM(3, 1, elem)); /***/ #define PCL_MINUSEQSC_POINT_TAG(r, data, elem) \ pcl::traits::minusscalar (p.BOOST_PP_TUPLE_ELEM(3, 1, elem), \ - scalar); \ + scalar); /***/ - //p.BOOST_PP_TUPLE_ELEM(3, 1, elem) -= scalar; \ + //p.BOOST_PP_TUPLE_ELEM(3, 1, elem) -= scalar; #define PCL_MULEQSC_POINT_TAG(r, data, elem) \ pcl::traits::mulscalar (p.BOOST_PP_TUPLE_ELEM(3, 1, elem), \ - scalar); \ + scalar); /***/ #define PCL_DIVEQSC_POINT_TAG(r, data, elem) \ pcl::traits::divscalar (p.BOOST_PP_TUPLE_ELEM(3, 1, elem), \ - scalar); \ + scalar); /***/ // Construct type traits given full sequence of (type, name, tag) triples @@ -306,7 +306,7 @@ namespace pcl inline const name operator/ (const name& p, const float& scalar) \ { name result = p; result /= scalar; return (result); } \ } \ - } \ + } /***/ #define POINT_CLOUD_REGISTER_FIELD_TAG(r, name, elem) \ @@ -324,14 +324,14 @@ namespace pcl const char name::value[] = \ - BOOST_PP_STRINGIZE(BOOST_PP_TUPLE_ELEM(3, 2, elem)); \ + BOOST_PP_STRINGIZE(BOOST_PP_TUPLE_ELEM(3, 2, elem)); /***/ #define POINT_CLOUD_REGISTER_FIELD_OFFSET(r, name, elem) \ template<> struct offset \ { \ static const std::size_t value = offsetof(name, BOOST_PP_TUPLE_ELEM(3, 1, elem)); \ - }; \ + }; /***/ #define POINT_CLOUD_REGISTER_FIELD_DATATYPE(r, name, elem) \ @@ -341,7 +341,7 @@ namespace pcl using decomposed = decomposeArray; \ static const std::uint8_t value = asEnum::value; \ static const std::uint32_t size = decomposed::value; \ - }; \ + }; /***/ #define POINT_CLOUD_TAG_OP(s, data, elem) pcl::fields::BOOST_PP_TUPLE_ELEM(3, 2, elem) @@ -352,7 +352,7 @@ namespace pcl template<> struct fieldList \ { \ using type = boost::mpl::vector; \ - }; \ + }; /***/ #if defined _MSC_VER diff --git a/common/src/parse.cpp b/common/src/parse.cpp index affa06e4..26917636 100644 --- a/common/src/parse.cpp +++ b/common/src/parse.cpp @@ -2,8 +2,7 @@ * Software License Agreement (BSD License) * * Point Cloud Library (PCL) - www.pointclouds.org - * Copyright (c) 2010-2012, Willow Garage, Inc. - * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2020-, Open Perception, Inc. * * All rights reserved. * @@ -37,17 +36,16 @@ */ #include +#include +#include #include +#include +#include + #include #include -#include -//////////////////////////////////////////////////////////////////////////////// -bool -pcl::console::find_switch (int argc, const char * const * argv, const char * argument_name) -{ - return (find_argument (argc, argv, argument_name) != -1); -} +#include //////////////////////////////////////////////////////////////////////////////// int @@ -64,6 +62,13 @@ pcl::console::find_argument (int argc, const char * const * argv, const char * a return (-1); } +//////////////////////////////////////////////////////////////////////////////// +bool +pcl::console::find_switch (int argc, const char * const * argv, const char * argument_name) +{ + return (find_argument (argc, argv, argument_name) != -1); +} + //////////////////////////////////////////////////////////////////////////////// int pcl::console::parse_argument (int argc, const char * const * argv, const char * str, std::string &val) @@ -76,63 +81,143 @@ pcl::console::parse_argument (int argc, const char * const * argv, const char * } //////////////////////////////////////////////////////////////////////////////// -int -pcl::console::parse_argument (int argc, const char * const * argv, const char * str, bool &val) +namespace pcl +{ +namespace console { +template int +parse_generic (V convert_func, int argc, const char* const* argv, const char* str, T& val) +{ + char *endptr = nullptr; int index = find_argument (argc, argv, str) + 1; + errno = 0; if (index > 0 && index < argc ) - val = atoi (argv[index]) == 1; + { + val = convert_func (argv[index], &endptr); // similar to strtol, strtod, strtof + // handle out-of-range, junk at the end and no conversion + if (errno == ERANGE || *endptr != '\0' || str == endptr) + { + return -1; + } + } return (index - 1); } -//////////////////////////////////////////////////////////////////////////////// int -pcl::console::parse_argument (int argc, const char * const * argv, const char * str, double &val) +parse_argument (int argc, const char * const * argv, const char * str, long int &val) noexcept { - int index = find_argument (argc, argv, str) + 1; - - if (index > 0 && index < argc ) - val = atof (argv[index]); + const auto strtol_10 = [](const char *str, char **str_end){ return strtol(str, str_end, 10); }; + return parse_generic(strtol_10, argc, argv, str, val); +} - return (index - 1); +int +parse_argument (int argc, const char * const * argv, const char * str, long long int &val) noexcept +{ + const auto strtoll_10 = [](const char *str, char **str_end){ return strtoll(str, str_end, 10); }; + return parse_generic(strtoll_10, argc, argv, str, val); } -//////////////////////////////////////////////////////////////////////////////// int -pcl::console::parse_argument (int argc, const char * const * argv, const char * str, float &val) +parse_argument (int argc, const char * const * argv, const char * str, unsigned long long int &val) noexcept { - int index = find_argument (argc, argv, str) + 1; + long long int dummy; + const auto ret = parse_argument (argc, argv, str, dummy); + if ((ret == -1) || dummy < 0) + { + return -1; + } + val = dummy; + return ret; +} - if (index > 0 && index < argc ) - val = static_cast (atof (argv[index])); +namespace detail +{ +template +constexpr auto legally_representable_v = (std::numeric_limits::max () >= std::numeric_limits::max ()) && + (std::numeric_limits::lowest () <= std::numeric_limits::lowest ()); +template +struct legally_representable { + constexpr static bool value = legally_representable_v; +}; + +// assumptions: +// * either long int or long long int is a valid type for storing Integral +// * unsigned long long int is handled specially +template +using primary_legal_input_type = std::conditional_t, + long int, long long int>; + +// special handling if unsigned [long] int is of same size as long long int +template +using legal_input_type = std::conditional_t<(std::is_unsigned::value && + (sizeof (Integral) == sizeof (long long int))), + unsigned long long int, + primary_legal_input_type>; +} - return (index - 1); +template +using IsIntegral = std::enable_if_t::value, bool>; + +template = true> int +parse_argument (int argc, const char * const * argv, const char * str, T &val) noexcept +{ + using InputType = detail::legal_input_type; + InputType dummy; + const auto ret = parse_argument (argc, argv, str, dummy); + if ((ret == -1) || + (dummy < static_cast (std::numeric_limits::min ())) || + (dummy > static_cast (std::numeric_limits::max ()))) + { + return -1; + } + + val = static_cast (dummy); + return ret; +} +} } //////////////////////////////////////////////////////////////////////////////// int -pcl::console::parse_argument (int argc, const char * const * argv, const char * str, int &val) +pcl::console::parse_argument (int argc, const char * const * argv, const char * str, double &val) { - int index = find_argument (argc, argv, str) + 1; - - if (index > 0 && index < argc ) - val = atoi (argv[index]); + return parse_generic(strtod, argc, argv, str, val); +} - return (index - 1); +//////////////////////////////////////////////////////////////////////////////// +int +pcl::console::parse_argument (int argc, const char * const * argv, const char * str, float &val) +{ + return parse_generic(strtof, argc, argv, str, val); } //////////////////////////////////////////////////////////////////////////////// int pcl::console::parse_argument (int argc, const char * const * argv, const char * str, unsigned int &val) { - int index = find_argument (argc, argv, str) + 1; + return parse_argument (argc, argv, str, val); +} - if (index > 0 && index < argc ) - val = atoi (argv[index]); +//////////////////////////////////////////////////////////////////////////////// +int +pcl::console::parse_argument (int argc, const char * const * argv, const char * str, int &val) +{ + return parse_argument (argc, argv, str, val); +} - return (index - 1); +//////////////////////////////////////////////////////////////////////////////// +int +pcl::console::parse_argument (int argc, const char * const * argv, const char * str, bool &val) +{ + long int dummy; + const auto ret = parse_argument (argc, argv, str, dummy); + if (ret != -1) + { + val = static_cast (dummy); + } + return ret; } //////////////////////////////////////////////////////////////////////////////// diff --git a/cuda/apps/src/kinect_mapping.cpp b/cuda/apps/src/kinect_mapping.cpp index 0706edb0..9072db83 100644 --- a/cuda/apps/src/kinect_mapping.cpp +++ b/cuda/apps/src/kinect_mapping.cpp @@ -237,33 +237,33 @@ class MultiRansac { this->use_viewer = use_viewer; //cudaDeviceSetCacheConfig (cudaFuncCachePreferL1); - pcl::Grabber* interface = new pcl::OpenNIGrabber(); + pcl::OpenNIGrabber interface {}; boost::signals2::connection c; if (use_device) { std::cerr << "[RANSAC] Using GPU..." << std::endl; std::function f = std::bind (&MultiRansac::cloud_cb, this, _1, _2, _3); - c = interface->registerCallback (f); + c = interface.registerCallback (f); } else { std::cerr << "[RANSAC] Using CPU..." << std::endl; std::function f = std::bind (&MultiRansac::cloud_cb, this, _1, _2, _3); - c = interface->registerCallback (f); + c = interface.registerCallback (f); } if (use_viewer) viewer.runOnVisualizationThread (std::bind(&MultiRansac::viz_cb, this, _1), "viz_cb"); - interface->start (); + interface.start (); while (!viewer.wasStopped ()) { sleep (1); } - interface->stop (); + interface.stop (); } pcl::PointCloud::ConstPtr logo_cloud_; diff --git a/cuda/apps/src/kinect_normals_cuda.cpp b/cuda/apps/src/kinect_normals_cuda.cpp index e36b1fc9..de659d2d 100644 --- a/cuda/apps/src/kinect_normals_cuda.cpp +++ b/cuda/apps/src/kinect_normals_cuda.cpp @@ -183,62 +183,60 @@ class NormalEstimation { if (use_file) { - pcl::Grabber* filegrabber = 0; float frames_per_second = 1; bool repeat = false; std::string path = "./frame_0.pcd"; - filegrabber = new pcl::PCDGrabber (path, frames_per_second, repeat); + pcl::PCDGrabber filegrabber {path, frames_per_second, repeat}; if (use_device) { std::cerr << "[NormalEstimation] Using GPU..." << std::endl; std::function::ConstPtr&)> f = std::bind (&NormalEstimation::file_cloud_cb, this, _1); - filegrabber->registerCallback (f); + filegrabber.registerCallback (f); } else { std::cerr << "[NormalEstimation] Using CPU..." << std::endl; std::function::ConstPtr&)> f = std::bind (&NormalEstimation::file_cloud_cb, this, _1); - filegrabber->registerCallback (f); + filegrabber.registerCallback (f); } - filegrabber->start (); + filegrabber.start (); while (go_on)//!viewer.wasStopped () && go_on) { pcl_sleep (1); } - filegrabber->stop (); + filegrabber.stop (); } else { - pcl::Grabber* grabber = new pcl::OpenNIGrabber(); + pcl::OpenNIGrabber grabber {}; - boost::signals2::connection c; if (use_device) { std::cerr << "[NormalEstimation] Using GPU..." << std::endl; std::function f = std::bind (&NormalEstimation::cloud_cb, this, _1, _2, _3); - c = grabber->registerCallback (f); + grabber.registerCallback (f); } else { std::cerr << "[NormalEstimation] Using CPU..." << std::endl; std::function f = std::bind (&NormalEstimation::cloud_cb, this, _1, _2, _3); - c = grabber->registerCallback (f); + grabber.registerCallback (f); } viewer.runOnVisualizationThread (std::bind(&NormalEstimation::viz_cb, this, _1), "viz_cb"); - grabber->start (); + grabber.start (); while (!viewer.wasStopped ()) { pcl_sleep (1); } - grabber->stop (); + grabber.stop (); } } diff --git a/cuda/apps/src/kinect_planes_cuda.cpp b/cuda/apps/src/kinect_planes_cuda.cpp index 728593f5..def05ff4 100644 --- a/cuda/apps/src/kinect_planes_cuda.cpp +++ b/cuda/apps/src/kinect_planes_cuda.cpp @@ -248,45 +248,42 @@ class MultiRansac { this->use_viewer = use_viewer; //cudaDeviceSetCacheConfig (cudaFuncCachePreferL1); - pcl::Grabber* interface = new pcl::OpenNIGrabber(); + pcl::OpenNIGrabber interface {}; boost::signals2::connection c; if (use_device) { std::cerr << "[RANSAC] Using GPU..." << std::endl; std::function f = std::bind (&MultiRansac::cloud_cb, this, _1, _2, _3); - c = interface->registerCallback (f); + c = interface.registerCallback (f); } else { std::cerr << "[RANSAC] Using CPU..." << std::endl; std::function f = std::bind (&MultiRansac::cloud_cb, this, _1, _2, _3); - c = interface->registerCallback (f); + c = interface.registerCallback (f); } if (use_viewer) viewer.runOnVisualizationThread (std::bind(&MultiRansac::viz_cb, this, _1), "viz_cb"); - interface->start (); + interface.start (); //--------------------- load pcl logo file - //pcl::Grabber* filegrabber = 0; - //float frames_per_second = 1; //bool repeat = false; //std::string path = "./pcl_logo.pcd"; - //if (!path.empty() && boost::filesystem::exists (path)) + //if (path.empty() || !boost::filesystem::exists (path)) //{ - // filegrabber = new pcl::PCDGrabber (path, frames_per_second, repeat); - //} - //else // std::cerr << "did not find file" << std::endl; + //} + //pcl::PCDGrabber filegrabber {path, frames_per_second, repeat}; // //std::function::ConstPtr&) > f = std::bind (&MultiRansac::logo_cb, this, _1); - //boost::signals2::connection c1 = filegrabber->registerCallback (f); + //boost::signals2::connection c1 = filegrabber.registerCallback (f); - //filegrabber->start (); + //filegrabber.start (); //------- END --------- load pcl logo file // while (!viewer.wasStopped ()) @@ -294,8 +291,8 @@ class MultiRansac pcl_sleep (1); } - //filegrabber->stop (); - interface->stop (); + //filegrabber.stop (); + interface.stop (); } pcl::PointCloud::ConstPtr logo_cloud_; diff --git a/cuda/apps/src/kinect_ransac.cpp b/cuda/apps/src/kinect_ransac.cpp index d1e1b51f..85f27a71 100644 --- a/cuda/apps/src/kinect_ransac.cpp +++ b/cuda/apps/src/kinect_ransac.cpp @@ -164,57 +164,55 @@ class SimpleKinectTool bool repeat = false; std::string path = "./frame_0.pcd"; - filegrabber = new pcl::PCDGrabber (path, frames_per_second, repeat); + pcl::PCDGrabber filegrabber (path, frames_per_second, repeat); if (use_device) { std::cerr << "[RANSAC] Using GPU..." << std::endl; std::function::ConstPtr&)> f = std::bind (&SimpleKinectTool::file_cloud_cb, this, _1); - filegrabber->registerCallback (f); + filegrabber.registerCallback (f); } else { std::cerr << "[RANSAC] Using CPU..." << std::endl; std::function::ConstPtr&)> f = std::bind (&SimpleKinectTool::file_cloud_cb, this, _1); - filegrabber->registerCallback (f); + filegrabber.registerCallback (f); } - filegrabber->start (); + filegrabber.start (); while (go_on)//!viewer.wasStopped () && go_on) { sleep (1); } - filegrabber->stop (); + filegrabber.stop (); //------- END --------- load pcl logo file #else - - pcl::Grabber* interface = new pcl::OpenNIGrabber(); - + pcl::OpenNIGrabber interface {}; boost::signals2::connection c; if (use_device) { std::cerr << "[RANSAC] Using GPU..." << std::endl; std::function f = std::bind (&SimpleKinectTool::cloud_cb, this, _1, _2, _3); - c = interface->registerCallback (f); + c = interface.registerCallback (f); } else { std::cerr << "[RANSAC] Using CPU..." << std::endl; std::function f = std::bind (&SimpleKinectTool::cloud_cb, this, _1, _2, _3); - c = interface->registerCallback (f); + c = interface.registerCallback (f); } //viewer.runOnVisualizationThread (fn, "viz_cb"); - interface->start (); + interface.start (); while (!viewer.wasStopped ()) { sleep (1); } - interface->stop (); + interface.stop (); #endif } diff --git a/cuda/apps/src/kinect_segmentation_cuda.cpp b/cuda/apps/src/kinect_segmentation_cuda.cpp index 36c57d3e..0227d0f9 100644 --- a/cuda/apps/src/kinect_segmentation_cuda.cpp +++ b/cuda/apps/src/kinect_segmentation_cuda.cpp @@ -363,56 +363,56 @@ class Segmentation bool repeat = false; std::string path = "./frame_0.pcd"; - filegrabber = new pcl::PCDGrabber (path, frames_per_second, repeat); + pcl::PCDGrabber filegrabber {path, frames_per_second, repeat}; if (use_device) { std::cerr << "[Segmentation] Using GPU..." << std::endl; std::function::ConstPtr&)> f = std::bind (&Segmentation::file_cloud_cb, this, _1); - filegrabber->registerCallback (f); + filegrabber.registerCallback (f); } else { // std::cerr << "[Segmentation] Using CPU..." << std::endl; // std::function::ConstPtr&)> f = std::bind (&Segmentation::file_cloud_cb, this, _1); -// filegrabber->registerCallback (f); +// filegrabber.registerCallback (f); } - filegrabber->start (); + filegrabber.start (); while (go_on)//!viewer.wasStopped () && go_on) { pcl_sleep (1); } - filegrabber->stop (); + filegrabber.stop (); } else { - pcl::Grabber* grabber = new pcl::OpenNIGrabber(); + pcl::OpenNIGrabber grabber {}; boost::signals2::connection c; if (use_device) { std::cerr << "[Segmentation] Using GPU..." << std::endl; std::function f = std::bind (&Segmentation::cloud_cb, this, _1, _2, _3); - c = grabber->registerCallback (f); + c = grabber.registerCallback (f); } else { // std::cerr << "[Segmentation] Using CPU..." << std::endl; // std::function f = std::bind (&Segmentation::cloud_cb, this, _1, _2, _3); -// c = grabber->registerCallback (f); +// c = grabber.registerCallback (f); } viewer.runOnVisualizationThread (std::bind(&Segmentation::viz_cb, this, _1), "viz_cb"); - grabber->start (); + grabber.start (); while (!viewer.wasStopped ()) { pcl_sleep (1); } - grabber->stop (); + grabber.stop (); } } diff --git a/cuda/apps/src/kinect_segmentation_planes_cuda.cpp b/cuda/apps/src/kinect_segmentation_planes_cuda.cpp index 8f4ca7de..af410b77 100644 --- a/cuda/apps/src/kinect_segmentation_planes_cuda.cpp +++ b/cuda/apps/src/kinect_segmentation_planes_cuda.cpp @@ -232,62 +232,60 @@ class Segmentation { if (use_file) { - pcl::Grabber* filegrabber = 0; - float frames_per_second = 1; bool repeat = false; std::string path = "./frame_0.pcd"; - filegrabber = new pcl::PCDGrabber (path, frames_per_second, repeat); + pcl::PCDGrabber filegrabber {path, frames_per_second, repeat}; if (use_device) { std::cerr << "[Segmentation] Using GPU..." << std::endl; std::function::ConstPtr&)> f = std::bind (&Segmentation::file_cloud_cb, this, _1); - filegrabber->registerCallback (f); + filegrabber.registerCallback (f); } else { // std::cerr << "[Segmentation] Using CPU..." << std::endl; // std::function::ConstPtr&)> f = std::bind (&Segmentation::file_cloud_cb, this, _1); -// filegrabber->registerCallback (f); +// filegrabber.registerCallback (f); } - filegrabber->start (); + filegrabber.start (); while (go_on)//!viewer.wasStopped () && go_on) { pcl_sleep (1); } - filegrabber->stop (); + filegrabber.stop (); } else { - pcl::Grabber* grabber = new pcl::OpenNIGrabber(); + pcl::OpenNIGrabber grabber {}; boost::signals2::connection c; if (use_device) { std::cerr << "[Segmentation] Using GPU..." << std::endl; std::function f = std::bind (&Segmentation::cloud_cb, this, _1, _2, _3); - c = grabber->registerCallback (f); + c = grabber.registerCallback (f); } else { // std::cerr << "[Segmentation] Using CPU..." << std::endl; // std::function f = std::bind (&Segmentation::cloud_cb, this, _1, _2, _3); -// c = grabber->registerCallback (f); +// c = grabber.registerCallback (f); } viewer.runOnVisualizationThread (std::bind(&Segmentation::viz_cb, this, _1), "viz_cb"); - grabber->start (); + grabber.start (); while (!viewer.wasStopped ()) { pcl_sleep (1); } - grabber->stop (); + grabber.stop (); } } diff --git a/cuda/common/include/pcl/cuda/point_cloud.h b/cuda/common/include/pcl/cuda/point_cloud.h index 8aee4375..fb78b70f 100644 --- a/cuda/common/include/pcl/cuda/point_cloud.h +++ b/cuda/common/include/pcl/cuda/point_cloud.h @@ -39,7 +39,7 @@ #include #include -#include +#include namespace pcl { diff --git a/cuda/nn/organized_neighbor_search.hpp b/cuda/nn/organized_neighbor_search.hpp index 4dfce7c1..86fb9baa 100644 --- a/cuda/nn/organized_neighbor_search.hpp +++ b/cuda/nn/organized_neighbor_search.hpp @@ -163,11 +163,8 @@ namespace pcl { int x_pos, y_pos, x, y, idx; - int leftX, rightX, leftY, rightY; - int radiusSearchPointCount; - int maxSearchDistance; double squaredMaxSearchRadius; assert (k_arg>0); @@ -206,7 +203,7 @@ namespace pcl // select point from organized pointcloud x = x_pos + (*radiusSearchLookup_Iterator).x_diff_; y = y_pos + (*radiusSearchLookup_Iterator).y_diff_; - radiusSearchLookup_Iterator++; + ++radiusSearchLookup_Iterator; radiusSearchPointCount++; if ((x >= 0) && (y >= 0) && (x < (int)input_->width) && (y < (int)input_->height)) @@ -254,6 +251,7 @@ namespace pcl squared_radius = std::min(nearestNeighbors.back ().squared_distance_, squaredMaxSearchRadius); + int leftX, rightX, leftY, rightY; this->getProjectedRadiusSearchBox(p_q_arg, squared_radius, leftX, rightX, leftY, rightY); leftX *=leftX; @@ -264,7 +262,7 @@ namespace pcl pointCountRadiusSearch = (rightX-leftX)*(rightY-leftY); // search for maximum distance between search point to window borders in 2-D search window - maxSearchDistance = 0; + int maxSearchDistance = 0; maxSearchDistance = std::max (maxSearchDistance, leftX + leftY); maxSearchDistance = std::max (maxSearchDistance, leftX + rightY); maxSearchDistance = std::max (maxSearchDistance, rightX + leftY); @@ -284,7 +282,7 @@ namespace pcl // select point from organized point cloud x = x_pos + (*radiusSearchLookup_Iterator).x_diff_; y = y_pos + (*radiusSearchLookup_Iterator).y_diff_; - radiusSearchLookup_Iterator++; + ++radiusSearchLookup_Iterator; if ((x >= 0) && (y >= 0) && (x < (int)input_->width) && (y < (int)input_->height)) { diff --git a/doc/tutorials/content/compression.rst b/doc/tutorials/content/compression.rst index f3600e56..2825ba2e 100644 --- a/doc/tutorials/content/compression.rst +++ b/doc/tutorials/content/compression.rst @@ -210,13 +210,13 @@ Command line tool for PCL point cloud stream compression -------------------------------------------------------- The pcl apps component contains a command line tool for point cloud compression -and streaming: Simply execute "./openni_stream_compression -?" to see a full +and streaming: Simply execute "./pcl_openni_octree_compression -?" to see a full list of options (note: the output on screen may differ):: PCL point cloud stream compression - usage: ./openni_stream_compression [mode] [profile] [parameters] + usage: ./pcl_openni_octree_compression [mode] [profile] [parameters] I/O: -f file : file name @@ -249,15 +249,15 @@ list of options (note: the output on screen may differ):: -e : show input cloud during encoding example: - ./openni_stream_compression -x -p highC -t -f pc_compressed.pcc + ./pcl_openni_octree_compression -x -p highC -t -f pc_compressed.pcc In order to stream compressed point cloud via TCP/IP, you can start the server with:: - $ ./openni_stream_compression -s + $ ./pcl_openni_octree_compression -s It will listen on port 6666 for incoming connections. Now start the client with:: - $ ./openni_stream_compression -c SERVER_NAME + $ ./pcl_openni_octree_compression -c SERVER_NAME and remotely captured point clouds will be locally shown in the point cloud viewer. diff --git a/doc/tutorials/content/davidsdk.rst b/doc/tutorials/content/davidsdk.rst index 52e45088..b28eae96 100644 --- a/doc/tutorials/content/davidsdk.rst +++ b/doc/tutorials/content/davidsdk.rst @@ -14,9 +14,9 @@ Install davidSDK You need a davidSDK to run the SDK on the server side, the official davidSDK does not come with a Makefile or a CMake project. An un-official fork provides a CMake project that enables to easily use the SDK under Linux (with minor tweaks) * `Official davidSDK download page `_ - * `Victor Lamoine davidSDK fork `_ + * `Victor Lamoine davidSDK fork `_ -Please test `the example project `_ before going further. +Please test `the example project `_ before going further. .. note:: If you use the trial version of the server, the only format available is OBJ (used by default) diff --git a/examples/segmentation/example_cpc_segmentation.cpp b/examples/segmentation/example_cpc_segmentation.cpp index 336c5eba..bdd29409 100644 --- a/examples/segmentation/example_cpc_segmentation.cpp +++ b/examples/segmentation/example_cpc_segmentation.cpp @@ -452,8 +452,6 @@ CPCSegmentation Parameters: \n\ using AdjacencyIterator = LCCPSegmentation::AdjacencyIterator; using EdgeID = LCCPSegmentation::EdgeID; - std::set edge_drawn; - const unsigned char black_color [3] = {0, 0, 0}; const unsigned char white_color [3] = {255, 255, 255}; const unsigned char concave_color [3] = {255, 0, 0}; diff --git a/examples/segmentation/example_extract_clusters_normals.cpp b/examples/segmentation/example_extract_clusters_normals.cpp index 09af47b7..684cf2a3 100644 --- a/examples/segmentation/example_extract_clusters_normals.cpp +++ b/examples/segmentation/example_extract_clusters_normals.cpp @@ -79,7 +79,6 @@ main (int, char **argv) tree_ec->setInputCloud (cloud_ptr); // Extracting Euclidean clusters using cloud and its normals - std::vector indices; std::vector cluster_indices; const float tolerance = 0.5f; // 50cm tolerance in (x, y, z) coordinate system const double eps_angle = 5 * (M_PI / 180.0); // 5degree tolerance in normals diff --git a/examples/segmentation/example_lccp_segmentation.cpp b/examples/segmentation/example_lccp_segmentation.cpp index b3e69f8f..e7e0dcb7 100644 --- a/examples/segmentation/example_lccp_segmentation.cpp +++ b/examples/segmentation/example_lccp_segmentation.cpp @@ -377,8 +377,6 @@ LCCPSegmentation Parameters: \n\ using AdjacencyIterator = LCCPSegmentation::AdjacencyIterator; using EdgeID = LCCPSegmentation::EdgeID; - std::set edge_drawn; - const unsigned char convex_color [3] = {255, 255, 255}; const unsigned char concave_color [3] = {255, 0, 0}; const unsigned char* color; diff --git a/features/CMakeLists.txt b/features/CMakeLists.txt index b9c05b7e..bbc5d5cf 100644 --- a/features/CMakeLists.txt +++ b/features/CMakeLists.txt @@ -130,6 +130,7 @@ set(srcs src/crh.cpp src/don.cpp src/fpfh.cpp + src/from_meshes.cpp src/gasd.cpp src/gfpfh.cpp src/integral_image_normal.cpp diff --git a/features/include/pcl/features/cvfh.h b/features/include/pcl/features/cvfh.h index cf73c112..a7b20672 100644 --- a/features/include/pcl/features/cvfh.h +++ b/features/include/pcl/features/cvfh.h @@ -147,8 +147,8 @@ namespace pcl inline void getCentroidClusters (std::vector > & centroids) { - for (const auto& centroid: centroids_dominant_orientations_) - centroids.push_back (centroid); + centroids.insert (centroids.cend (), centroids_dominant_orientations_.cbegin (), + centroids_dominant_orientations_.cend ()); } /** \brief Get the normal centroids used to compute different CVFH descriptors diff --git a/features/include/pcl/features/from_meshes.h b/features/include/pcl/features/from_meshes.h index e4c25578..b7e6baba 100644 --- a/features/include/pcl/features/from_meshes.h +++ b/features/include/pcl/features/from_meshes.h @@ -52,7 +52,7 @@ namespace pcl } - /** \brief Compute GICP-style covariance matrices given a point cloud and + /** \brief Compute GICP-style covariance matrices given a point cloud and * the corresponding surface normals. * \param[in] cloud Point cloud containing the XYZ coordinates, * \param[in] normals Point cloud containing the corresponding surface normals. @@ -60,7 +60,7 @@ namespace pcl * \param[in] Optional: Epsilon for the expected noise along the surface normal (default: 0.001) */ template inline void - computeApproximateCovariances(const pcl::PointCloud& cloud, + computeApproximateCovariances(const pcl::PointCloud& cloud, const pcl::PointCloud& normals, std::vector >& covariances, double epsilon = 0.001) @@ -72,9 +72,9 @@ namespace pcl covariances.reserve (nr_points); for (const auto& point: normals.points) { - Eigen::Vector3d normal (normals.points[i].normal_x, - normals.points[i].normal_y, - normals.points[i].normal_z); + Eigen::Vector3d normal (point.normal_x, + point.normal_y, + point.normal_z); // compute rotation matrix Eigen::Matrix3d rot; @@ -97,3 +97,6 @@ namespace pcl } } + +#define PCL_INSTANTIATE_computeApproximateCovariances(T,NT) template PCL_EXPORTS void pcl::features::computeApproximateCovariances \ + (const pcl::PointCloud&, const pcl::PointCloud&, std::vector>&, double); diff --git a/features/include/pcl/features/impl/3dsc.hpp b/features/include/pcl/features/impl/3dsc.hpp index e25afa61..7b9386ff 100644 --- a/features/include/pcl/features/impl/3dsc.hpp +++ b/features/include/pcl/features/impl/3dsc.hpp @@ -80,14 +80,14 @@ pcl::ShapeContext3DEstimation::initCompute () radii_interval_[j] = static_cast (std::exp (std::log (min_radius_) + ((static_cast (j) / static_cast (radius_bins_)) * std::log (search_radius_ / min_radius_)))); // Fill theta divisions of elevation - theta_divisions_.resize (elevation_bins_ + 1); - for (std::size_t k = 0; k < elevation_bins_ + 1; k++) - theta_divisions_[k] = static_cast (k) * elevation_interval; + theta_divisions_.resize (elevation_bins_ + 1, elevation_interval); + theta_divisions_[0] = 0.f; + std::partial_sum(theta_divisions_.begin (), theta_divisions_.end (), theta_divisions_.begin ()); // Fill phi didvisions of elevation - phi_divisions_.resize (azimuth_bins_ + 1); - for (std::size_t l = 0; l < azimuth_bins_ + 1; l++) - phi_divisions_[l] = static_cast (l) * azimuth_interval; + phi_divisions_.resize (azimuth_bins_ + 1, azimuth_interval); + phi_divisions_[0] = 0.f; + std::partial_sum(phi_divisions_.begin (), phi_divisions_.end (), phi_divisions_.begin ()); // LookUp Table that contains the volume of all the bins // "phi" term of the volume integral @@ -141,23 +141,13 @@ pcl::ShapeContext3DEstimation::computePoint ( const std::size_t neighb_cnt = searchForNeighbors ((*indices_)[index], search_radius_, nn_indices, nn_dists); if (neighb_cnt == 0) { - for (float &descriptor : desc) - descriptor = std::numeric_limits::quiet_NaN (); - - memset (rf, 0, sizeof (rf[0]) * 9); + std::fill (desc.begin (), desc.end (), std::numeric_limits::quiet_NaN ()); + std::fill (rf, rf + 9, 0.f); return (false); } - float minDist = std::numeric_limits::max (); - int minIndex = -1; - for (std::size_t i = 0; i < nn_indices.size (); i++) - { - if (nn_dists[i] < minDist) - { - minDist = nn_dists[i]; - minIndex = nn_indices[i]; - } - } + const auto minDistanceIt = std::min_element(nn_dists.begin (), nn_dists.end ()); + const auto minIndex = nn_indices[std::distance (nn_dists.begin (), minDistanceIt)]; // Get origin point Vector3fMapConst origin = input_->points[(*indices_)[index]].getVector3fMap (); @@ -214,38 +204,15 @@ pcl::ShapeContext3DEstimation::computePoint ( float theta = normal.dot (no); theta = pcl::rad2deg (std::acos (std::min (1.0f, std::max (-1.0f, theta)))); - // Bin (j, k, l) - std::size_t j = 0; - std::size_t k = 0; - std::size_t l = 0; - // Compute the Bin(j, k, l) coordinates of current neighbour - for (std::size_t rad = 1; rad < radius_bins_+1; rad++) - { - if (r <= radii_interval_[rad]) - { - j = rad-1; - break; - } - } - - for (std::size_t ang = 1; ang < elevation_bins_+1; ang++) - { - if (theta <= theta_divisions_[ang]) - { - k = ang-1; - break; - } - } + const auto rad_min = std::lower_bound(std::next (radii_interval_.cbegin ()), radii_interval_.cend (), r); + const auto theta_min = std::lower_bound(std::next (theta_divisions_.cbegin ()), theta_divisions_.cend (), theta); + const auto phi_min = std::lower_bound(std::next (phi_divisions_.cbegin ()), phi_divisions_.cend (), phi); - for (std::size_t ang = 1; ang < azimuth_bins_+1; ang++) - { - if (phi <= phi_divisions_[ang]) - { - l = ang-1; - break; - } - } + // Bin (j, k, l) + const auto j = std::distance(radii_interval_.cbegin (), std::prev(rad_min)); + const auto k = std::distance(theta_divisions_.cbegin (), std::prev(theta_min)); + const auto l = std::distance(phi_divisions_.cbegin (), std::prev(phi_min)); // Local point density = number of points in a sphere of radius "point_density_radius_" around the current neighbour std::vector neighbour_indices; @@ -270,7 +237,7 @@ pcl::ShapeContext3DEstimation::computePoint ( } // end for each neighbour // 3DSC does not define a repeatable local RF, we set it to zero to signal it to the user - memset (rf, 0, sizeof (rf[0]) * 9); + std::fill (rf, rf + 9, 0); return (true); } @@ -289,10 +256,9 @@ pcl::ShapeContext3DEstimation::computeFeature (Poi // If the point is not finite, set the descriptor to NaN and continue if (!isFinite ((*input_)[(*indices_)[point_index]])) { - for (std::size_t i = 0; i < descriptor_length_; ++i) - output[point_index].descriptor[i] = std::numeric_limits::quiet_NaN (); - - memset (output[point_index].rf, 0, sizeof (output[point_index].rf[0]) * 9); + std::fill (output[point_index].descriptor, output[point_index].descriptor + descriptor_length_, + std::numeric_limits::quiet_NaN ()); + std::fill (output[point_index].rf, output[point_index].rf + 9, 0); output.is_dense = false; continue; } @@ -300,8 +266,7 @@ pcl::ShapeContext3DEstimation::computeFeature (Poi std::vector descriptor (descriptor_length_); if (!computePoint (point_index, *normals_, output[point_index].rf, descriptor)) output.is_dense = false; - for (std::size_t j = 0; j < descriptor_length_; ++j) - output[point_index].descriptor[j] = descriptor[j]; + std::copy (descriptor.begin (), descriptor.end (), output[point_index].descriptor); } } diff --git a/features/include/pcl/features/impl/board.hpp b/features/include/pcl/features/impl/board.hpp index da4f54ca..fcd1a416 100644 --- a/features/include/pcl/features/impl/board.hpp +++ b/features/include/pcl/features/impl/board.hpp @@ -145,23 +145,10 @@ pcl::BOARDLocalReferenceFrameEstimation::planeFitt } //find the center by averaging the points positions - center.setZero (); - - for (int i = 0; i < n_points; ++i) - { - center += points.row (i); - } - - center /= static_cast (n_points); + center = points.colwise().mean().transpose(); //copy points - average (center) - Eigen::Matrix A (n_points, 3); //PointData - for (int i = 0; i < n_points; ++i) - { - A (i, 0) = points (i, 0) - center.x (); - A (i, 1) = points (i, 1) - center.y (); - A (i, 2) = points (i, 2) - center.z (); - } + const Eigen::Matrix A = points.rowwise() - center.transpose(); Eigen::JacobiSVD svd (A, Eigen::ComputeFullV); norm = svd.matrixV ().col (2); @@ -265,14 +252,12 @@ pcl::BOARDLocalReferenceFrameEstimation::computePo lrf.row (0).matrix () = x_axis; - for (int i = 0; i < check_margin_array_size_; i++) - { - check_margin_array_[i] = false; - margin_array_min_angle_[i] = std::numeric_limits::max (); - margin_array_max_angle_[i] = -std::numeric_limits::max (); - margin_array_min_angle_normal_[i] = -1.0; - margin_array_max_angle_normal_[i] = -1.0; - } + check_margin_array_.assign(check_margin_array_size_, false); + margin_array_min_angle_.assign(check_margin_array_size_, std::numeric_limits::max ()); + margin_array_max_angle_.assign(check_margin_array_size_, -std::numeric_limits::max ()); + margin_array_min_angle_normal_.assign(check_margin_array_size_, -1.0); + margin_array_max_angle_normal_.assign(check_margin_array_size_, -1.0); + max_boundary_angle = (2 * static_cast (M_PI)) / static_cast (check_margin_array_size_); } @@ -448,23 +433,16 @@ pcl::BOARDLocalReferenceFrameEstimation::computePo int hole_end; int hole_first; - //find first no border pie - int first_no_border = -1; - if (check_margin_array_[check_margin_array_size_ - 1]) - { - first_no_border = 0; - } - else - { - for (int i = 0; i < check_margin_array_size_; i++) + const auto find_first_no_border_pie = [](const auto& array) -> std::size_t { + if (array.back()) { - if (check_margin_array_[i]) - { - first_no_border = i; - break; - } + return 0; } - } + const auto result = std::find_if(array.cbegin (), array.cend (), + [](const auto& x) -> bool { return x;}); + return std::distance(array.cbegin (), result); + }; + const auto first_no_border = find_first_no_border_pie(check_margin_array_); //float steep_prob = 0.0; float max_hole_prob = -std::numeric_limits::max (); diff --git a/features/include/pcl/features/impl/brisk_2d.hpp b/features/include/pcl/features/impl/brisk_2d.hpp index 9c12de92..0ee753e3 100644 --- a/features/include/pcl/features/impl/brisk_2d.hpp +++ b/features/include/pcl/features/impl/brisk_2d.hpp @@ -128,17 +128,16 @@ pcl::BRISK2DEstimation::generateKern size_list_[scale] = 0; // generate the pattern points look-up - double alpha, theta; for (std::size_t rot = 0; rot < n_rot_; ++rot) { // this is the rotation of the feature - theta = double (rot) * 2 * M_PI / double (n_rot_); + double theta = double (rot) * 2 * M_PI / double (n_rot_); for (int ring = 0; ring < rings; ++ring) { for (int num = 0; num < number_list[ring]; ++num) { // the actual coordinates on the circle - alpha = double (num) * 2 * M_PI / double (number_list[ring]); + double alpha = double (num) * 2 * M_PI / double (number_list[ring]); // feature rotation plus angle of the point pattern_iterator->x = scale_list_[scale] * radius_list[ring] * static_cast (std::cos (alpha + theta)); diff --git a/features/include/pcl/features/impl/crh.hpp b/features/include/pcl/features/impl/crh.hpp index 2bbb21fe..4146c446 100644 --- a/features/include/pcl/features/impl/crh.hpp +++ b/features/include/pcl/features/impl/crh.hpp @@ -98,12 +98,11 @@ pcl::CRHEstimation::computeFeature (PointCloudOut // the initialization is made with () after the [nbins] std::vector spatial_data(nbins); - float sum_w = 0, w = 0; - int bin = 0; + float sum_w = 0; for (const auto &point : grid.points) { - bin = static_cast ((((std::atan2 (point.normal_y, point.normal_x) + M_PI) * 180 / M_PI) / bin_angle)) % nbins; - w = std::sqrt (point.normal_y * point.normal_y + point.normal_x * point.normal_x); + int bin = static_cast ((((std::atan2 (point.normal_y, point.normal_x) + M_PI) * 180 / M_PI) / bin_angle)) % nbins; + float w = std::sqrt (point.normal_y * point.normal_y + point.normal_x * point.normal_x); sum_w += w; spatial_data[bin] += w; } diff --git a/features/include/pcl/features/impl/esf.hpp b/features/include/pcl/features/impl/esf.hpp index 93096f9a..58ef7201 100644 --- a/features/include/pcl/features/impl/esf.hpp +++ b/features/include/pcl/features/impl/esf.hpp @@ -58,7 +58,6 @@ pcl::ESFEstimation::computeESF ( srand (static_cast (time (nullptr))); int maxindex = static_cast (pc.points.size ()); - int index1, index2, index3; std::vector d2v, d1v, d3v, wt_d3; std::vector wt_d2; d1v.reserve (sample_size); @@ -89,9 +88,9 @@ pcl::ESFEstimation::computeESF ( for (std::size_t nn_idx = 0; nn_idx < sample_size; ++nn_idx) { // get a new random point - index1 = rand()%maxindex; - index2 = rand()%maxindex; - index3 = rand()%maxindex; + int index1 = rand()%maxindex; + int index2 = rand()%maxindex; + int index3 = rand()%maxindex; if (index1==index2 || index1 == index3 || index2 == index3) { @@ -425,20 +424,19 @@ pcl::ESFEstimation::lci ( template void pcl::ESFEstimation::voxelize9 (PointCloudIn &cluster) { - int xi,yi,zi,xx,yy,zz; for (std::size_t i = 0; i < cluster.points.size (); ++i) { - xx = cluster.points[i].x<0.0? static_cast(std::floor(cluster.points[i].x)+GRIDSIZE_H) : static_cast(std::ceil(cluster.points[i].x)+GRIDSIZE_H-1); - yy = cluster.points[i].y<0.0? static_cast(std::floor(cluster.points[i].y)+GRIDSIZE_H) : static_cast(std::ceil(cluster.points[i].y)+GRIDSIZE_H-1); - zz = cluster.points[i].z<0.0? static_cast(std::floor(cluster.points[i].z)+GRIDSIZE_H) : static_cast(std::ceil(cluster.points[i].z)+GRIDSIZE_H-1); + int xx = cluster.points[i].x<0.0? static_cast(std::floor(cluster.points[i].x)+GRIDSIZE_H) : static_cast(std::ceil(cluster.points[i].x)+GRIDSIZE_H-1); + int yy = cluster.points[i].y<0.0? static_cast(std::floor(cluster.points[i].y)+GRIDSIZE_H) : static_cast(std::ceil(cluster.points[i].y)+GRIDSIZE_H-1); + int zz = cluster.points[i].z<0.0? static_cast(std::floor(cluster.points[i].z)+GRIDSIZE_H) : static_cast(std::ceil(cluster.points[i].z)+GRIDSIZE_H-1); for (int x = -1; x < 2; x++) for (int y = -1; y < 2; y++) for (int z = -1; z < 2; z++) { - xi = xx + x; - yi = yy + y; - zi = zz + z; + int xi = xx + x; + int yi = yy + y; + int zi = zz + z; if (yi >= GRIDSIZE || xi >= GRIDSIZE || zi>=GRIDSIZE || yi < 0 || xi < 0 || zi < 0) { @@ -454,20 +452,19 @@ pcl::ESFEstimation::voxelize9 (PointCloudIn &cluster) template void pcl::ESFEstimation::cleanup9 (PointCloudIn &cluster) { - int xi,yi,zi,xx,yy,zz; for (std::size_t i = 0; i < cluster.points.size (); ++i) { - xx = cluster.points[i].x<0.0? static_cast(std::floor(cluster.points[i].x)+GRIDSIZE_H) : static_cast(std::ceil(cluster.points[i].x)+GRIDSIZE_H-1); - yy = cluster.points[i].y<0.0? static_cast(std::floor(cluster.points[i].y)+GRIDSIZE_H) : static_cast(std::ceil(cluster.points[i].y)+GRIDSIZE_H-1); - zz = cluster.points[i].z<0.0? static_cast(std::floor(cluster.points[i].z)+GRIDSIZE_H) : static_cast(std::ceil(cluster.points[i].z)+GRIDSIZE_H-1); + int xx = cluster.points[i].x<0.0? static_cast(std::floor(cluster.points[i].x)+GRIDSIZE_H) : static_cast(std::ceil(cluster.points[i].x)+GRIDSIZE_H-1); + int yy = cluster.points[i].y<0.0? static_cast(std::floor(cluster.points[i].y)+GRIDSIZE_H) : static_cast(std::ceil(cluster.points[i].y)+GRIDSIZE_H-1); + int zz = cluster.points[i].z<0.0? static_cast(std::floor(cluster.points[i].z)+GRIDSIZE_H) : static_cast(std::ceil(cluster.points[i].z)+GRIDSIZE_H-1); for (int x = -1; x < 2; x++) for (int y = -1; y < 2; y++) for (int z = -1; z < 2; z++) { - xi = xx + x; - yi = yy + y; - zi = zz + z; + int xi = xx + x; + int yi = yy + y; + int zi = zz + z; if (yi >= GRIDSIZE || xi >= GRIDSIZE || zi>=GRIDSIZE || yi < 0 || xi < 0 || zi < 0) { @@ -487,12 +484,12 @@ pcl::ESFEstimation::scale_points_unit_sphere ( pcl::compute3DCentroid (pc, centroid); pcl::demeanPointCloud (pc, centroid, local_cloud_); - float max_distance = 0, d; + float max_distance = 0; pcl::PointXYZ cog (0, 0, 0); for (std::size_t i = 0; i < local_cloud_.points.size (); ++i) { - d = pcl::euclideanDistance(cog,local_cloud_.points[i]); + float d = pcl::euclideanDistance(cog,local_cloud_.points[i]); if (d > max_distance) max_distance = d; } diff --git a/features/include/pcl/features/impl/fpfh.hpp b/features/include/pcl/features/impl/fpfh.hpp index 83ab5c4b..e2bf64dd 100644 --- a/features/include/pcl/features/impl/fpfh.hpp +++ b/features/include/pcl/features/impl/fpfh.hpp @@ -65,6 +65,7 @@ pcl::FPFHEstimation::computePointSPFHSignature ( { Eigen::Vector4f pfh_tuple; // Get the number of bins from the histograms size + // @TODO: use arrays int nr_bins_f1 = static_cast (hist_f1.cols ()); int nr_bins_f2 = static_cast (hist_f2.cols ()); int nr_bins_f3 = static_cast (hist_f3.cols ()); @@ -108,6 +109,7 @@ pcl::FPFHEstimation::weightPointSPFHSignature ( const std::vector &indices, const std::vector &dists, Eigen::VectorXf &fpfh_histogram) { assert (indices.size () == dists.size ()); + // @TODO: use arrays double sum_f1 = 0.0, sum_f2 = 0.0, sum_f3 = 0.0; float weight = 0.0, val_f1, val_f2, val_f3; @@ -161,12 +163,15 @@ pcl::FPFHEstimation::weightPointSPFHSignature ( sum_f3 = 100.0 / sum_f3; // histogram values sum up to 100 // Adjust final FPFH values - for (int f1_i = 0; f1_i < nr_bins_f1; ++f1_i) - fpfh_histogram[f1_i] *= static_cast (sum_f1); - for (int f2_i = 0; f2_i < nr_bins_f2; ++f2_i) - fpfh_histogram[f2_i + nr_bins_f1] *= static_cast (sum_f2); - for (int f3_i = 0; f3_i < nr_bins_f3; ++f3_i) - fpfh_histogram[f3_i + nr_bins_f12] *= static_cast (sum_f3); + const auto denormalize_with = [](auto factor) + { + return [=](const auto& data) { return data * factor; }; + }; + + auto last = fpfh_histogram.data (); + last = std::transform(last, last + nr_bins_f1, last, denormalize_with (sum_f1)); + last = std::transform(last, last + nr_bins_f2, last, denormalize_with (sum_f2)); + std::transform(last, last + nr_bins_f3, last, denormalize_with (sum_f3)); } ////////////////////////////////////////////////////////////////////////////////////////////// @@ -262,8 +267,7 @@ pcl::FPFHEstimation::computeFeature (PointCloudOut weightPointSPFHSignature (hist_f1_, hist_f2_, hist_f3_, nn_indices, nn_dists, fpfh_histogram_); // ...and copy it into the output cloud - for (Eigen::Index d = 0; d < fpfh_histogram_.size (); ++d) - output.points[idx].histogram[d] = fpfh_histogram_[d]; + std::copy_n(fpfh_histogram_.data (), fpfh_histogram_.size (), output.points[idx].histogram); } } else @@ -290,8 +294,7 @@ pcl::FPFHEstimation::computeFeature (PointCloudOut weightPointSPFHSignature (hist_f1_, hist_f2_, hist_f3_, nn_indices, nn_dists, fpfh_histogram_); // ...and copy it into the output cloud - for (Eigen::Index d = 0; d < fpfh_histogram_.size (); ++d) - output.points[idx].histogram[d] = fpfh_histogram_[d]; + std::copy_n(fpfh_histogram_.data (), fpfh_histogram_.size (), output.points[idx].histogram); } } } diff --git a/features/include/pcl/features/impl/fpfh_omp.hpp b/features/include/pcl/features/impl/fpfh_omp.hpp index e91f49a2..c0015a79 100644 --- a/features/include/pcl/features/impl/fpfh_omp.hpp +++ b/features/include/pcl/features/impl/fpfh_omp.hpp @@ -41,6 +41,8 @@ #ifndef PCL_FEATURES_IMPL_FPFH_OMP_H_ #define PCL_FEATURES_IMPL_FPFH_OMP_H_ +#include + #include ////////////////////////////////////////////////////////////////////////////////////////////// @@ -83,14 +85,14 @@ pcl::FPFHEstimationOMP::computeFeature (PointCloud spfh_indices_set.insert (nn_indices.begin (), nn_indices.end ()); } spfh_indices_vec.resize (spfh_indices_set.size ()); - std::copy (spfh_indices_set.begin (), spfh_indices_set.end (), spfh_indices_vec.begin ()); + std::copy (spfh_indices_set.cbegin (), spfh_indices_set.cend (), spfh_indices_vec.begin ()); } else { // Special case: When a feature must be computed at every point, there is no need for a neighborhood search spfh_indices_vec.resize (indices_->size ()); - for (int idx = 0; idx < static_cast (indices_->size ()); ++idx) - spfh_indices_vec[idx] = idx; + std::iota(spfh_indices_vec.begin (), spfh_indices_vec.end (), + static_cast(0)); } // Initialize the arrays that will store the SPFH signatures diff --git a/features/include/pcl/features/impl/gasd.hpp b/features/include/pcl/features/impl/gasd.hpp index 1be9cf30..02223d6b 100644 --- a/features/include/pcl/features/impl/gasd.hpp +++ b/features/include/pcl/features/impl/gasd.hpp @@ -281,10 +281,10 @@ pcl::GASDEstimation::computeFeature (PointCloudOut &output) hist_incr_ = 100.0f / static_cast (shape_samples_.size () - 1); // for each sample - for (std::size_t i = 0; i < shape_samples_.size (); ++i) + for (const auto& sample: shape_samples_) { // compute shape histogram array coord based on distance between sample and centroid - const Eigen::Vector4f p (shape_samples_[i].x, shape_samples_[i].y, shape_samples_[i].z, 0.0f); + const Eigen::Vector4f p (sample.x, sample.y, sample.z, 0.0f); const float d = p.norm (); const float shape_grid_step = distance_normalization_factor / shape_half_grid_size_; @@ -347,32 +347,32 @@ pcl::GASDColorEstimation::computeFeature (PointCloudOut &ou Eigen::VectorXf::Zero (color_hists_size_ + 2)); // for each sample - for (std::size_t i = 0; i < shape_samples_.size (); ++i) + for (const auto& sample: shape_samples_) { // compute shape histogram array coord based on distance between sample and centroid - const Eigen::Vector4f p (shape_samples_[i].x, shape_samples_[i].y, shape_samples_[i].z, 0.0f); + const Eigen::Vector4f p (sample.x, sample.y, sample.z, 0.0f); // compute hue value float hue = 0.f; - const unsigned char max = std::max (shape_samples_[i].r, std::max (shape_samples_[i].g, shape_samples_[i].b)); - const unsigned char min = std::min (shape_samples_[i].r, std::min (shape_samples_[i].g, shape_samples_[i].b)); + const unsigned char max = std::max (sample.r, std::max (sample.g, sample.b)); + const unsigned char min = std::min (sample.r, std::min (sample.g, sample.b)); const float diff_inv = 1.f / static_cast (max - min); if (std::isfinite (diff_inv)) { - if (max == shape_samples_[i].r) + if (max == sample.r) { - hue = 60.f * (static_cast (shape_samples_[i].g - shape_samples_[i].b) * diff_inv); + hue = 60.f * (static_cast (sample.g - sample.b) * diff_inv); } - else if (max == shape_samples_[i].g) + else if (max == sample.g) { - hue = 60.f * (2.f + static_cast (shape_samples_[i].b - shape_samples_[i].r) * diff_inv); + hue = 60.f * (2.f + static_cast (sample.b - sample.r) * diff_inv); } else { - hue = 60.f * (4.f + static_cast (shape_samples_[i].r - shape_samples_[i].g) * diff_inv); // max == b + hue = 60.f * (4.f + static_cast (sample.r - sample.g) * diff_inv); // max == b } if (hue < 0.f) diff --git a/features/include/pcl/features/impl/gfpfh.hpp b/features/include/pcl/features/impl/gfpfh.hpp index 9a588840..08a99213 100644 --- a/features/include/pcl/features/impl/gfpfh.hpp +++ b/features/include/pcl/features/impl/gfpfh.hpp @@ -130,7 +130,7 @@ pcl::GFPFHEstimation::computeFeature (PointCloudOu output.width = 1; output.height = 1; output.points.resize (1); - std::copy (gfpfh_histogram.begin (), gfpfh_histogram.end (), output.points[0].histogram); + std::copy (gfpfh_histogram.cbegin (), gfpfh_histogram.cend (), output.points[0].histogram); } ////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/features/include/pcl/features/impl/grsd.hpp b/features/include/pcl/features/impl/grsd.hpp index 658d2db6..daf15ec8 100644 --- a/features/include/pcl/features/impl/grsd.hpp +++ b/features/include/pcl/features/impl/grsd.hpp @@ -92,21 +92,21 @@ pcl::GRSDEstimation::computeFeature (PointCloudOut // Save the type of each point int NR_CLASS = 5; // TODO make this nicer std::vector types (radii->points.size ()); - for (std::size_t idx = 0; idx < radii->points.size (); ++idx) - types[idx] = getSimpleType (radii->points[idx].r_min, radii->points[idx].r_max); + std::transform(radii->points.cbegin (), radii->points.cend (), types.begin (), + [](const auto& point) { + // GCC 5.4 can't find unqualified getSimpleType + return GRSDEstimation::getSimpleType(point.r_min, point.r_max); }); // Get the transitions between surface types between neighbors of occupied cells Eigen::MatrixXi transition_matrix = Eigen::MatrixXi::Zero (NR_CLASS + 1, NR_CLASS + 1); for (std::size_t idx = 0; idx < cloud_downsampled->points.size (); ++idx) { - int source_type = types[idx]; + const int source_type = types[idx]; std::vector neighbors = grid.getNeighborCentroidIndices (cloud_downsampled->points[idx], relative_coordinates_all_); for (const int &neighbor : neighbors) { - int neighbor_type; - if (neighbor == -1) // empty - neighbor_type = NR_CLASS; - else + int neighbor_type = NR_CLASS; + if (neighbor != -1) // not empty neighbor_type = types[neighbor]; transition_matrix (source_type, neighbor_type)++; } diff --git a/features/include/pcl/features/impl/integral_image_normal.hpp b/features/include/pcl/features/impl/integral_image_normal.hpp index d78357ec..8dd65609 100644 --- a/features/include/pcl/features/impl/integral_image_normal.hpp +++ b/features/include/pcl/features/impl/integral_image_normal.hpp @@ -1047,7 +1047,7 @@ pcl::IntegralImageNormalEstimation::computeFeaturePart (con continue; } - if (u < border || v > right) + if (u < border || u > right) { output.points[idx].getNormalVector3fMap ().setConstant (bad_point); output.points[idx].curvature = bad_point; @@ -1091,7 +1091,7 @@ pcl::IntegralImageNormalEstimation::computeFeaturePart (con continue; } - if (u < border || v > right) + if (u < border || u > right) { output.points[idx].getNormalVector3fMap ().setConstant (bad_point); output.points[idx].curvature = bad_point; @@ -1114,8 +1114,8 @@ pcl::IntegralImageNormalEstimation::computeFeaturePart (con } else { - output [pt_index].getNormalVector3fMap ().setConstant (bad_point); - output [pt_index].curvature = bad_point; + output [idx].getNormalVector3fMap ().setConstant (bad_point); + output [idx].curvature = bad_point; } } } diff --git a/features/include/pcl/features/impl/moment_invariants.hpp b/features/include/pcl/features/impl/moment_invariants.hpp index c39f7633..14baabc6 100644 --- a/features/include/pcl/features/impl/moment_invariants.hpp +++ b/features/include/pcl/features/impl/moment_invariants.hpp @@ -90,12 +90,12 @@ pcl::MomentInvariantsEstimation::computePointMomentInvarian float mu200 = 0, mu020 = 0, mu002 = 0, mu110 = 0, mu101 = 0, mu011 = 0; // Iterate over the nearest neighbors set - for (std::size_t nn_idx = 0; nn_idx < cloud.points.size (); ++nn_idx ) + for (const auto& point: cloud.points) { // Demean the points - temp_pt_[0] = cloud.points[nn_idx].x - xyz_centroid_[0]; - temp_pt_[1] = cloud.points[nn_idx].y - xyz_centroid_[1]; - temp_pt_[2] = cloud.points[nn_idx].z - xyz_centroid_[2]; + temp_pt_[0] = point.x - xyz_centroid_[0]; + temp_pt_[1] = point.y - xyz_centroid_[1]; + temp_pt_[2] = point.z - xyz_centroid_[2]; mu200 += temp_pt_[0] * temp_pt_[0]; mu020 += temp_pt_[1] * temp_pt_[1]; diff --git a/features/include/pcl/features/impl/multiscale_feature_persistence.hpp b/features/include/pcl/features/impl/multiscale_feature_persistence.hpp index 1484aaf8..5e878715 100644 --- a/features/include/pcl/features/impl/multiscale_feature_persistence.hpp +++ b/features/include/pcl/features/impl/multiscale_feature_persistence.hpp @@ -40,6 +40,7 @@ #ifndef PCL_FEATURES_IMPL_MULTISCALE_FEATURE_PERSISTENCE_H_ #define PCL_FEATURES_IMPL_MULTISCALE_FEATURE_PERSISTENCE_H_ +#include #include ////////////////////////////////////////////////////////////////////////////////////////////// @@ -136,20 +137,24 @@ template void pcl::MultiscaleFeaturePersistence::calculateMeanFeature () { // Reset mean feature - for (int i = 0; i < feature_representation_->getNumberOfDimensions (); ++i) - mean_feature_[i] = 0.0f; + std::fill_n(mean_feature_.begin (), mean_feature_.size (), 0.f); - float normalization_factor = 0.0f; + std::size_t normalization_factor = 0; for (const auto& scale: features_at_scale_vectorized_) { - normalization_factor += static_cast (scale.size ()); + normalization_factor += scale.size (); // not using accumulate for cache efficiency for (const auto &feature : scale) - for (int dim_i = 0; dim_i < feature_representation_->getNumberOfDimensions (); ++dim_i) - mean_feature_[dim_i] += feature[dim_i]; + std::transform(mean_feature_.cbegin (), mean_feature_.cend (), + feature.cbegin (), mean_feature_.begin (), std::plus<>{}); } - for (int dim_i = 0; dim_i < feature_representation_->getNumberOfDimensions (); ++dim_i) - mean_feature_[dim_i] /= normalization_factor; + const float factor = std::min(1, normalization_factor); + std::transform(mean_feature_.cbegin(), + mean_feature_.cend(), + mean_feature_.begin(), + [factor](const auto& mean) { + return mean / factor; + }); } diff --git a/features/include/pcl/features/impl/normal_3d_omp.hpp b/features/include/pcl/features/impl/normal_3d_omp.hpp index cf62ddb7..a849a385 100644 --- a/features/include/pcl/features/impl/normal_3d_omp.hpp +++ b/features/include/pcl/features/impl/normal_3d_omp.hpp @@ -71,7 +71,7 @@ pcl::NormalEstimationOMP::computeFeature (PointCloudOut &ou if (input_->is_dense) { #ifdef _OPENMP -#pragma omp parallel for shared (output) private (nn_indices, nn_dists) num_threads(threads_) +#pragma omp parallel for shared (output) firstprivate (nn_indices, nn_dists) num_threads(threads_) #endif // Iterating over the entire index vector for (std::ptrdiff_t idx = 0; idx < static_cast (indices_->size ()); ++idx) @@ -98,7 +98,7 @@ pcl::NormalEstimationOMP::computeFeature (PointCloudOut &ou else { #ifdef _OPENMP -#pragma omp parallel for shared (output) private (nn_indices, nn_dists) num_threads(threads_) +#pragma omp parallel for shared (output) firstprivate (nn_indices, nn_dists) num_threads(threads_) #endif // Iterating over the entire index vector for (std::ptrdiff_t idx = 0; idx < static_cast (indices_->size ()); ++idx) diff --git a/features/include/pcl/features/impl/organized_edge_detection.hpp b/features/include/pcl/features/impl/organized_edge_detection.hpp index 88d1d7f5..04d645a9 100644 --- a/features/include/pcl/features/impl/organized_edge_detection.hpp +++ b/features/include/pcl/features/impl/organized_edge_detection.hpp @@ -128,11 +128,11 @@ pcl::OrganizedEdgeBase::extractEdges (pcl::PointCloud& if (!found_invalid_neighbor) { // Every neighboring points are valid - std::vector::iterator min_itr = std::min_element (nghr_dist.begin (), nghr_dist.end ()); - std::vector::iterator max_itr = std::max_element (nghr_dist.begin (), nghr_dist.end ()); + std::vector::const_iterator min_itr, max_itr; + std::tie (min_itr, max_itr) = std::minmax_element (nghr_dist.cbegin (), nghr_dist.cend ()); float nghr_dist_min = *min_itr; float nghr_dist_max = *max_itr; - float dist_dominant = std::abs (nghr_dist_min) > std::abs (nghr_dist_max) ? nghr_dist_min : nghr_dist_max; + float dist_dominant = std::max(std::abs (nghr_dist_min),std::abs (nghr_dist_max)); if (std::abs (dist_dominant) > th_depth_discon_*std::abs (curr_depth)) { // Found a depth discontinuity diff --git a/features/include/pcl/features/impl/principal_curvatures.hpp b/features/include/pcl/features/impl/principal_curvatures.hpp index 2126ad11..bfa2499d 100644 --- a/features/include/pcl/features/impl/principal_curvatures.hpp +++ b/features/include/pcl/features/impl/principal_curvatures.hpp @@ -73,15 +73,14 @@ pcl::PrincipalCurvaturesEstimation::computePointPr // Initialize to 0 covariance_matrix_.setZero (); - double demean_xy, demean_xz, demean_yz; // For each point in the cloud for (std::size_t idx = 0; idx < indices.size (); ++idx) { demean_ = projected_normals_[idx] - xyz_centroid_; - demean_xy = demean_[0] * demean_[1]; - demean_xz = demean_[0] * demean_[2]; - demean_yz = demean_[1] * demean_[2]; + double demean_xy = demean_[0] * demean_[1]; + double demean_xz = demean_[0] * demean_[2]; + double demean_yz = demean_[1] * demean_[2]; covariance_matrix_(0, 0) += demean_[0] * demean_[0]; covariance_matrix_(0, 1) += static_cast (demean_xy); diff --git a/features/include/pcl/features/impl/rift.hpp b/features/include/pcl/features/impl/rift.hpp index e8c0fd44..d5ed46b2 100644 --- a/features/include/pcl/features/impl/rift.hpp +++ b/features/include/pcl/features/impl/rift.hpp @@ -170,11 +170,8 @@ pcl::RIFTEstimation::computeFeature (PointCloudO // Compute the RIFT descriptor computeRIFT (*surface_, *gradient_, (*indices_)[idx], static_cast (search_radius_), nn_indices, nn_dist_sqr, rift_descriptor); - // Copy into the resultant cloud - std::size_t bin = 0; - for (Eigen::Index g_bin = 0; g_bin < rift_descriptor.cols (); ++g_bin) - for (Eigen::Index d_bin = 0; d_bin < rift_descriptor.rows (); ++d_bin) - output.points[idx].histogram[bin++] = rift_descriptor (d_bin, g_bin); + // Default layout is column major, copy elementwise + std::copy_n (rift_descriptor.data (), rift_descriptor.size (), output.points[idx].histogram); } } diff --git a/features/include/pcl/features/impl/rops_estimation.hpp b/features/include/pcl/features/impl/rops_estimation.hpp index 66a9f2f0..53213c32 100644 --- a/features/include/pcl/features/impl/rops_estimation.hpp +++ b/features/include/pcl/features/impl/rops_estimation.hpp @@ -193,17 +193,19 @@ pcl::ROPSEstimation ::computeFeature (PointCloudOut &output } while (theta < 90.0f); } - float norm = 0.0f; - for (std::size_t i_dim = 0; i_dim < feature_size; i_dim++) - norm += std::abs (feature[i_dim]); + const float norm = std::accumulate( + feature.cbegin(), feature.cend(), 0.f, [](const auto& sum, const auto& val) { + return sum + std::abs(val); + }); + float invert_norm; if (norm < std::numeric_limits ::epsilon ()) - norm = 1.0f; + invert_norm = 1.0f; else - norm = 1.0f / norm; + invert_norm = 1.0f / norm; output.points.emplace_back (); for (std::size_t i_dim = 0; i_dim < feature_size; i_dim++) - output.points.back ().histogram[i_dim] = feature[i_dim] * norm; + output.points.back().histogram[i_dim] = feature[i_dim] * invert_norm; } } @@ -447,13 +449,11 @@ pcl::ROPSEstimation ::rotateCloud (const PointInT& axis, co rotated_point.z = point (2); rotated_cloud.points.emplace_back (rotated_point); - if (min (0) > rotated_point.x) min (0) = rotated_point.x; - if (min (1) > rotated_point.y) min (1) = rotated_point.y; - if (min (2) > rotated_point.z) min (2) = rotated_point.z; - - if (max (0) < rotated_point.x) max (0) = rotated_point.x; - if (max (1) < rotated_point.y) max (1) = rotated_point.y; - if (max (2) < rotated_point.z) max (2) = rotated_point.z; + for (int i = 0; i < 3; ++i) + { + min(i) = std::min(min(i), point(i)); + max(i) = std::max(max(i), point(i)); + } } } @@ -489,7 +489,7 @@ pcl::ROPSEstimation ::getDistributionMatrix (const unsigned matrix (row, col) += 1.0f; } - matrix /= static_cast (std::min (1, cloud.points.size ())); + matrix /= std::max (1, cloud.points.size ()); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/features/include/pcl/features/impl/rsd.hpp b/features/include/pcl/features/impl/rsd.hpp index a70d8a32..f71470ac 100644 --- a/features/include/pcl/features/impl/rsd.hpp +++ b/features/include/pcl/features/impl/rsd.hpp @@ -103,8 +103,8 @@ pcl::computeRSD (const pcl::PointCloud &surface, const pcl::PointCloud } // update min-max values for distance bins - if (min_max_angle_by_dist[bin_d][0] > angle) min_max_angle_by_dist[bin_d][0] = angle; - if (min_max_angle_by_dist[bin_d][1] < angle) min_max_angle_by_dist[bin_d][1] = angle; + min_max_angle_by_dist[bin_d][0] = std::min(angle, min_max_angle_by_dist[bin_d][0]); + min_max_angle_by_dist[bin_d][1] = std::max(angle, min_max_angle_by_dist[bin_d][1]); } // Estimate radius from min and max lines diff --git a/features/include/pcl/features/impl/usc.hpp b/features/include/pcl/features/impl/usc.hpp index 2675d4d3..da6487ac 100644 --- a/features/include/pcl/features/impl/usc.hpp +++ b/features/include/pcl/features/impl/usc.hpp @@ -95,15 +95,15 @@ pcl::UniqueShapeContext::initCompute () for (std::size_t j = 0; j < radius_bins_ + 1; j++) radii_interval_[j] = static_cast (std::exp (std::log (min_radius_) + ((static_cast (j) / static_cast (radius_bins_)) * std::log (search_radius_/min_radius_)))); - // Fill theta didvisions of elevation - theta_divisions_.resize (elevation_bins_+1); - for (std::size_t k = 0; k < elevation_bins_+1; k++) - theta_divisions_[k] = static_cast (k) * elevation_interval; + // Fill theta divisions of elevation + theta_divisions_.resize (elevation_bins_ + 1, elevation_interval); + theta_divisions_[0] = 0; + std::partial_sum(theta_divisions_.begin (), theta_divisions_.end (), theta_divisions_.begin ()); - // Fill phi didvisions of elevation - phi_divisions_.resize (azimuth_bins_+1); - for (std::size_t l = 0; l < azimuth_bins_+1; l++) - phi_divisions_[l] = static_cast (l) * azimuth_interval; + // Fill phi divisions of elevation + phi_divisions_.resize (azimuth_bins_ + 1, azimuth_interval); + phi_divisions_[0] = 0; + std::partial_sum(phi_divisions_.begin (), phi_divisions_.end (), phi_divisions_.begin ()); // LookUp Table that contains the volume of all the bins // "phi" term of the volume integral @@ -188,38 +188,15 @@ pcl::UniqueShapeContext::computePointDescriptor ( float theta = normal.dot (no); theta = pcl::rad2deg (std::acos (std::min (1.0f, std::max (-1.0f, theta)))); - /// Bin (j, k, l) - std::size_t j = 0; - std::size_t k = 0; - std::size_t l = 0; - /// Compute the Bin(j, k, l) coordinates of current neighbour - for (std::size_t rad = 1; rad < radius_bins_ + 1; rad++) - { - if (r <= radii_interval_[rad]) - { - j = rad - 1; - break; - } - } + const auto rad_min = std::lower_bound(std::next (radii_interval_.cbegin ()), radii_interval_.cend (), r); + const auto theta_min = std::lower_bound(std::next (theta_divisions_.cbegin ()), theta_divisions_.cend (), theta); + const auto phi_min = std::lower_bound(std::next (phi_divisions_.cbegin ()), phi_divisions_.cend (), phi); - for (std::size_t ang = 1; ang < elevation_bins_ + 1; ang++) - { - if (theta <= theta_divisions_[ang]) - { - k = ang - 1; - break; - } - } - - for (std::size_t ang = 1; ang < azimuth_bins_ + 1; ang++) - { - if (phi <= phi_divisions_[ang]) - { - l = ang - 1; - break; - } - } + /// Bin (j, k, l) + const auto j = std::distance(radii_interval_.cbegin (), std::prev(rad_min)); + const auto k = std::distance(theta_divisions_.cbegin (), std::prev(theta_min)); + const auto l = std::distance(phi_divisions_.cbegin (), std::prev(phi_min)); /// Local point density = number of points in a sphere of radius "point_density_radius_" around the current neighbour std::vector neighbour_indices; @@ -261,10 +238,9 @@ pcl::UniqueShapeContext::computeFeature (PointClo !std::isfinite (current_frame.y_axis[0]) || !std::isfinite (current_frame.z_axis[0]) ) { - for (std::size_t i = 0; i < descriptor_length_; ++i) - output[point_index].descriptor[i] = std::numeric_limits::quiet_NaN (); - - memset (output[point_index].rf, 0, sizeof (output[point_index].rf[0]) * 9); + std::fill (output.points[point_index].descriptor, output.points[point_index].descriptor + descriptor_length_, + std::numeric_limits::quiet_NaN ()); + std::fill (output.points[point_index].rf, output.points[point_index].rf + 9, 0); output.is_dense = false; continue; } @@ -278,8 +254,7 @@ pcl::UniqueShapeContext::computeFeature (PointClo std::vector descriptor (descriptor_length_); computePointDescriptor (point_index, descriptor); - for (std::size_t j = 0; j < descriptor_length_; ++j) - output [point_index].descriptor[j] = descriptor[j]; + std::copy (descriptor.begin (), descriptor.end (), output.points[point_index].descriptor); } } diff --git a/features/include/pcl/features/impl/vfh.hpp b/features/include/pcl/features/impl/vfh.hpp index 9f8e93ed..f47180d2 100644 --- a/features/include/pcl/features/impl/vfh.hpp +++ b/features/include/pcl/features/impl/vfh.hpp @@ -97,10 +97,10 @@ pcl::VFHEstimation::computePointSPFHSignature (con { Eigen::Vector4f pfh_tuple; // Reset the whole thing - hist_f1_.setZero (nr_bins_f1_); - hist_f2_.setZero (nr_bins_f2_); - hist_f3_.setZero (nr_bins_f3_); - hist_f4_.setZero (nr_bins_f4_); + for (int i = 0; i < 4; ++i) + { + hist_f_[i].setZero (nr_bins_f_[i]); + } // Get the bounding box of the current cluster //Eigen::Vector4f min_pt, max_pt; @@ -139,38 +139,24 @@ pcl::VFHEstimation::computePointSPFHSignature (con continue; // Normalize the f1, f2, f3, f4 features and push them in the histogram - int h_index = static_cast (std::floor (nr_bins_f1_ * ((pfh_tuple[0] + M_PI) * d_pi_))); - if (h_index < 0) - h_index = 0; - if (h_index >= nr_bins_f1_) - h_index = nr_bins_f1_ - 1; - hist_f1_ (h_index) += hist_incr; - - h_index = static_cast (std::floor (nr_bins_f2_ * ((pfh_tuple[1] + 1.0) * 0.5))); - if (h_index < 0) - h_index = 0; - if (h_index >= nr_bins_f2_) - h_index = nr_bins_f2_ - 1; - hist_f2_ (h_index) += hist_incr; - - h_index = static_cast (std::floor (nr_bins_f3_ * ((pfh_tuple[2] + 1.0) * 0.5))); - if (h_index < 0) - h_index = 0; - if (h_index >= nr_bins_f3_) - h_index = nr_bins_f3_ - 1; - hist_f3_ (h_index) += hist_incr; - - if (normalize_distances_) - h_index = static_cast (std::floor (nr_bins_f4_ * (pfh_tuple[3] / distance_normalization_factor))); - else - h_index = static_cast (pcl_round (pfh_tuple[3] * 100)); - - if (h_index < 0) - h_index = 0; - if (h_index >= nr_bins_f4_) - h_index = nr_bins_f4_ - 1; + for (int i = 0; i < 3; ++i) + { + const int raw_index = static_cast (std::floor (nr_bins_f_[i] * ((pfh_tuple[i] + M_PI) * d_pi_))); + const int h_index = std::max(std::min(raw_index, nr_bins_f_[i] - 1), 0); + hist_f_[i] (h_index) += hist_incr; + } - hist_f4_ (h_index) += hist_incr_size_component; + if (hist_incr_size_component) + { + int h_index; + if (normalize_distances_) + h_index = static_cast (std::floor (nr_bins_f_[3] * (pfh_tuple[3] / distance_normalization_factor))); + else + h_index = static_cast (pcl_round (pfh_tuple[3] * 100)); + + h_index = std::max (std::min (h_index, nr_bins_f_[3] - 1), 0); + hist_f_[3] (h_index) += hist_incr_size_component; + } } } ////////////////////////////////////////////////////////////////////////////////////////////// @@ -187,13 +173,13 @@ pcl::VFHEstimation::computeFeature (PointCloudOut // ---[ Step 1b : compute the centroid in normal space Eigen::Vector4f normal_centroid = Eigen::Vector4f::Zero (); - std::size_t cp = 0; // If the data is dense, we don't need to check for NaN if (use_given_normal_) normal_centroid = normal_to_use_; else { + std::size_t cp = 0; if (normals_->is_dense) { for (const auto& index: *indices_) @@ -240,11 +226,9 @@ pcl::VFHEstimation::computeFeature (PointCloudOut normals_->points[index].normal[2], 0); // Normalize double alpha = (normal.dot (d_vp_p) + 1.0) * 0.5; - int fi = static_cast (std::floor (alpha * static_cast (hist_vp_.size ()))); - if (fi < 0) - fi = 0; - if (fi > (static_cast (hist_vp_.size ()) - 1)) - fi = static_cast (hist_vp_.size ()) - 1; + std::size_t fi = static_cast (std::floor (alpha * hist_vp_.size ())); + fi = std::max (0u, fi); + fi = std::min (hist_vp_.size () - 1, fi); // Bin into the histogram hist_vp_ [fi] += hist_incr; } @@ -257,10 +241,10 @@ pcl::VFHEstimation::computeFeature (PointCloudOut // Estimate the FPFH at nn_indices[0] using the entire cloud and copy the resultant signature auto outPtr = std::begin (output.points[0].histogram); - outPtr = std::copy_n (hist_f1_.data (), hist_f1_.size (), outPtr); - outPtr = std::copy_n (hist_f2_.data (), hist_f2_.size (), outPtr); - outPtr = std::copy_n (hist_f3_.data (), hist_f3_.size (), outPtr); - outPtr = std::copy_n (hist_f4_.data (), hist_f4_.size (), outPtr); + for (int i = 0; i < 4; ++i) + { + outPtr = std::copy_n (hist_f_[i].data (), hist_f_[i].size (), outPtr); + } outPtr = std::copy_n (hist_vp_.data (), hist_vp_.size (), outPtr); } diff --git a/features/include/pcl/features/rsd.h b/features/include/pcl/features/rsd.h index e0c9d2a4..edda7a5c 100644 --- a/features/include/pcl/features/rsd.h +++ b/features/include/pcl/features/rsd.h @@ -89,7 +89,7 @@ namespace pcl int nr_subdiv, double plane_radius, PointOutT &radii, bool compute_histogram = false); template - [[deprecated("use computeRSD() overload that takes input point clouds by const reference")]] + PCL_DEPRECATED("use computeRSD() overload that takes input point clouds by const reference") Eigen::MatrixXf computeRSD (typename pcl::PointCloud::ConstPtr &surface, typename pcl::PointCloud::ConstPtr &normals, const std::vector &indices, double max_dist, @@ -115,7 +115,7 @@ namespace pcl int nr_subdiv, double plane_radius, PointOutT &radii, bool compute_histogram = false); template - [[deprecated("use computeRSD() overload that takes input point cloud by const reference")]] + PCL_DEPRECATED("use computeRSD() overload that takes input point cloud by const reference") Eigen::MatrixXf computeRSD (typename pcl::PointCloud::ConstPtr &normals, const std::vector &indices, const std::vector &sqr_dists, double max_dist, diff --git a/features/include/pcl/features/vfh.h b/features/include/pcl/features/vfh.h index 3e4c93bf..7a7b7f30 100644 --- a/features/include/pcl/features/vfh.h +++ b/features/include/pcl/features/vfh.h @@ -40,6 +40,8 @@ #pragma once +#include + #include #include @@ -86,16 +88,16 @@ namespace pcl /** \brief Empty constructor. */ VFHEstimation () : - nr_bins_f1_ (45), nr_bins_f2_ (45), nr_bins_f3_ (45), nr_bins_f4_ (45), nr_bins_vp_ (128), + nr_bins_f_ ({45, 45, 45, 45}), nr_bins_vp_ (128), vpx_ (0), vpy_ (0), vpz_ (0), use_given_normal_ (false), use_given_centroid_ (false), normalize_bins_ (true), normalize_distances_ (false), size_component_ (false), d_pi_ (1.0f / (2.0f * static_cast (M_PI))) { - hist_f1_.setZero (nr_bins_f1_); - hist_f2_.setZero (nr_bins_f2_); - hist_f3_.setZero (nr_bins_f3_); - hist_f4_.setZero (nr_bins_f4_); + for (int i = 0; i < 4; ++i) + { + hist_f_[i].setZero (nr_bins_f_[i]); + } search_radius_ = 0; k_ = 0; feature_name_ = "VFHEstimation"; @@ -212,7 +214,8 @@ namespace pcl private: /** \brief The number of subdivisions for each feature interval. */ - int nr_bins_f1_, nr_bins_f2_, nr_bins_f3_, nr_bins_f4_, nr_bins_vp_; + std::array nr_bins_f_; + int nr_bins_vp_; /** \brief Values describing the viewpoint ("pinhole" camera model assumed). For per point viewpoints, inherit * from VFHEstimation and provide your own computeFeature (). By default, the viewpoint is set to 0,0,0. @@ -233,13 +236,7 @@ namespace pcl initCompute () override; /** \brief Placeholder for the f1 histogram. */ - Eigen::VectorXf hist_f1_; - /** \brief Placeholder for the f2 histogram. */ - Eigen::VectorXf hist_f2_; - /** \brief Placeholder for the f3 histogram. */ - Eigen::VectorXf hist_f3_; - /** \brief Placeholder for the f4 histogram. */ - Eigen::VectorXf hist_f4_; + std::array hist_f_; /** \brief Placeholder for the vp histogram. */ Eigen::VectorXf hist_vp_; diff --git a/features/src/from_meshes.cpp b/features/src/from_meshes.cpp new file mode 100644 index 00000000..0e8550aa --- /dev/null +++ b/features/src/from_meshes.cpp @@ -0,0 +1,50 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * 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 + +#ifndef PCL_NO_PRECOMPILE +#include +#include +// Instantiations of specific point types +#ifdef PCL_ONLY_CORE_POINT_TYPES + PCL_INSTANTIATE_PRODUCT(computeApproximateCovariances, ((pcl::PointXYZ)(pcl::PointXYZI)(pcl::PointXYZRGBA))((pcl::Normal))) +#else + PCL_INSTANTIATE_PRODUCT(computeApproximateCovariances, (PCL_XYZ_POINT_TYPES)(PCL_NORMAL_POINT_TYPES)) +#endif +#endif // PCL_NO_PRECOMPILE diff --git a/features/src/narf.cpp b/features/src/narf.cpp index cc1df787..50258ddf 100644 --- a/features/src/narf.cpp +++ b/features/src/narf.cpp @@ -260,16 +260,11 @@ Narf::extractFromRangeImageWithBestRotation (const RangeImage& range_image, cons getRotations(rotations, strengths); if (rotations.empty()) return false; - float best_rotation=rotations[0], best_strength=strengths[0]; - for (std::size_t i = 1; i < rotations.size(); ++i) - { - if (strengths[i] > best_strength) - { - best_rotation = rotations[i]; - best_strength = strengths[i]; - } - } - + + const auto max_it = std::max_element(strengths.cbegin (), strengths.cend ()); + const auto max_idx = std::distance(strengths.cbegin (), max_it); + const auto best_rotation = rotations[max_idx]; + transformation_ = Eigen::AngleAxisf(-best_rotation, Eigen::Vector3f(0.0f, 0.0f, 1.0f))*transformation_; surface_patch_rotation_ = best_rotation; return extractDescriptor(descriptor_size_); diff --git a/features/src/range_image_border_extractor.cpp b/features/src/range_image_border_extractor.cpp index 287a0146..6a270c21 100644 --- a/features/src/range_image_border_extractor.cpp +++ b/features/src/range_image_border_extractor.cpp @@ -149,7 +149,7 @@ RangeImageBorderExtractor::extractLocalSurfaceStructure () void RangeImageBorderExtractor::extractBorderScoreImages () { - if (border_scores_left_.empty()) + if (!border_scores_left_.empty()) return; extractLocalSurfaceStructure(); diff --git a/filters/include/pcl/filters/box_clipper3D.h b/filters/include/pcl/filters/box_clipper3D.h index 76ceb668..0e68a411 100644 --- a/filters/include/pcl/filters/box_clipper3D.h +++ b/filters/include/pcl/filters/box_clipper3D.h @@ -89,7 +89,7 @@ namespace pcl /** * \brief virtual destructor */ - ~BoxClipper3D () throw (); + ~BoxClipper3D () noexcept; bool clipPoint3D (const PointT& point) const override; diff --git a/filters/include/pcl/filters/clipper3D.h b/filters/include/pcl/filters/clipper3D.h index b24666bb..0196ca08 100644 --- a/filters/include/pcl/filters/clipper3D.h +++ b/filters/include/pcl/filters/clipper3D.h @@ -59,7 +59,7 @@ namespace pcl /** * \brief virtual destructor. Never throws an exception. */ - virtual ~Clipper3D () throw () {} + virtual ~Clipper3D () noexcept {} /** * \brief interface to clip a single point diff --git a/filters/include/pcl/filters/impl/box_clipper3D.hpp b/filters/include/pcl/filters/impl/box_clipper3D.hpp index b7784427..2cb641c9 100644 --- a/filters/include/pcl/filters/impl/box_clipper3D.hpp +++ b/filters/include/pcl/filters/impl/box_clipper3D.hpp @@ -53,7 +53,7 @@ pcl::BoxClipper3D::BoxClipper3D (const Eigen::Vector3f& rodrigues, const //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template -pcl::BoxClipper3D::~BoxClipper3D () throw () +pcl::BoxClipper3D::~BoxClipper3D () noexcept { } diff --git a/filters/include/pcl/filters/impl/conditional_removal.hpp b/filters/include/pcl/filters/impl/conditional_removal.hpp index 98104056..bce677a9 100644 --- a/filters/include/pcl/filters/impl/conditional_removal.hpp +++ b/filters/include/pcl/filters/impl/conditional_removal.hpp @@ -689,13 +689,14 @@ pcl::ConditionalRemoval::applyFilter (PointCloud &output) output.points.resize (input_->points.size ()); removed_indices_->resize (input_->points.size ()); - int nr_p = 0; int nr_removed_p = 0; if (!keep_organized_) { + int nr_p = 0; for (std::size_t index: (*Filter::indices_)) { + const PointT& point = input_->points[index]; // Check if the point is invalid if (!std::isfinite (point.x) diff --git a/filters/include/pcl/filters/impl/convolution_3d.hpp b/filters/include/pcl/filters/impl/convolution_3d.hpp index 551cb389..2421ddad 100644 --- a/filters/include/pcl/filters/impl/convolution_3d.hpp +++ b/filters/include/pcl/filters/impl/convolution_3d.hpp @@ -44,6 +44,11 @@ #include #include +#include +#include +#include +#include + /////////////////////////////////////////////////////////////////////////////////////////////////// namespace pcl { diff --git a/filters/include/pcl/filters/impl/filter.hpp b/filters/include/pcl/filters/impl/filter.hpp index 1e6f0291..03d17c1d 100644 --- a/filters/include/pcl/filters/impl/filter.hpp +++ b/filters/include/pcl/filters/impl/filter.hpp @@ -112,12 +112,17 @@ pcl::removeNaNNormalsFromPointCloud (const pcl::PointCloud &cloud_in, index.resize (cloud_in.points.size ()); std::size_t j = 0; + // Assume cloud is dense + cloud_out.is_dense = true; + for (std::size_t i = 0; i < cloud_in.points.size (); ++i) { if (!std::isfinite (cloud_in.points[i].normal_x) || !std::isfinite (cloud_in.points[i].normal_y) || !std::isfinite (cloud_in.points[i].normal_z)) continue; + if (cloud_out.is_dense && !pcl::isFinite(cloud_in.points[i])) + cloud_out.is_dense = false; cloud_out.points[j] = cloud_in.points[i]; index[j] = static_cast(i); j++; diff --git a/filters/include/pcl/filters/impl/normal_space.hpp b/filters/include/pcl/filters/impl/normal_space.hpp index 15eecd3a..f4b8f485 100644 --- a/filters/include/pcl/filters/impl/normal_space.hpp +++ b/filters/include/pcl/filters/impl/normal_space.hpp @@ -211,7 +211,7 @@ pcl::NormalSpaceSampling::applyFilter (std::vector &indice random_access[i].resize (normals_hg[i].size ()); std::size_t j = 0; - for (std::list::iterator itr = normals_hg[i].begin (); itr != normals_hg[i].end (); itr++, j++) + for (std::list::iterator itr = normals_hg[i].begin (); itr != normals_hg[i].end (); ++itr, ++j) random_access[i][j] = itr; } std::vector start_index (normals_hg.size ()); diff --git a/filters/include/pcl/filters/impl/plane_clipper3D.hpp b/filters/include/pcl/filters/impl/plane_clipper3D.hpp index f1cb9a30..0adb6814 100644 --- a/filters/include/pcl/filters/impl/plane_clipper3D.hpp +++ b/filters/include/pcl/filters/impl/plane_clipper3D.hpp @@ -44,7 +44,7 @@ pcl::PlaneClipper3D::PlaneClipper3D (const Eigen::Vector4f& plane_params } template -pcl::PlaneClipper3D::~PlaneClipper3D () throw () +pcl::PlaneClipper3D::~PlaneClipper3D () noexcept { } diff --git a/filters/include/pcl/filters/impl/voxel_grid_occlusion_estimation.hpp b/filters/include/pcl/filters/impl/voxel_grid_occlusion_estimation.hpp index eba99311..689f592c 100644 --- a/filters/include/pcl/filters/impl/voxel_grid_occlusion_estimation.hpp +++ b/filters/include/pcl/filters/impl/voxel_grid_occlusion_estimation.hpp @@ -296,9 +296,6 @@ pcl::VoxelGridOcclusionEstimation::rayTraversal (const Eigen::Vector3i& float t_delta_y = leaf_size_[1] / static_cast (std::abs (direction[1])); float t_delta_z = leaf_size_[2] / static_cast (std::abs (direction[2])); - // index of the point in the point cloud - int index; - while ( (ijk[0] < max_b_[0]+1) && (ijk[0] >= min_b_[0]) && (ijk[1] < max_b_[1]+1) && (ijk[1] >= min_b_[1]) && (ijk[2] < max_b_[2]+1) && (ijk[2] >= min_b_[2]) ) @@ -307,8 +304,9 @@ pcl::VoxelGridOcclusionEstimation::rayTraversal (const Eigen::Vector3i& if (ijk[0] == target_voxel[0] && ijk[1] == target_voxel[1] && ijk[2] == target_voxel[2]) return 0; + // index of the point in the point cloud + int index = this->getCentroidIndexAt (ijk); // check if voxel is occupied, if yes return 1 for occluded - index = this->getCentroidIndexAt (ijk); if (index != -1) return 1; @@ -397,7 +395,6 @@ pcl::VoxelGridOcclusionEstimation::rayTraversal (std::vector (std::abs (direction[2])); // the index of the cloud (-1 if empty) - int index = -1; int result = 0; while ( (ijk[0] < max_b_[0]+1) && (ijk[0] >= min_b_[0]) && @@ -412,7 +409,7 @@ pcl::VoxelGridOcclusionEstimation::rayTraversal (std::vectorgetCentroidIndexAt (ijk); + int index = this->getCentroidIndexAt (ijk); if (index != -1) result = 1; diff --git a/filters/include/pcl/filters/passthrough.h b/filters/include/pcl/filters/passthrough.h index 69e63fd3..3f5e2b6d 100644 --- a/filters/include/pcl/filters/passthrough.h +++ b/filters/include/pcl/filters/passthrough.h @@ -39,6 +39,7 @@ #pragma once +#include #include namespace pcl @@ -286,7 +287,7 @@ namespace pcl * Default: false. * \param[in] limit_negative return data inside the interval (false) or outside (true) */ - [[deprecated("use inherited FilterIndices::setNegative() instead")]] + PCL_DEPRECATED("use inherited FilterIndices::setNegative() instead") inline void setFilterLimitsNegative (const bool limit_negative) { @@ -296,7 +297,7 @@ namespace pcl /** \brief Get whether the data outside the interval (min/max) is to be returned (true) or inside (false). * \param[out] limit_negative true if data \b outside the interval [min; max] is to be returned, false otherwise */ - [[deprecated("use inherited FilterIndices::getNegative() instead")]] + PCL_DEPRECATED("use inherited FilterIndices::getNegative() instead") inline void getFilterLimitsNegative (bool &limit_negative) const { @@ -306,7 +307,7 @@ namespace pcl /** \brief Get whether the data outside the interval (min/max) is to be returned (true) or inside (false). * \return true if data \b outside the interval [min; max] is to be returned, false otherwise */ - [[deprecated("use inherited FilterIndices::getNegative() instead")]] + PCL_DEPRECATED("use inherited FilterIndices::getNegative() instead") inline bool getFilterLimitsNegative () const { diff --git a/filters/include/pcl/filters/plane_clipper3D.h b/filters/include/pcl/filters/plane_clipper3D.h index 50e69aa2..eb73227b 100644 --- a/filters/include/pcl/filters/plane_clipper3D.h +++ b/filters/include/pcl/filters/plane_clipper3D.h @@ -61,7 +61,7 @@ namespace pcl */ PlaneClipper3D (const Eigen::Vector4f& plane_params); - virtual ~PlaneClipper3D () throw (); + virtual ~PlaneClipper3D () noexcept; /** * \brief Set new plane parameters diff --git a/filters/include/pcl/filters/uniform_sampling.h b/filters/include/pcl/filters/uniform_sampling.h index 7e52e4b4..a32f6692 100644 --- a/filters/include/pcl/filters/uniform_sampling.h +++ b/filters/include/pcl/filters/uniform_sampling.h @@ -55,7 +55,7 @@ namespace pcl * represents the underlying surface more accurately. * * \author Radu Bogdan Rusu - * \ingroup keypoints + * \ingroup filters */ template class UniformSampling: public Filter diff --git a/gpu/containers/src/error.cpp b/gpu/containers/src/error.cpp index f08b6b43..a944e591 100644 --- a/gpu/containers/src/error.cpp +++ b/gpu/containers/src/error.cpp @@ -42,5 +42,5 @@ void pcl::gpu::error(const char *error_string, const char *file, const int line, const char *func) { std::cout << "Error: " << error_string << "\t" << file << ":" << line << std::endl; - exit(0); + exit(EXIT_FAILURE); } diff --git a/gpu/features/test/test_normals.cpp b/gpu/features/test/test_normals.cpp index 06ecf3aa..7580e0d6 100644 --- a/gpu/features/test/test_normals.cpp +++ b/gpu/features/test/test_normals.cpp @@ -371,6 +371,63 @@ TEST(PCL_FeaturesGPU, normals_highlevel_4) } } +// Test from issue: +// - https://github.com/PointCloudLibrary/pcl/issues/2371#issuecomment-577727912 +TEST(PCL_FeaturesGPU, issue_2371) +{ + // This number is magic, do not set to lower value. + // It may affect error reproducibility. + const std::size_t N = 1000; + std::vector cloud_cpu(N, {0.0, 0.0, 0.0}); + + pcl::gpu::NormalEstimation::PointCloud cloud_gpu; + cloud_gpu.upload(cloud_cpu); + + pcl::gpu::NormalEstimation ne_gpu; + ne_gpu.setInputCloud(cloud_gpu); + + const float radius_search = 2.0F; + const int max_results = 500; + ne_gpu.setRadiusSearch(radius_search, max_results); + + pcl::gpu::NormalEstimation::Normals normals_gpu; + ne_gpu.compute(normals_gpu); +} + +// See: +// - https://github.com/PointCloudLibrary/pcl/pull/3627#discussion_r375826172 +TEST(PCL_FeaturesGPU, normals_nan_gpu) +{ + const std::size_t N = 5; + + PointCloud cloud; + cloud.points.assign(N, {0.0, 0.0, 0.0}); + + const float radius_search = 2.0F; + const int max_results = 500; + + pcl::gpu::NormalEstimation::PointCloud cloud_device; + cloud_device.upload(cloud.points); + + pcl::gpu::NormalEstimation ne_device; + ne_device.setInputCloud(cloud_device); + ne_device.setRadiusSearch(radius_search, max_results); + + pcl::gpu::NormalEstimation::Normals normals_device; + ne_device.compute(normals_device); + + std::vector downloaded; + normals_device.download(downloaded); + + ASSERT_EQ(downloaded.size(), N); + + for(const auto& n : downloaded) + { + ASSERT_TRUE(std::isnan(n.x)); + ASSERT_TRUE(std::isnan(n.y)); + ASSERT_TRUE(std::isnan(n.z)); + } +} int main (int argc, char** argv) { diff --git a/gpu/kinfu/include/pcl/gpu/kinfu/raycaster.h b/gpu/kinfu/include/pcl/gpu/kinfu/raycaster.h index 4ce1f647..2bd37d0e 100644 --- a/gpu/kinfu/include/pcl/gpu/kinfu/raycaster.h +++ b/gpu/kinfu/include/pcl/gpu/kinfu/raycaster.h @@ -41,7 +41,7 @@ #include #include #include -#include +#include #include namespace pcl diff --git a/gpu/kinfu/tools/camera_pose.h b/gpu/kinfu/tools/camera_pose.h index 6b6bfe30..e54ba723 100644 --- a/gpu/kinfu/tools/camera_pose.h +++ b/gpu/kinfu/tools/camera_pose.h @@ -43,6 +43,8 @@ #include #include +#include + /** * @brief The CameraPoseProcessor class is an interface to extract * camera pose data generated by the pcl_kinfu_app program. @@ -52,8 +54,8 @@ class CameraPoseProcessor { public: - using Ptr = shared_ptr; - using ConstPtr = shared_ptr; + using Ptr = pcl::shared_ptr; + using ConstPtr = pcl::shared_ptr; virtual ~CameraPoseProcessor () {} diff --git a/gpu/kinfu/tools/evaluation.h b/gpu/kinfu/tools/evaluation.h index 4f7ef0d3..0ea5ad9e 100644 --- a/gpu/kinfu/tools/evaluation.h +++ b/gpu/kinfu/tools/evaluation.h @@ -39,7 +39,7 @@ #include #include -#include +#include #include #include @@ -50,8 +50,8 @@ class Evaluation { public: - using Ptr = shared_ptr; - using ConstPtr = shared_ptr; + using Ptr = pcl::shared_ptr; + using ConstPtr = pcl::shared_ptr; using RGB = pcl::gpu::KinfuTracker::PixelRGB; Evaluation(const std::string& folder); diff --git a/gpu/kinfu/tools/kinfu_app.cpp b/gpu/kinfu/tools/kinfu_app.cpp index a9337228..f8c4ed1e 100644 --- a/gpu/kinfu/tools/kinfu_app.cpp +++ b/gpu/kinfu/tools/kinfu_app.cpp @@ -116,8 +116,8 @@ namespace pcl using RgbCloudConstPtr = pcl::PointCloud::ConstPtr; public: - using Ptr = shared_ptr >; - using ConstPtr = shared_ptr >; + using Ptr = pcl::shared_ptr >; + using ConstPtr = pcl::shared_ptr >; /** \brief Constructor. */ PointCloudColorHandlerRGBCloud (const PointCloudConstPtr& cloud, const RgbCloudConstPtr& colors) @@ -307,8 +307,8 @@ pcl::PolygonMesh::Ptr convertToMesh(const DeviceArray& triangles) struct CurrentFrameCloudView { - using Ptr = shared_ptr; - using ConstPtr = shared_ptr; + using Ptr = pcl::shared_ptr; + using ConstPtr = pcl::shared_ptr; CurrentFrameCloudView() : cloud_device_ (480, 640), cloud_viewer_ ("Frame Cloud Viewer") { diff --git a/gpu/kinfu/tools/kinfu_app_sim.cpp b/gpu/kinfu/tools/kinfu_app_sim.cpp index 4e048e79..fa5c7dc0 100644 --- a/gpu/kinfu/tools/kinfu_app_sim.cpp +++ b/gpu/kinfu/tools/kinfu_app_sim.cpp @@ -101,7 +101,7 @@ using ScopeTimeT = pcl::ScopeTime; #include #include #include -#include +#include #ifdef _WIN32 # define WIN32_LEAN_AND_MEAN # include diff --git a/gpu/kinfu_large_scale/tools/evaluation.h b/gpu/kinfu_large_scale/tools/evaluation.h index 8c130271..0fd6537c 100644 --- a/gpu/kinfu_large_scale/tools/evaluation.h +++ b/gpu/kinfu_large_scale/tools/evaluation.h @@ -39,7 +39,7 @@ #include #include -#include +#include #include #include @@ -50,8 +50,8 @@ class Evaluation { public: - using Ptr = shared_ptr; - using ConstPtr = shared_ptr; + using Ptr = pcl::shared_ptr; + using ConstPtr = pcl::shared_ptr; using RGB = pcl::gpu::kinfuLS::PixelRGB; Evaluation(const std::string& folder); diff --git a/gpu/kinfu_large_scale/tools/kinfuLS_app.cpp b/gpu/kinfu_large_scale/tools/kinfuLS_app.cpp index 497ed484..82cf19e8 100644 --- a/gpu/kinfu_large_scale/tools/kinfuLS_app.cpp +++ b/gpu/kinfu_large_scale/tools/kinfuLS_app.cpp @@ -771,16 +771,15 @@ struct KinFuLSApp void initRegistration () { - registration_ = - #ifdef HAVE_OPENNI - capture_.providesCallback () - #endif - #if defined(HAVE_OPENNI) && defined(HAVE_OPENNI2) - || - #endif - #ifdef HAVE_OPENNI2 - capture_.providesCallback (); - #endif + registration_ = + #if defined(HAVE_OPENNI) && !defined(HAVE_OPENNI2) + capture_.providesCallback (); + #elif !defined(HAVE_OPENNI) && defined(HAVE_OPENNI2) + capture_.providesCallback (); + #elif defined(HAVE_OPENNI) && defined(HAVE_OPENNI2) + capture_.providesCallback () || + capture_.providesCallback (); + #endif std::cout << "Registration mode: " << (registration_ ? "On" : "Off (not supported by source)") << std::endl; } @@ -1010,8 +1009,8 @@ struct KinFuLSApp { #ifdef HAVE_OPENNI2 using namespace pcl::io; - using DepthImagePtr = shared_ptr; - using ImagePtr = shared_ptr; + using DepthImagePtr = pcl::shared_ptr; + using ImagePtr = pcl::shared_ptr; std::function func1 = [this] (const ImagePtr& img, const DepthImagePtr& depth, float constant) @@ -1041,8 +1040,8 @@ struct KinFuLSApp { #ifdef HAVE_OPENNI using namespace openni_wrapper; - using DepthImagePtr = shared_ptr; - using ImagePtr = shared_ptr; + using DepthImagePtr = pcl::shared_ptr; + using ImagePtr = pcl::shared_ptr; std::function func1 = [this] (const ImagePtr& img, const DepthImagePtr& depth, float constant) @@ -1177,7 +1176,7 @@ struct KinFuLSApp SceneCloudView scene_cloud_view_; ImageView image_view_; - shared_ptr current_frame_cloud_view_; + pcl::shared_ptr current_frame_cloud_view_; KinfuTracker::DepthMap depth_device_; @@ -1332,7 +1331,7 @@ main (int argc, char* argv[]) // if (checkIfPreFermiGPU(device)) // return std::cout << std::endl << "Kinfu is supported only for Fermi and Kepler arhitectures. It is not even compiled for pre-Fermi by default. Exiting..." << std::endl, 1; - shared_ptr capture; + pcl::shared_ptr capture; bool triggered_capture = false; bool pcd_input = false; bool oni2_interface = false; diff --git a/gpu/kinfu_large_scale/tools/record_maps_rgb.cpp b/gpu/kinfu_large_scale/tools/record_maps_rgb.cpp index 28d1a46c..080ba8d1 100644 --- a/gpu/kinfu_large_scale/tools/record_maps_rgb.cpp +++ b/gpu/kinfu_large_scale/tools/record_maps_rgb.cpp @@ -85,8 +85,8 @@ class MapsBuffer struct MapsRgb { - using Ptr = shared_ptr; - using ConstPtr = shared_ptr; + using Ptr = pcl::shared_ptr; + using ConstPtr = pcl::shared_ptr; pcl::gpu::PtrStepSz rgb_; pcl::gpu::PtrStepSz depth_; diff --git a/gpu/octree/src/cuda/octree_builder.cu b/gpu/octree/src/cuda/octree_builder.cu index b0b877f1..dfd2093a 100644 --- a/gpu/octree/src/cuda/octree_builder.cu +++ b/gpu/octree/src/cuda/octree_builder.cu @@ -212,7 +212,7 @@ namespace pcl __syncthreads(); - while (tasks_beg < tasks_end) + while (tasks_beg < tasks_end && level < Morton::levels) { int task_count = tasks_end - tasks_beg; int iters = divUp(task_count, CTA_SIZE); @@ -245,7 +245,7 @@ namespace pcl octree.begs [offset + i] = cell_begs[i]; octree.ends [offset + i] = cell_begs[i + 1]; octree.codes[offset + i] = parent_code_shifted + cell_code[i]; - + octree.nodes[offset + i] = 0; octree.parent[offset + i] = task; mask |= (1 << cell_code[i]); } diff --git a/gpu/people/CMakeLists.txt b/gpu/people/CMakeLists.txt index 4c384d44..38cb26c2 100644 --- a/gpu/people/CMakeLists.txt +++ b/gpu/people/CMakeLists.txt @@ -70,4 +70,6 @@ PCL_MAKE_PKGCONFIG(${LIB_NAME} COMPONENT ${SUBSYS_NAME} DESC ${SUBSYS_DESC} PCL_ # install include files PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_PATH}" ${hdrs} ${hdrs_cuda}) -add_subdirectory(tools) +if(BUILD_tools) + add_subdirectory(tools) +endif() diff --git a/gpu/people/tools/CMakeLists.txt b/gpu/people/tools/CMakeLists.txt index 0c2c8a5f..3d45dbc4 100644 --- a/gpu/people/tools/CMakeLists.txt +++ b/gpu/people/tools/CMakeLists.txt @@ -1,3 +1,5 @@ +set(SUBSYS_NAME tools) + if(NOT VTK_FOUND) set(DEFAULT FALSE) set(REASON "VTK was not found.") diff --git a/gpu/people/tools/people_app.cpp b/gpu/people/tools/people_app.cpp index ebf8181d..fb63014d 100644 --- a/gpu/people/tools/people_app.cpp +++ b/gpu/people/tools/people_app.cpp @@ -60,14 +60,11 @@ #include namespace pc = pcl::console; -using namespace pcl::visualization; -using namespace pcl::gpu; -using namespace pcl; -using namespace std; +using namespace std::chrono_literals; /////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -std::vector getPcdFilesInDir(const string& directory) +std::vector getPcdFilesInDir(const std::string& directory) { namespace fs = boost::filesystem; fs::path dir(directory); @@ -75,7 +72,7 @@ std::vector getPcdFilesInDir(const string& directory) if (!fs::exists(dir) || !fs::is_directory(dir)) PCL_THROW_EXCEPTION(pcl::IOException, "Wrong PCD directory"); - std::vector result; + std::vector result; fs::directory_iterator pos(dir); fs::directory_iterator end; @@ -89,7 +86,7 @@ std::vector getPcdFilesInDir(const string& directory) /////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -struct SampledScopeTime : public StopWatch +struct SampledScopeTime : public pcl::StopWatch { enum { EACH = 33 }; SampledScopeTime(int& time_ms) : time_ms_(time_ms) {} @@ -110,7 +107,7 @@ struct SampledScopeTime : public StopWatch /////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -string +std::string make_name(int counter, const char* suffix) { char buf[4096]; @@ -167,15 +164,14 @@ class PeoplePCDApp rgba_host_.points.resize(COLS * ROWS); rgb_host_.resize(COLS * ROWS * 3); - people::uploadColorMap(color_map_); - + pcl::gpu::people::uploadColorMap(color_map_); } void visualizeAndWrite() { const PeopleDetector::Labels& labels = people_detector_.rdf_detector_->getLabels(); - people::colorizeLabels(color_map_, labels, cmap_device_); + pcl::gpu::people::colorizeLabels(color_map_, labels, cmap_device_); //people::colorizeMixedLabels( int c; @@ -212,7 +208,7 @@ class PeoplePCDApp } } - void source_cb1(const PointCloud::ConstPtr& cloud) + void source_cb1(const pcl::PointCloud::ConstPtr& cloud) { { std::lock_guard lock(data_ready_mutex_); @@ -260,7 +256,7 @@ class PeoplePCDApp for(std::size_t i = 0; i < rgba_host_.size(); ++i) { const unsigned char *pixel = &rgb_host_[i * 3]; - RGB& rgba = rgba_host_.points[i]; + pcl::RGB& rgba = rgba_host_.points[i]; rgba.r = pixel[0]; rgba.g = pixel[1]; rgba.b = pixel[2]; @@ -275,14 +271,14 @@ class PeoplePCDApp { cloud_cb_ = false; - PCDGrabberBase* ispcd = dynamic_cast(&capture_); + auto ispcd = dynamic_cast(&capture_); if (ispcd) cloud_cb_= true; using DepthImagePtr = openni_wrapper::DepthImage::Ptr; using ImagePtr = openni_wrapper::Image::Ptr; - std::function::ConstPtr&)> func1 = [this] (const PointCloud::ConstPtr& cloud) { source_cb1 (cloud); }; + std::function::ConstPtr&)> func1 = [this] (const pcl::PointCloud::ConstPtr& cloud) { source_cb1 (cloud); }; std::function func2 = [this] (const ImagePtr& img, const DepthImagePtr& depth, float constant) { source_cb2 (img, depth, constant); @@ -345,12 +341,12 @@ class PeoplePCDApp pcl::PointCloud rgba_host_; std::vector rgb_host_; - PointCloud cloud_host_; + pcl::PointCloud cloud_host_; - ImageViewer final_view_; - ImageViewer depth_view_; + pcl::visualization::ImageViewer final_view_; + pcl::visualization::ImageViewer depth_view_; - DeviceArray color_map_; + pcl::device::DeviceArray color_map_; }; void print_help() @@ -387,8 +383,8 @@ int main(int argc, char** argv) pc::parse_argument (argc, argv, "-w", write); // selecting data source - shared_ptr capture; - string openni_device, oni_file, pcd_file, pcd_folder; + pcl::shared_ptr capture; + std::string openni_device, oni_file, pcd_file, pcd_folder; try { @@ -404,29 +400,23 @@ int main(int argc, char** argv) else if (pc::parse_argument (argc, argv, "-pcd", pcd_file) > 0) { - capture.reset( new pcl::PCDGrabber(vector(31, pcd_file), 30, true) ); + capture.reset( new pcl::PCDGrabber(std::vector(31, pcd_file), 30, true) ); } else if (pc::parse_argument (argc, argv, "-pcd_folder", pcd_folder) > 0) { - std::vector pcd_files = getPcdFilesInDir(pcd_folder); - capture.reset( new pcl::PCDGrabber(pcd_files, 30, true) ); + std::vector pcd_files = getPcdFilesInDir(pcd_folder); + capture.reset( new pcl::PCDGrabber(pcd_files, 30, true) ); } else { capture.reset( new pcl::OpenNIGrabber() ); - //capture.reset( new pcl::ONIGrabber("d:/onis/20111013-224932.oni", true, true) ); - - //vector pcd_files(31, "d:/3/0008.pcd"); - //vector pcd_files(31, "d:/git/pcl/gpu/people/tools/test.pcd"); - //vector pcd_files = getPcdFilesInDir("d:/3/"); - //capture.reset( new pcl::PCDGrabber(pcd_files, 30, true) ); } } catch (const pcl::PCLException& /*e*/) { return std::cout << "Can't open depth source" << std::endl, -1; } //selecting tree files - std::vector tree_files; + std::vector tree_files; tree_files.emplace_back("Data/forest1/tree_20.txt"); tree_files.emplace_back("Data/forest2/tree_20.txt"); tree_files.emplace_back("Data/forest3/tree_20.txt"); diff --git a/io/include/pcl/compression/entropy_range_coder.h b/io/include/pcl/compression/entropy_range_coder.h index 50ebe7fb..34adb11f 100644 --- a/io/include/pcl/compression/entropy_range_coder.h +++ b/io/include/pcl/compression/entropy_range_coder.h @@ -49,6 +49,8 @@ #include #include +#include + namespace pcl { @@ -168,7 +170,7 @@ namespace pcl * \param n_arg: some value * \return binary logarithm (log2) of argument n_arg */ - [[deprecated("use std::log2 instead")]] + PCL_DEPRECATED("use std::log2 instead") inline double Log2 (double n_arg) { diff --git a/io/include/pcl/compression/impl/entropy_range_coder.hpp b/io/include/pcl/compression/impl/entropy_range_coder.hpp index 02aff405..d348b4d5 100644 --- a/io/include/pcl/compression/impl/entropy_range_coder.hpp +++ b/io/include/pcl/compression/impl/entropy_range_coder.hpp @@ -294,7 +294,7 @@ pcl::StaticRangeCoder::encodeIntVectorToStream (std::vector& input // calculate amount of bytes per frequency table entry std::uint8_t frequencyTableByteSize = static_cast (std::ceil ( - std::log2 (static_cast (cFreqTable_[static_cast (frequencyTableSize - 1)])) / 8.0)); + std::log2 (static_cast (cFreqTable_[static_cast (frequencyTableSize - 1)] + 1)) / 8.0)); // write size of frequency table to output stream outputByteStream_arg.write (reinterpret_cast (&frequencyTableSize), sizeof(frequencyTableSize)); diff --git a/io/include/pcl/compression/impl/octree_pointcloud_compression.hpp b/io/include/pcl/compression/impl/octree_pointcloud_compression.hpp index eec54095..43747389 100644 --- a/io/include/pcl/compression/impl/octree_pointcloud_compression.hpp +++ b/io/include/pcl/compression/impl/octree_pointcloud_compression.hpp @@ -509,7 +509,6 @@ namespace pcl OctreePointCloudCompression::deserializeTreeCallback (LeafT&, const OctreeKey& key_arg) { - double lowerVoxelCorner[3]; PointT newPoint; std::size_t pointCount = 1; @@ -528,6 +527,7 @@ namespace pcl output_->points.push_back (newPoint); // calculcate position of lower voxel corner + double lowerVoxelCorner[3]; 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_; diff --git a/io/include/pcl/io/ascii_io.h b/io/include/pcl/io/ascii_io.h index 7c15686e..c7ebf40a 100644 --- a/io/include/pcl/io/ascii_io.h +++ b/io/include/pcl/io/ascii_io.h @@ -37,6 +37,7 @@ #pragma once +#include #include #include #include @@ -116,7 +117,7 @@ namespace pcl * \param[in] p a point type */ template - [[deprecated("use parameterless setInputFields() instead")]] + PCL_DEPRECATED("use parameterless setInputFields() instead") inline void setInputFields (const PointT p) { (void) p; diff --git a/io/include/pcl/io/davidsdk_grabber.h b/io/include/pcl/io/davidsdk_grabber.h index 88d4ebe5..6f7f5645 100644 --- a/io/include/pcl/io/davidsdk_grabber.h +++ b/io/include/pcl/io/davidsdk_grabber.h @@ -88,7 +88,7 @@ namespace pcl /** @brief Destructor inherited from the Grabber interface. It never throws. */ virtual - ~DavidSDKGrabber () throw (); + ~DavidSDKGrabber () noexcept; /** @brief [Connect](http://docs.david-3d.com/sdk/en/classdavid_1_1_client_json_rpc.html#a4b948e57a2e5e7f9cdcf1171c500aa24) client * @param[in] address diff --git a/io/include/pcl/io/depth_sense/depth_sense_grabber_impl.h b/io/include/pcl/io/depth_sense/depth_sense_grabber_impl.h index 40848e9b..8ebf2a86 100644 --- a/io/include/pcl/io/depth_sense/depth_sense_grabber_impl.h +++ b/io/include/pcl/io/depth_sense/depth_sense_grabber_impl.h @@ -105,7 +105,7 @@ namespace pcl DepthSenseGrabberImpl (DepthSenseGrabber* parent, const std::string& device_id); - ~DepthSenseGrabberImpl () throw (); + ~DepthSenseGrabberImpl () noexcept; void start (); diff --git a/io/include/pcl/io/depth_sense_grabber.h b/io/include/pcl/io/depth_sense_grabber.h index 5f2584f1..28a902be 100644 --- a/io/include/pcl/io/depth_sense_grabber.h +++ b/io/include/pcl/io/depth_sense_grabber.h @@ -92,7 +92,7 @@ namespace pcl DepthSenseGrabber (const std::string& device_id = ""); virtual - ~DepthSenseGrabber () throw (); + ~DepthSenseGrabber () noexcept; virtual void start (); diff --git a/io/include/pcl/io/dinast_grabber.h b/io/include/pcl/io/dinast_grabber.h index f79c77a6..7ee5cadb 100644 --- a/io/include/pcl/io/dinast_grabber.h +++ b/io/include/pcl/io/dinast_grabber.h @@ -68,7 +68,7 @@ namespace pcl DinastGrabber (const int device_position=1); /** \brief Destructor. It never throws. */ - ~DinastGrabber () throw (); + ~DinastGrabber () noexcept; /** \brief Check if the grabber is running * \return true if grabber is running / streaming. False otherwise. diff --git a/io/include/pcl/io/ensenso_grabber.h b/io/include/pcl/io/ensenso_grabber.h index ce6c2799..c445e6c0 100644 --- a/io/include/pcl/io/ensenso_grabber.h +++ b/io/include/pcl/io/ensenso_grabber.h @@ -88,7 +88,7 @@ namespace pcl /** @brief Destructor inherited from the Grabber interface. It never throws. */ virtual - ~EnsensoGrabber () throw (); + ~EnsensoGrabber () noexcept; /** @brief Searches for available devices * @returns The number of Ensenso devices connected */ diff --git a/io/include/pcl/io/grabber.h b/io/include/pcl/io/grabber.h index 596b2b06..b94d42d0 100644 --- a/io/include/pcl/io/grabber.h +++ b/io/include/pcl/io/grabber.h @@ -57,12 +57,8 @@ namespace pcl class PCL_EXPORTS Grabber { public: - - /** \brief Constructor. */ - Grabber () {} - /** \brief virtual destructor. */ - virtual inline ~Grabber () throw (); + virtual inline ~Grabber () noexcept; /** \brief registers a callback function/method to a signal with the corresponding signature * \param[in] callback: the callback function/method @@ -76,7 +72,7 @@ namespace pcl * \return Connection object, that can be used to disconnect the callback method from the signal again. */ template class FunctionT> - [[deprecated ("please assign the callback to a std::function.")]] + PCL_DEPRECATED ("please assign the callback to a std::function.") boost::signals2::connection registerCallback (const FunctionT& callback) { @@ -101,6 +97,13 @@ namespace pcl virtual void stop () = 0; + /** \brief For devices that are streaming, stopped streams are started and running stream are stopped. + * For triggered devices, the behavior is not defined. + * \return true if grabber is running / streaming. False otherwise. + */ + inline bool + toggle (); + /** \brief returns the name of the concrete subclass. * \return the name of the concrete driver. */ @@ -151,12 +154,25 @@ namespace pcl std::map > shared_connections_; } ; - Grabber::~Grabber () throw () + Grabber::~Grabber () noexcept { for (auto &signal : signals_) delete signal.second; } + bool + Grabber::toggle () + { + if (isRunning ()) + { + stop (); + } else + { + start (); + } + return isRunning (); + } + template boost::signals2::signal* Grabber::find_signal () const { diff --git a/io/include/pcl/io/hdl_grabber.h b/io/include/pcl/io/hdl_grabber.h index c60debb0..d33352ea 100644 --- a/io/include/pcl/io/hdl_grabber.h +++ b/io/include/pcl/io/hdl_grabber.h @@ -39,6 +39,7 @@ #pragma once #include "pcl/pcl_config.h" +#include #include #include @@ -70,7 +71,7 @@ namespace pcl */ using sig_cb_velodyne_hdl_scan_point_cloud_xyzrgba = void (const pcl::PointCloud::ConstPtr &, float, float); - using sig_cb_velodyne_hdl_scan_point_cloud_xyzrgb [[deprecated("use sig_cb_velodyne_hdl_scan_point_cloud_xyzrgba instead")]] + using sig_cb_velodyne_hdl_scan_point_cloud_xyzrgb PCL_DEPRECATED("use sig_cb_velodyne_hdl_scan_point_cloud_xyzrgba instead") = sig_cb_velodyne_hdl_scan_point_cloud_xyzrgba; /** \brief Signal used for a single sector @@ -96,7 +97,7 @@ namespace pcl */ using sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgba = void (const pcl::PointCloud::ConstPtr &); - using sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgb [[deprecated("use sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgba instead")]] + using sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgb PCL_DEPRECATED("use sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgba instead") = sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgba; /** \brief Constructor taking an optional path to an HDL corrections file. The Grabber will listen on the default IP/port for data packets [192.168.3.255/2368] @@ -117,7 +118,7 @@ namespace pcl /** \brief virtual Destructor inherited from the Grabber interface. It never throws. */ - ~HDLGrabber () throw (); + ~HDLGrabber () noexcept; /** \brief Starts processing the Velodyne packets, either from the network or PCAP file. */ void diff --git a/io/include/pcl/io/image_grabber.h b/io/include/pcl/io/image_grabber.h index a5f8453a..0b505c6a 100644 --- a/io/include/pcl/io/image_grabber.h +++ b/io/include/pcl/io/image_grabber.h @@ -94,7 +94,7 @@ namespace pcl } /** \brief Virtual destructor. */ - ~ImageGrabberBase () throw (); + ~ImageGrabberBase () noexcept; /** \brief Starts playing the list of PCD files if frames_per_second is > 0. Otherwise it works as a trigger: publishes only the next PCD file in the list. */ void @@ -235,7 +235,7 @@ namespace pcl bool repeat = false); /** \brief Empty destructor */ - ~ImageGrabber () throw () {} + ~ImageGrabber () noexcept {} // Inherited from FileGrabber const typename pcl::PointCloud::ConstPtr diff --git a/io/include/pcl/io/image_ir.h b/io/include/pcl/io/image_ir.h index 7b461ba9..ad6c2753 100644 --- a/io/include/pcl/io/image_ir.h +++ b/io/include/pcl/io/image_ir.h @@ -63,7 +63,7 @@ namespace pcl IRImage (FrameWrapper::Ptr ir_metadata); IRImage (FrameWrapper::Ptr ir_metadata, Timestamp time); - ~IRImage () throw () + ~IRImage () noexcept {} void diff --git a/io/include/pcl/io/image_rgb24.h b/io/include/pcl/io/image_rgb24.h index dfb2467d..36d5e03a 100644 --- a/io/include/pcl/io/image_rgb24.h +++ b/io/include/pcl/io/image_rgb24.h @@ -55,7 +55,7 @@ namespace pcl ImageRGB24 (FrameWrapper::Ptr image_metadata); ImageRGB24 (FrameWrapper::Ptr image_metadata, Timestamp timestamp); - ~ImageRGB24 () throw (); + ~ImageRGB24 () noexcept; inline Encoding getEncoding () const override diff --git a/io/include/pcl/io/image_yuv422.h b/io/include/pcl/io/image_yuv422.h index 90ac51f5..08f7d227 100644 --- a/io/include/pcl/io/image_yuv422.h +++ b/io/include/pcl/io/image_yuv422.h @@ -55,7 +55,7 @@ namespace pcl ImageYUV422 (FrameWrapper::Ptr image_metadata); ImageYUV422 (FrameWrapper::Ptr image_metadata, Timestamp timestamp); - ~ImageYUV422 () throw (); + ~ImageYUV422 () noexcept; inline Encoding getEncoding () const override diff --git a/io/include/pcl/io/impl/ascii_io.hpp b/io/include/pcl/io/impl/ascii_io.hpp index 1e8a6b5a..ba314145 100644 --- a/io/include/pcl/io/impl/ascii_io.hpp +++ b/io/include/pcl/io/impl/ascii_io.hpp @@ -46,7 +46,7 @@ pcl::ASCIIReader::setInputFields () // Remove empty fields and adjust offset int offset =0; for (std::vector::iterator field_iter = fields_.begin (); - field_iter != fields_.end (); field_iter++) + field_iter != fields_.end (); ++field_iter) { if (field_iter->name == "_") field_iter = fields_.erase (field_iter); diff --git a/io/include/pcl/io/impl/lzf_image_io.hpp b/io/include/pcl/io/impl/lzf_image_io.hpp index 96fe127e..fe8360dc 100644 --- a/io/include/pcl/io/impl/lzf_image_io.hpp +++ b/io/include/pcl/io/impl/lzf_image_io.hpp @@ -41,6 +41,12 @@ #include #include +#include +#include +#include +#include +#include + #define CLIP_CHAR(c) static_cast ((c)>255?255:(c)<0?0:(c)) ////////////////////////////////////////////////////////////////////////////// diff --git a/io/include/pcl/io/impl/pcd_io.hpp b/io/include/pcl/io/impl/pcd_io.hpp index 374e9c19..6a78597c 100644 --- a/io/include/pcl/io/impl/pcd_io.hpp +++ b/io/include/pcl/io/impl/pcd_io.hpp @@ -121,7 +121,7 @@ pcl::PCDWriter::writeBinary (const std::string &file_name, oss.flush (); data_idx = static_cast (oss.tellp ()); -#if _WIN32 +#ifdef _WIN32 HANDLE h_native_file = CreateFileA (file_name.c_str (), GENERIC_READ | GENERIC_WRITE, 0, NULL, CREATE_ALWAYS, FILE_ATTRIBUTE_NORMAL, NULL); if (h_native_file == INVALID_HANDLE_VALUE) { @@ -161,7 +161,7 @@ pcl::PCDWriter::writeBinary (const std::string &file_name, data_size = cloud.points.size () * fsize; // Prepare the map -#if _WIN32 +#ifdef _WIN32 HANDLE fm = CreateFileMappingA (h_native_file, NULL, PAGE_READWRITE, 0, (DWORD) (data_idx + data_size), NULL); if (fm == NULL) { @@ -209,13 +209,13 @@ pcl::PCDWriter::writeBinary (const std::string &file_name, } // If the user set the synchronization flag on, call msync -#if !_WIN32 +#ifndef _WIN32 if (map_synchronization_) msync (map, data_idx + data_size, MS_SYNC); #endif // Unmap the pages of memory -#if _WIN32 +#ifdef _WIN32 UnmapViewOfFile (map); #else if (::munmap (map, (data_idx + data_size)) == -1) @@ -227,7 +227,7 @@ pcl::PCDWriter::writeBinary (const std::string &file_name, } #endif // Close file -#if _WIN32 +#ifdef _WIN32 CloseHandle (h_native_file); #else io::raw_close (fd); @@ -252,7 +252,7 @@ pcl::PCDWriter::writeBinaryCompressed (const std::string &file_name, oss.flush (); data_idx = static_cast (oss.tellp ()); -#if _WIN32 +#ifdef _WIN32 HANDLE h_native_file = CreateFileA (file_name.c_str (), GENERIC_READ | GENERIC_WRITE, 0, NULL, CREATE_ALWAYS, FILE_ATTRIBUTE_NORMAL, NULL); if (h_native_file == INVALID_HANDLE_VALUE) { @@ -355,7 +355,7 @@ pcl::PCDWriter::writeBinaryCompressed (const std::string &file_name, } else { -#if !_WIN32 +#ifndef _WIN32 io::raw_close (fd); #endif resetLockingPermissions (file_name, file_lock); @@ -364,7 +364,7 @@ pcl::PCDWriter::writeBinaryCompressed (const std::string &file_name, } // Prepare the map -#if _WIN32 +#ifdef _WIN32 HANDLE fm = CreateFileMapping (h_native_file, NULL, PAGE_READWRITE, 0, compressed_final_size, NULL); char *map = static_cast(MapViewOfFile (fm, FILE_MAP_READ | FILE_MAP_WRITE, 0, 0, compressed_final_size)); CloseHandle (fm); @@ -396,14 +396,14 @@ pcl::PCDWriter::writeBinaryCompressed (const std::string &file_name, // Copy the compressed data memcpy (&map[data_idx], temp_buf, data_size); -#if !_WIN32 +#ifndef _WIN32 // If the user set the synchronization flag on, call msync if (map_synchronization_) msync (map, compressed_final_size, MS_SYNC); #endif // Unmap the pages of memory -#if _WIN32 +#ifdef _WIN32 UnmapViewOfFile (map); #else if (::munmap (map, (compressed_final_size)) == -1) @@ -416,7 +416,7 @@ pcl::PCDWriter::writeBinaryCompressed (const std::string &file_name, #endif // Close file -#if _WIN32 +#ifdef _WIN32 CloseHandle (h_native_file); #else io::raw_close (fd); @@ -598,7 +598,7 @@ pcl::PCDWriter::writeBinary (const std::string &file_name, oss.flush (); data_idx = static_cast (oss.tellp ()); -#if _WIN32 +#ifdef _WIN32 HANDLE h_native_file = CreateFileA (file_name.c_str (), GENERIC_READ | GENERIC_WRITE, 0, NULL, CREATE_ALWAYS, FILE_ATTRIBUTE_NORMAL, NULL); if (h_native_file == INVALID_HANDLE_VALUE) { @@ -638,7 +638,7 @@ pcl::PCDWriter::writeBinary (const std::string &file_name, data_size = indices.size () * fsize; // Prepare the map -#if _WIN32 +#ifdef _WIN32 HANDLE fm = CreateFileMapping (h_native_file, NULL, PAGE_READWRITE, 0, data_idx + data_size, NULL); char *map = static_cast(MapViewOfFile (fm, FILE_MAP_READ | FILE_MAP_WRITE, 0, 0, data_idx + data_size)); CloseHandle (fm); @@ -680,14 +680,14 @@ pcl::PCDWriter::writeBinary (const std::string &file_name, } } -#if !_WIN32 +#ifndef _WIN32 // If the user set the synchronization flag on, call msync if (map_synchronization_) msync (map, data_idx + data_size, MS_SYNC); #endif // Unmap the pages of memory -#if _WIN32 +#ifdef _WIN32 UnmapViewOfFile (map); #else if (::munmap (map, (data_idx + data_size)) == -1) @@ -699,7 +699,7 @@ pcl::PCDWriter::writeBinary (const std::string &file_name, } #endif // Close file -#if _WIN32 +#ifdef _WIN32 CloseHandle(h_native_file); #else io::raw_close (fd); diff --git a/io/include/pcl/io/io_exception.h b/io/include/pcl/io/io_exception.h index b053708a..c78f0ec7 100644 --- a/io/include/pcl/io/io_exception.h +++ b/io/include/pcl/io/io_exception.h @@ -67,7 +67,7 @@ namespace pcl unsigned line_number, const std::string& message); - ~IOException () throw (); + ~IOException () noexcept; IOException& operator= (const IOException& exception); diff --git a/io/include/pcl/io/oni_grabber.h b/io/include/pcl/io/oni_grabber.h index 1ac13820..877ff809 100644 --- a/io/include/pcl/io/oni_grabber.h +++ b/io/include/pcl/io/oni_grabber.h @@ -88,7 +88,7 @@ namespace pcl ONIGrabber (const std::string& file_name, bool repeat, bool stream); /** \brief destructor never throws an exception */ - ~ONIGrabber () throw (); + ~ONIGrabber () noexcept; /** \brief For devices that are streaming, the streams are started by calling this method. * Trigger-based devices, just trigger the device once for each call of start. diff --git a/io/include/pcl/io/openni2_grabber.h b/io/include/pcl/io/openni2_grabber.h index a61db35a..83a2aab9 100644 --- a/io/include/pcl/io/openni2_grabber.h +++ b/io/include/pcl/io/openni2_grabber.h @@ -147,7 +147,7 @@ namespace pcl const Mode& image_mode = OpenNI_Default_Mode); /** \brief virtual Destructor inherited from the Grabber interface. It never throws. */ - ~OpenNI2Grabber () throw (); + ~OpenNI2Grabber () noexcept; /** \brief Start the data acquisition. */ void diff --git a/io/include/pcl/io/openni_camera/openni_depth_image.h b/io/include/pcl/io/openni_camera/openni_depth_image.h index 32fbe0cd..66e54403 100644 --- a/io/include/pcl/io/openni_camera/openni_depth_image.h +++ b/io/include/pcl/io/openni_camera/openni_depth_image.h @@ -69,10 +69,10 @@ namespace openni_wrapper * \param[in] no_sample_value defines which values in the depth data are indicating that no depth (disparity) could be determined . * \attention The focal length may change, depending whether the depth stream is registered/mapped to the RGB stream or not. */ - inline DepthImage (pcl::shared_ptr depth_meta_data, float baseline, float focal_length, XnUInt64 shadow_value, XnUInt64 no_sample_value) throw (); + inline DepthImage (pcl::shared_ptr depth_meta_data, float baseline, float focal_length, XnUInt64 shadow_value, XnUInt64 no_sample_value) noexcept; /** \brief Destructor. Never throws an exception. */ - inline virtual ~DepthImage () throw (); + inline virtual ~DepthImage () noexcept; /** \brief method to access the internal data structure from OpenNI. If the data is accessed just read-only, then this method is faster than a fillXXX method * \return the actual depth data of type xn::DepthMetaData. @@ -163,14 +163,14 @@ namespace openni_wrapper XnUInt64 no_sample_value_; } ; - DepthImage::DepthImage (pcl::shared_ptr depth_meta_data, float baseline, float focal_length, XnUInt64 shadow_value, XnUInt64 no_sample_value) throw () + DepthImage::DepthImage (pcl::shared_ptr depth_meta_data, float baseline, float focal_length, XnUInt64 shadow_value, XnUInt64 no_sample_value) noexcept : depth_md_ (std::move(depth_meta_data)) , baseline_ (baseline) , focal_length_ (focal_length) , shadow_value_ (shadow_value) , no_sample_value_ (no_sample_value) { } - DepthImage::~DepthImage () throw () { } + DepthImage::~DepthImage () noexcept { } const xn::DepthMetaData& DepthImage::getDepthMetaData () const throw () diff --git a/io/include/pcl/io/openni_camera/openni_device.h b/io/include/pcl/io/openni_camera/openni_device.h index 72b80826..afdc35b2 100644 --- a/io/include/pcl/io/openni_camera/openni_device.h +++ b/io/include/pcl/io/openni_camera/openni_device.h @@ -90,7 +90,7 @@ namespace openni_wrapper public: /** \brief virtual destructor. Never throws an exception. */ - virtual ~OpenNIDevice () throw (); + virtual ~OpenNIDevice () noexcept; /** \brief finds an image output mode that can be used to retrieve images in desired output mode. * e.g If device just supports VGA at 30Hz, then the desired mode QVGA at 30Hz would be possible by down sampling, @@ -290,7 +290,7 @@ namespace openni_wrapper * \return a callback handler that can be used to remove the user callback from list of image-stream callbacks. */ CallbackHandle - registerImageCallback (const ImageCallbackFunction& callback, void* cookie = nullptr) throw (); + registerImageCallback (const ImageCallbackFunction& callback, void* cookie = nullptr) noexcept; /** \brief registers a callback function for the image stream with an optional user defined parameter. * This version is used to register a member function of any class. @@ -301,14 +301,14 @@ namespace openni_wrapper * \return a callback handler that can be used to remove the user callback from list of image-stream callbacks. */ template CallbackHandle - registerImageCallback (void (T::*callback)(Image::Ptr, void* cookie), T& instance, void* cookie = nullptr) throw (); + registerImageCallback (void (T::*callback)(Image::Ptr, void* cookie), T& instance, void* cookie = nullptr) noexcept; /** \brief unregisters a callback function. i.e. removes that function from the list of image stream callbacks. * \param[in] callbackHandle the handle of the callback to unregister. * \return true, if callback was in list and could be unregistered, false otherwise. */ bool - unregisterImageCallback (const CallbackHandle& callbackHandle) throw (); + unregisterImageCallback (const CallbackHandle& callbackHandle) noexcept; /** \brief registers a callback function of std::function type for the depth stream with an optional user defined parameter. @@ -318,7 +318,7 @@ namespace openni_wrapper * \return a callback handler that can be used to remove the user callback from list of depth-stream callbacks. */ CallbackHandle - registerDepthCallback (const DepthImageCallbackFunction& callback, void* cookie = nullptr) throw (); + registerDepthCallback (const DepthImageCallbackFunction& callback, void* cookie = nullptr) noexcept; /** \brief registers a callback function for the depth stream with an optional user defined parameter. * This version is used to register a member function of any class. @@ -329,14 +329,14 @@ namespace openni_wrapper * \return a callback handler that can be used to remove the user callback from list of depth-stream callbacks. */ template CallbackHandle - registerDepthCallback (void (T::*callback)(DepthImage::Ptr, void* cookie), T& instance, void* cookie = nullptr) throw (); + registerDepthCallback (void (T::*callback)(DepthImage::Ptr, void* cookie), T& instance, void* cookie = nullptr) noexcept; /** \brief unregisters a callback function. i.e. removes that function from the list of depth stream callbacks. * \param[in] callbackHandle the handle of the callback to unregister. * \return true, if callback was in list and could be unregistered, false otherwise. */ bool - unregisterDepthCallback (const CallbackHandle& callbackHandle) throw (); + unregisterDepthCallback (const CallbackHandle& callbackHandle) noexcept; /** \brief registers a callback function of std::function type for the IR stream with an optional user defined parameter. * The callback will always be called with a new IR image and the user data "cookie". @@ -345,7 +345,7 @@ namespace openni_wrapper * \return a callback handler that can be used to remove the user callback from list of IR-stream callbacks. */ CallbackHandle - registerIRCallback (const IRImageCallbackFunction& callback, void* cookie = nullptr) throw (); + registerIRCallback (const IRImageCallbackFunction& callback, void* cookie = nullptr) noexcept; /** \brief registers a callback function for the IR stream with an optional user defined parameter. * This version is used to register a member function of any class. @@ -356,14 +356,14 @@ namespace openni_wrapper * \return a callback handler that can be used to remove the user callback from list of IR-stream callbacks. */ template CallbackHandle - registerIRCallback (void (T::*callback)(IRImage::Ptr, void* cookie), T& instance, void* cookie = nullptr) throw (); + registerIRCallback (void (T::*callback)(IRImage::Ptr, void* cookie), T& instance, void* cookie = nullptr) noexcept; /** \brief unregisters a callback function. i.e. removes that function from the list of IR stream callbacks. * \param[in] callbackHandle the handle of the callback to unregister. * \return true, if callback was in list and could be unregistered, false otherwise. */ bool - unregisterIRCallback (const CallbackHandle& callbackHandle) throw (); + unregisterIRCallback (const CallbackHandle& callbackHandle) noexcept; /** \brief returns the serial number for device. * \attention This might be an empty string!!! @@ -455,9 +455,9 @@ namespace openni_wrapper OpenNIDevice (xn::Context& context, const xn::NodeInfo& device_node, const xn::NodeInfo& image_node, const xn::NodeInfo& depth_node, const xn::NodeInfo& ir_node); OpenNIDevice (xn::Context& context, const xn::NodeInfo& device_node, const xn::NodeInfo& depth_node, const xn::NodeInfo& ir_node); OpenNIDevice (xn::Context& context); - static void __stdcall NewDepthDataAvailable (xn::ProductionNode& node, void* cookie) throw (); - static void __stdcall NewImageDataAvailable (xn::ProductionNode& node, void* cookie) throw (); - static void __stdcall NewIRDataAvailable (xn::ProductionNode& node, void* cookie) throw (); + static void __stdcall NewDepthDataAvailable (xn::ProductionNode& node, void* cookie) noexcept; + static void __stdcall NewImageDataAvailable (xn::ProductionNode& node, void* cookie) noexcept; + static void __stdcall NewIRDataAvailable (xn::ProductionNode& node, void* cookie) noexcept; // This is a workaround, since in the NewDepthDataAvailable function WaitAndUpdateData leads to a dead-lock behaviour // and retrieving image data without WaitAndUpdateData leads to incomplete images!!! @@ -593,7 +593,7 @@ namespace openni_wrapper //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template OpenNIDevice::CallbackHandle - OpenNIDevice::registerImageCallback (void (T::*callback)(Image::Ptr, void* cookie), T& instance, void* custom_data) throw () + OpenNIDevice::registerImageCallback (void (T::*callback)(Image::Ptr, void* cookie), T& instance, void* custom_data) noexcept { image_callback_[image_callback_handle_counter_] = [=, &instance] (Image::Ptr img) { (instance.*callback) (img, custom_data); }; return (image_callback_handle_counter_++); @@ -601,7 +601,7 @@ namespace openni_wrapper //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template OpenNIDevice::CallbackHandle - OpenNIDevice::registerDepthCallback (void (T::*callback)(DepthImage::Ptr, void* cookie), T& instance, void* custom_data) throw () + OpenNIDevice::registerDepthCallback (void (T::*callback)(DepthImage::Ptr, void* cookie), T& instance, void* custom_data) noexcept { depth_callback_[depth_callback_handle_counter_] = [=, &instance] (DepthImage::Ptr img) { (instance.*callback) (img, custom_data); }; return (depth_callback_handle_counter_++); @@ -609,7 +609,7 @@ namespace openni_wrapper //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template OpenNIDevice::CallbackHandle - OpenNIDevice::registerIRCallback (void (T::*callback)(IRImage::Ptr, void* cookie), T& instance, void* custom_data) throw () + OpenNIDevice::registerIRCallback (void (T::*callback)(IRImage::Ptr, void* cookie), T& instance, void* custom_data) noexcept { ir_callback_[ir_callback_handle_counter_] = [=, &instance] (IRImage::Ptr img) { (instance.*callback) (img, custom_data); }; return (ir_callback_handle_counter_++); diff --git a/io/include/pcl/io/openni_camera/openni_device_kinect.h b/io/include/pcl/io/openni_camera/openni_device_kinect.h index e7c33a17..c719c7ea 100644 --- a/io/include/pcl/io/openni_camera/openni_device_kinect.h +++ b/io/include/pcl/io/openni_camera/openni_device_kinect.h @@ -58,22 +58,22 @@ namespace openni_wrapper friend class OpenNIDriver; public: DeviceKinect (xn::Context& context, const xn::NodeInfo& device_node, const xn::NodeInfo& image_node, const xn::NodeInfo& depth_node, const xn::NodeInfo& ir_node); - ~DeviceKinect () throw (); + ~DeviceKinect () noexcept; - inline void setDebayeringMethod (const ImageBayerGRBG::DebayeringMethod& debayering_method) throw (); + inline void setDebayeringMethod (const ImageBayerGRBG::DebayeringMethod& debayering_method) noexcept; inline const ImageBayerGRBG::DebayeringMethod& getDebayeringMethod () const throw (); bool isSynchronizationSupported () const throw () override; protected: Image::Ptr getCurrentImage (pcl::shared_ptr image_meta_data) const throw () override; - void enumAvailableModes () throw (); + void enumAvailableModes () noexcept; bool isImageResizeSupported (unsigned input_width, unsigned input_height, unsigned output_width, unsigned output_height) const throw () override; ImageBayerGRBG::DebayeringMethod debayering_method_; } ; void - DeviceKinect::setDebayeringMethod (const ImageBayerGRBG::DebayeringMethod& debayering_method) throw () + DeviceKinect::setDebayeringMethod (const ImageBayerGRBG::DebayeringMethod& debayering_method) noexcept { debayering_method_ = debayering_method; } diff --git a/io/include/pcl/io/openni_camera/openni_device_oni.h b/io/include/pcl/io/openni_camera/openni_device_oni.h index 2e031dfa..913ec8ef 100644 --- a/io/include/pcl/io/openni_camera/openni_device_oni.h +++ b/io/include/pcl/io/openni_camera/openni_device_oni.h @@ -66,7 +66,7 @@ namespace openni_wrapper using ConstPtr = pcl::shared_ptr; DeviceONI (xn::Context& context, const std::string& file_name, bool repeat = false, bool streaming = true); - ~DeviceONI () throw (); + ~DeviceONI () noexcept; void startImageStream () override; void stopImageStream () override; @@ -102,9 +102,9 @@ namespace openni_wrapper Image::Ptr getCurrentImage (pcl::shared_ptr image_meta_data) const throw () override; void PlayerThreadFunction (); - static void __stdcall NewONIDepthDataAvailable (xn::ProductionNode& node, void* cookie) throw (); - static void __stdcall NewONIImageDataAvailable (xn::ProductionNode& node, void* cookie) throw (); - static void __stdcall NewONIIRDataAvailable (xn::ProductionNode& node, void* cookie) throw (); + static void __stdcall NewONIDepthDataAvailable (xn::ProductionNode& node, void* cookie) noexcept; + static void __stdcall NewONIImageDataAvailable (xn::ProductionNode& node, void* cookie) noexcept; + static void __stdcall NewONIIRDataAvailable (xn::ProductionNode& node, void* cookie) noexcept; xn::Player player_; std::thread player_thread_; diff --git a/io/include/pcl/io/openni_camera/openni_device_primesense.h b/io/include/pcl/io/openni_camera/openni_device_primesense.h index c8250276..08b19df3 100644 --- a/io/include/pcl/io/openni_camera/openni_device_primesense.h +++ b/io/include/pcl/io/openni_camera/openni_device_primesense.h @@ -59,12 +59,12 @@ class DevicePrimesense : public OpenNIDevice friend class OpenNIDriver; public: DevicePrimesense (xn::Context& context, const xn::NodeInfo& device_node, const xn::NodeInfo& image_node, const xn::NodeInfo& depth_node, const xn::NodeInfo& ir_node); - ~DevicePrimesense () throw (); + ~DevicePrimesense () noexcept; //virtual void setImageOutputMode (const XnMapOutputMode& output_mode); protected: Image::Ptr getCurrentImage (pcl::shared_ptr image_meta_data) const throw () override; - void enumAvailableModes () throw (); + void enumAvailableModes () noexcept; bool isImageResizeSupported (unsigned input_width, unsigned input_height, unsigned output_width, unsigned output_height) const throw () override; void startImageStream () override; diff --git a/io/include/pcl/io/openni_camera/openni_device_xtion.h b/io/include/pcl/io/openni_camera/openni_device_xtion.h index e9971000..ec210275 100644 --- a/io/include/pcl/io/openni_camera/openni_device_xtion.h +++ b/io/include/pcl/io/openni_camera/openni_device_xtion.h @@ -61,12 +61,12 @@ namespace openni_wrapper friend class OpenNIDriver; public: DeviceXtionPro (xn::Context& context, const xn::NodeInfo& device_node, const xn::NodeInfo& depth_node, const xn::NodeInfo& ir_node); - ~DeviceXtionPro () throw (); + ~DeviceXtionPro () noexcept; //virtual void setImageOutputMode (const XnMapOutputMode& output_mode); protected: Image::Ptr getCurrentImage (pcl::shared_ptr image_meta_data) const throw () override; - void enumAvailableModes () throw (); + void enumAvailableModes () noexcept; bool isImageResizeSupported (unsigned input_width, unsigned input_height, unsigned output_width, unsigned output_height) const throw () override; void startDepthStream () override; diff --git a/io/include/pcl/io/openni_camera/openni_driver.h b/io/include/pcl/io/openni_camera/openni_driver.h index bcc30ad3..6ece4940 100644 --- a/io/include/pcl/io/openni_camera/openni_driver.h +++ b/io/include/pcl/io/openni_camera/openni_driver.h @@ -68,7 +68,7 @@ namespace openni_wrapper * @author Suat Gedikli * @brief virtual Destructor that never throws an exception */ - ~OpenNIDriver () throw (); + ~OpenNIDriver () noexcept; /** * @author Suat Gedikli @@ -225,7 +225,7 @@ namespace openni_wrapper #ifndef _WIN32 // workaround to get additional device nformation like serial number, vendor and product name, until Primesense fix this - void getDeviceInfos () throw (); + void getDeviceInfos () noexcept; #endif mutable std::vector device_context_; diff --git a/io/include/pcl/io/openni_camera/openni_exception.h b/io/include/pcl/io/openni_camera/openni_exception.h index d2ec7489..dd65e3f1 100644 --- a/io/include/pcl/io/openni_camera/openni_exception.h +++ b/io/include/pcl/io/openni_camera/openni_exception.h @@ -75,19 +75,19 @@ namespace openni_wrapper * @param[in] line_number the line number where this exception was created. * @param[in] message the message of the exception */ - OpenNIException (const std::string& function_name, const std::string& file_name, unsigned line_number, const std::string& message) throw (); + OpenNIException (const std::string& function_name, const std::string& file_name, unsigned line_number, const std::string& message) noexcept; /** * @brief virtual Destructor that never throws an exception */ - ~OpenNIException () throw (); + ~OpenNIException () noexcept; /** * @brief Assignment operator to allow copying the message of another exception variable. * @param[in] exception left hand side * @return */ - OpenNIException & operator= (const OpenNIException& exception) throw (); + OpenNIException & operator= (const OpenNIException& exception) noexcept; /** * @brief virtual method, derived from std::exception diff --git a/io/include/pcl/io/openni_camera/openni_image.h b/io/include/pcl/io/openni_camera/openni_image.h index 02fc1d78..df8c9318 100644 --- a/io/include/pcl/io/openni_camera/openni_image.h +++ b/io/include/pcl/io/openni_camera/openni_image.h @@ -74,13 +74,13 @@ namespace openni_wrapper * @brief Constructor * @param[in] image_meta_data the actual image data from the OpenNI driver */ - inline Image (pcl::shared_ptr image_meta_data) throw (); + inline Image (pcl::shared_ptr image_meta_data) noexcept; /** * @author Suat Gedikli * @brief virtual Destructor that never throws an exception. */ - inline virtual ~Image () throw (); + inline virtual ~Image () noexcept; /** * @author Suat Gedikli @@ -169,12 +169,12 @@ namespace openni_wrapper pcl::shared_ptr image_md_; } ; - Image::Image (pcl::shared_ptr image_meta_data) throw () + Image::Image (pcl::shared_ptr image_meta_data) noexcept : image_md_ (std::move(image_meta_data)) { } - Image::~Image () throw () { } + Image::~Image () noexcept { } unsigned Image::getWidth () const throw () diff --git a/io/include/pcl/io/openni_camera/openni_image_bayer_grbg.h b/io/include/pcl/io/openni_camera/openni_image_bayer_grbg.h index f809fea5..ca6803a5 100644 --- a/io/include/pcl/io/openni_camera/openni_image_bayer_grbg.h +++ b/io/include/pcl/io/openni_camera/openni_image_bayer_grbg.h @@ -61,8 +61,8 @@ namespace openni_wrapper EdgeAwareWeighted }; - ImageBayerGRBG (pcl::shared_ptr image_meta_data, DebayeringMethod method) throw (); - ~ImageBayerGRBG () throw (); + ImageBayerGRBG (pcl::shared_ptr image_meta_data, DebayeringMethod method) noexcept; + ~ImageBayerGRBG () noexcept; inline Encoding getEncoding () const override @@ -73,7 +73,7 @@ namespace openni_wrapper void fillRGB (unsigned width, unsigned height, unsigned char* rgb_buffer, unsigned rgb_line_step = 0) const override; void fillGrayscale (unsigned width, unsigned height, unsigned char* gray_buffer, unsigned gray_line_step = 0) const override; bool isResizingSupported (unsigned input_width, unsigned input_height, unsigned output_width, unsigned output_height) const override; - inline void setDebayeringMethod (const DebayeringMethod& method) throw (); + inline void setDebayeringMethod (const DebayeringMethod& method) noexcept; inline DebayeringMethod getDebayeringMethod () const throw (); inline static bool resizingSupported (unsigned input_width, unsigned input_height, unsigned output_width, unsigned output_height); @@ -83,7 +83,7 @@ namespace openni_wrapper }; void - ImageBayerGRBG::setDebayeringMethod (const ImageBayerGRBG::DebayeringMethod& method) throw () + ImageBayerGRBG::setDebayeringMethod (const ImageBayerGRBG::DebayeringMethod& method) noexcept { debayering_method_ = method; } diff --git a/io/include/pcl/io/openni_camera/openni_image_rgb24.h b/io/include/pcl/io/openni_camera/openni_image_rgb24.h index 4f096037..0ee267ad 100644 --- a/io/include/pcl/io/openni_camera/openni_image_rgb24.h +++ b/io/include/pcl/io/openni_camera/openni_image_rgb24.h @@ -56,8 +56,8 @@ namespace openni_wrapper { public: - ImageRGB24 (pcl::shared_ptr image_meta_data) throw (); - ~ImageRGB24 () throw (); + ImageRGB24 (pcl::shared_ptr image_meta_data) noexcept; + ~ImageRGB24 () noexcept; inline Encoding getEncoding () const override diff --git a/io/include/pcl/io/openni_camera/openni_image_yuv_422.h b/io/include/pcl/io/openni_camera/openni_image_yuv_422.h index 3a0f0173..f7d5208a 100644 --- a/io/include/pcl/io/openni_camera/openni_image_yuv_422.h +++ b/io/include/pcl/io/openni_camera/openni_image_yuv_422.h @@ -55,8 +55,8 @@ namespace openni_wrapper class PCL_EXPORTS ImageYUV422 : public Image { public: - ImageYUV422 (pcl::shared_ptr image_meta_data) throw (); - ~ImageYUV422 () throw (); + ImageYUV422 (pcl::shared_ptr image_meta_data) noexcept; + ~ImageYUV422 () noexcept; inline Encoding getEncoding () const override diff --git a/io/include/pcl/io/openni_camera/openni_ir_image.h b/io/include/pcl/io/openni_camera/openni_ir_image.h index 6a82e04a..fac253d5 100644 --- a/io/include/pcl/io/openni_camera/openni_ir_image.h +++ b/io/include/pcl/io/openni_camera/openni_ir_image.h @@ -55,8 +55,8 @@ public: using Ptr = pcl::shared_ptr; using ConstPtr = pcl::shared_ptr; - inline IRImage (pcl::shared_ptr ir_meta_data) throw (); - inline virtual ~IRImage () throw (); + inline IRImage (pcl::shared_ptr ir_meta_data) noexcept; + inline virtual ~IRImage () noexcept; void fillRaw (unsigned width, unsigned height, unsigned short* ir_buffer, unsigned line_step = 0) const; @@ -70,12 +70,12 @@ protected: pcl::shared_ptr ir_md_; }; -IRImage::IRImage (pcl::shared_ptr ir_meta_data) throw () +IRImage::IRImage (pcl::shared_ptr ir_meta_data) noexcept : ir_md_ (std::move(ir_meta_data)) { } -IRImage::~IRImage () throw () +IRImage::~IRImage () noexcept { } diff --git a/io/include/pcl/io/openni_grabber.h b/io/include/pcl/io/openni_grabber.h index eff84ad0..bc16916b 100644 --- a/io/include/pcl/io/openni_grabber.h +++ b/io/include/pcl/io/openni_grabber.h @@ -109,7 +109,7 @@ namespace pcl const Mode& image_mode = OpenNI_Default_Mode); /** \brief virtual Destructor inherited from the Grabber interface. It never throws. */ - ~OpenNIGrabber () throw (); + ~OpenNIGrabber () noexcept; /** \brief Start the data acquisition. */ void diff --git a/io/include/pcl/io/pcd_grabber.h b/io/include/pcl/io/pcd_grabber.h index e6a3ebed..de546dac 100644 --- a/io/include/pcl/io/pcd_grabber.h +++ b/io/include/pcl/io/pcd_grabber.h @@ -96,7 +96,7 @@ namespace pcl } /** \brief Virtual destructor. */ - ~PCDGrabberBase () throw (); + ~PCDGrabberBase () noexcept; /** \brief Starts playing the list of PCD files if frames_per_second is > 0. Otherwise it works as a trigger: publishes only the next PCD file in the list. */ void @@ -164,7 +164,7 @@ namespace pcl PCDGrabber (const std::vector& pcd_files, float frames_per_second = 0, bool repeat = false); /** \brief Virtual destructor. */ - ~PCDGrabber () throw () + ~PCDGrabber () noexcept { stop (); } diff --git a/io/include/pcl/io/ply/byte_order.h b/io/include/pcl/io/ply/byte_order.h index 8d65a763..46325479 100644 --- a/io/include/pcl/io/ply/byte_order.h +++ b/io/include/pcl/io/ply/byte_order.h @@ -41,6 +41,9 @@ #include +#include +#include + namespace pcl { namespace io diff --git a/io/include/pcl/io/real_sense_grabber.h b/io/include/pcl/io/real_sense_grabber.h index ee552faf..36ac096f 100644 --- a/io/include/pcl/io/real_sense_grabber.h +++ b/io/include/pcl/io/real_sense_grabber.h @@ -43,8 +43,12 @@ #include #include +#include #include +#include +#include #include +#include namespace pcl { @@ -140,7 +144,7 @@ namespace pcl RealSenseGrabber (const std::string& device_id = "", const Mode& mode = Mode (), bool strict = false); virtual - ~RealSenseGrabber () throw (); + ~RealSenseGrabber () noexcept; virtual void start (); @@ -269,7 +273,5 @@ namespace pcl /// Depth buffer to perform temporal filtering of the depth images std::shared_ptr > depth_buffer_; - }; - } diff --git a/io/include/pcl/io/robot_eye_grabber.h b/io/include/pcl/io/robot_eye_grabber.h index 2be6f784..92487f80 100644 --- a/io/include/pcl/io/robot_eye_grabber.h +++ b/io/include/pcl/io/robot_eye_grabber.h @@ -72,7 +72,7 @@ namespace pcl RobotEyeGrabber (const boost::asio::ip::address& ipAddress, unsigned short port=443); /** \brief virtual Destructor inherited from the Grabber interface. It never throws. */ - ~RobotEyeGrabber () throw (); + ~RobotEyeGrabber () noexcept; /** \brief Starts the RobotEye grabber. * The grabber runs on a separate thread, this call will return without blocking. */ diff --git a/io/include/pcl/io/vlp_grabber.h b/io/include/pcl/io/vlp_grabber.h index 1d9863f1..da863b54 100644 --- a/io/include/pcl/io/vlp_grabber.h +++ b/io/include/pcl/io/vlp_grabber.h @@ -71,7 +71,7 @@ namespace pcl /** \brief virtual Destructor inherited from the Grabber interface. It never throws. */ - ~VLPGrabber () throw (); + ~VLPGrabber () noexcept; /** \brief Obtains the name of this I/O Grabber * \return The name of the grabber diff --git a/io/src/davidsdk_grabber.cpp b/io/src/davidsdk_grabber.cpp index 2f23ca36..d13fb466 100644 --- a/io/src/davidsdk_grabber.cpp +++ b/io/src/davidsdk_grabber.cpp @@ -68,7 +68,7 @@ pcl::DavidSDKGrabber::DavidSDKGrabber () : } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -pcl::DavidSDKGrabber::~DavidSDKGrabber () throw () +pcl::DavidSDKGrabber::~DavidSDKGrabber () noexcept { try { diff --git a/io/src/depth_sense/depth_sense_grabber_impl.cpp b/io/src/depth_sense/depth_sense_grabber_impl.cpp index 77783b90..6cb6b48b 100644 --- a/io/src/depth_sense/depth_sense_grabber_impl.cpp +++ b/io/src/depth_sense/depth_sense_grabber_impl.cpp @@ -59,7 +59,7 @@ pcl::io::depth_sense::DepthSenseGrabberImpl::DepthSenseGrabberImpl (DepthSenseGr point_cloud_rgba_signal_ = p_->createSignal (); } -pcl::io::depth_sense::DepthSenseGrabberImpl::~DepthSenseGrabberImpl () throw () +pcl::io::depth_sense::DepthSenseGrabberImpl::~DepthSenseGrabberImpl () noexcept { stop (); diff --git a/io/src/depth_sense_grabber.cpp b/io/src/depth_sense_grabber.cpp index d65c045f..97dd009d 100644 --- a/io/src/depth_sense_grabber.cpp +++ b/io/src/depth_sense_grabber.cpp @@ -44,7 +44,7 @@ pcl::DepthSenseGrabber::DepthSenseGrabber (const std::string& device_id) { } -pcl::DepthSenseGrabber::~DepthSenseGrabber () throw () +pcl::DepthSenseGrabber::~DepthSenseGrabber () noexcept { delete p_; } diff --git a/io/src/dinast_grabber.cpp b/io/src/dinast_grabber.cpp index 3f0afca3..5bd40607 100644 --- a/io/src/dinast_grabber.cpp +++ b/io/src/dinast_grabber.cpp @@ -62,7 +62,7 @@ pcl::DinastGrabber::DinastGrabber (const int device_position) } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -pcl::DinastGrabber::~DinastGrabber () throw () +pcl::DinastGrabber::~DinastGrabber () noexcept { try { diff --git a/io/src/ensenso_grabber.cpp b/io/src/ensenso_grabber.cpp index 09569317..6ba3f6b7 100644 --- a/io/src/ensenso_grabber.cpp +++ b/io/src/ensenso_grabber.cpp @@ -85,7 +85,7 @@ pcl::EnsensoGrabber::EnsensoGrabber () : } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -pcl::EnsensoGrabber::~EnsensoGrabber () throw () +pcl::EnsensoGrabber::~EnsensoGrabber () noexcept { try { diff --git a/io/src/hdl_grabber.cpp b/io/src/hdl_grabber.cpp index bb16a900..2cd87b59 100644 --- a/io/src/hdl_grabber.cpp +++ b/io/src/hdl_grabber.cpp @@ -116,7 +116,7 @@ pcl::HDLGrabber::HDLGrabber (const boost::asio::ip::address& ipAddress, } ///////////////////////////////////////////////////////////////////////////// -pcl::HDLGrabber::~HDLGrabber () throw () +pcl::HDLGrabber::~HDLGrabber () noexcept { stop (); diff --git a/io/src/ifs_io.cpp b/io/src/ifs_io.cpp index 87cb55f8..8b07c4a3 100644 --- a/io/src/ifs_io.cpp +++ b/io/src/ifs_io.cpp @@ -60,7 +60,6 @@ pcl::IFSReader::readHeader (const std::string &file_name, pcl::PCLPointCloud2 &c std::uint32_t nr_points = 0; std::ifstream fs; - std::string line; if (file_name.empty() || !boost::filesystem::exists (file_name)) { diff --git a/io/src/image_grabber.cpp b/io/src/image_grabber.cpp index f0be2570..680e2966 100644 --- a/io/src/image_grabber.cpp +++ b/io/src/image_grabber.cpp @@ -848,7 +848,7 @@ pcl::ImageGrabberBase::ImageGrabberBase (const std::vector& depth_i } /////////////////////////////////////////////////////////////////////////////////////////// -pcl::ImageGrabberBase::~ImageGrabberBase () throw () +pcl::ImageGrabberBase::~ImageGrabberBase () noexcept { stop (); delete impl_; diff --git a/io/src/image_rgb24.cpp b/io/src/image_rgb24.cpp index 55c188ee..f0cd5eca 100644 --- a/io/src/image_rgb24.cpp +++ b/io/src/image_rgb24.cpp @@ -55,7 +55,7 @@ pcl::io::ImageRGB24::ImageRGB24 (FrameWrapper::Ptr image_metadata, Timestamp tim {} -pcl::io::ImageRGB24::~ImageRGB24 () throw () +pcl::io::ImageRGB24::~ImageRGB24 () noexcept {} bool diff --git a/io/src/image_yuv422.cpp b/io/src/image_yuv422.cpp index 1ad16dfb..24b43330 100644 --- a/io/src/image_yuv422.cpp +++ b/io/src/image_yuv422.cpp @@ -57,7 +57,7 @@ pcl::io::ImageYUV422::ImageYUV422 (FrameWrapper::Ptr image_metadata, Timestamp t {} -pcl::io::ImageYUV422::~ImageYUV422 () throw () +pcl::io::ImageYUV422::~ImageYUV422 () noexcept {} bool diff --git a/io/src/io_exception.cpp b/io/src/io_exception.cpp index 614ef1b6..7b3f57ce 100644 --- a/io/src/io_exception.cpp +++ b/io/src/io_exception.cpp @@ -47,7 +47,7 @@ pcl::io::IOException::IOException (const std::string& function_name, const std:: message_long_ = sstream.str (); } -pcl::io::IOException::~IOException () throw () +pcl::io::IOException::~IOException () noexcept { } diff --git a/io/src/oni_grabber.cpp b/io/src/oni_grabber.cpp index 77bfd760..42dd2ae6 100644 --- a/io/src/oni_grabber.cpp +++ b/io/src/oni_grabber.cpp @@ -140,7 +140,7 @@ ONIGrabber::ONIGrabber (const std::string& file_name, bool repeat, bool stream) } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -ONIGrabber::~ONIGrabber() throw () +ONIGrabber::~ONIGrabber() noexcept { try { diff --git a/io/src/openni2_grabber.cpp b/io/src/openni2_grabber.cpp index 19a0f439..667bb624 100644 --- a/io/src/openni2_grabber.cpp +++ b/io/src/openni2_grabber.cpp @@ -139,7 +139,7 @@ pcl::io::OpenNI2Grabber::OpenNI2Grabber (const std::string& device_id, const Mod } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -pcl::io::OpenNI2Grabber::~OpenNI2Grabber () throw () +pcl::io::OpenNI2Grabber::~OpenNI2Grabber () noexcept { try { diff --git a/io/src/openni_camera/openni_device.cpp b/io/src/openni_camera/openni_device.cpp index 36679b35..a0a816e8 100644 --- a/io/src/openni_camera/openni_device.cpp +++ b/io/src/openni_camera/openni_device.cpp @@ -274,7 +274,7 @@ openni_wrapper::OpenNIDevice::OpenNIDevice (xn::Context& context) } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -openni_wrapper::OpenNIDevice::~OpenNIDevice () throw () +openni_wrapper::OpenNIDevice::~OpenNIDevice () noexcept { // stop streams if (image_generator_.IsValid() && image_generator_.IsGenerating ()) @@ -910,7 +910,7 @@ openni_wrapper::OpenNIDevice::IRDataThreadFunction () ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void __stdcall -openni_wrapper::OpenNIDevice::NewDepthDataAvailable (xn::ProductionNode&, void* cookie) throw () +openni_wrapper::OpenNIDevice::NewDepthDataAvailable (xn::ProductionNode&, void* cookie) noexcept { OpenNIDevice* device = reinterpret_cast(cookie); device->depth_condition_.notify_all (); @@ -918,7 +918,7 @@ openni_wrapper::OpenNIDevice::NewDepthDataAvailable (xn::ProductionNode&, void* ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void __stdcall -openni_wrapper::OpenNIDevice::NewImageDataAvailable (xn::ProductionNode&, void* cookie) throw () +openni_wrapper::OpenNIDevice::NewImageDataAvailable (xn::ProductionNode&, void* cookie) noexcept { OpenNIDevice* device = reinterpret_cast(cookie); device->image_condition_.notify_all (); @@ -926,7 +926,7 @@ openni_wrapper::OpenNIDevice::NewImageDataAvailable (xn::ProductionNode&, void* ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void __stdcall -openni_wrapper::OpenNIDevice::NewIRDataAvailable (xn::ProductionNode&, void* cookie) throw () +openni_wrapper::OpenNIDevice::NewIRDataAvailable (xn::ProductionNode&, void* cookie) noexcept { OpenNIDevice* device = reinterpret_cast(cookie); device->ir_condition_.notify_all (); @@ -934,7 +934,7 @@ openni_wrapper::OpenNIDevice::NewIRDataAvailable (xn::ProductionNode&, void* coo ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// openni_wrapper::OpenNIDevice::CallbackHandle -openni_wrapper::OpenNIDevice::registerImageCallback (const ImageCallbackFunction& callback, void* custom_data) throw () +openni_wrapper::OpenNIDevice::registerImageCallback (const ImageCallbackFunction& callback, void* custom_data) noexcept { image_callback_[image_callback_handle_counter_] = [=] (const Image::Ptr& img) { callback (img, custom_data); }; return (image_callback_handle_counter_++); @@ -942,14 +942,14 @@ openni_wrapper::OpenNIDevice::registerImageCallback (const ImageCallbackFunction ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool -openni_wrapper::OpenNIDevice::unregisterImageCallback (const OpenNIDevice::CallbackHandle& callbackHandle) throw () +openni_wrapper::OpenNIDevice::unregisterImageCallback (const OpenNIDevice::CallbackHandle& callbackHandle) noexcept { return (image_callback_.erase (callbackHandle) != 0); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// openni_wrapper::OpenNIDevice::CallbackHandle -openni_wrapper::OpenNIDevice::registerDepthCallback (const DepthImageCallbackFunction& callback, void* custom_data) throw () +openni_wrapper::OpenNIDevice::registerDepthCallback (const DepthImageCallbackFunction& callback, void* custom_data) noexcept { depth_callback_[depth_callback_handle_counter_] = [=] (const DepthImage::Ptr& img) { callback (img, custom_data); }; return (depth_callback_handle_counter_++); @@ -957,14 +957,14 @@ openni_wrapper::OpenNIDevice::registerDepthCallback (const DepthImageCallbackFun ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool -openni_wrapper::OpenNIDevice::unregisterDepthCallback (const OpenNIDevice::CallbackHandle& callbackHandle) throw () +openni_wrapper::OpenNIDevice::unregisterDepthCallback (const OpenNIDevice::CallbackHandle& callbackHandle) noexcept { return (depth_callback_.erase (callbackHandle) != 0); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// openni_wrapper::OpenNIDevice::CallbackHandle -openni_wrapper::OpenNIDevice::registerIRCallback (const IRImageCallbackFunction& callback, void* custom_data) throw () +openni_wrapper::OpenNIDevice::registerIRCallback (const IRImageCallbackFunction& callback, void* custom_data) noexcept { ir_callback_[ir_callback_handle_counter_] = [=] (const IRImage::Ptr& img) { callback (img, custom_data); }; return (ir_callback_handle_counter_++); @@ -972,7 +972,7 @@ openni_wrapper::OpenNIDevice::registerIRCallback (const IRImageCallbackFunction& ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool -openni_wrapper::OpenNIDevice::unregisterIRCallback (const OpenNIDevice::CallbackHandle& callbackHandle) throw () +openni_wrapper::OpenNIDevice::unregisterIRCallback (const OpenNIDevice::CallbackHandle& callbackHandle) noexcept { return (ir_callback_.erase (callbackHandle) != 0); } diff --git a/io/src/openni_camera/openni_device_kinect.cpp b/io/src/openni_camera/openni_device_kinect.cpp index 7719acdc..73dbfd08 100644 --- a/io/src/openni_camera/openni_device_kinect.cpp +++ b/io/src/openni_camera/openni_device_kinect.cpp @@ -92,7 +92,7 @@ openni_wrapper::DeviceKinect::DeviceKinect (xn::Context& context, const xn::Node } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -openni_wrapper::DeviceKinect::~DeviceKinect () throw () +openni_wrapper::DeviceKinect::~DeviceKinect () noexcept { depth_mutex_.lock (); depth_generator_.UnregisterFromNewDataAvailable (depth_callback_handle_); @@ -112,7 +112,7 @@ openni_wrapper::DeviceKinect::isImageResizeSupported (unsigned input_width, unsi ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void -openni_wrapper::DeviceKinect::enumAvailableModes () throw () +openni_wrapper::DeviceKinect::enumAvailableModes () noexcept { XnMapOutputMode output_mode; available_image_modes_.clear(); diff --git a/io/src/openni_camera/openni_device_oni.cpp b/io/src/openni_camera/openni_device_oni.cpp index 3479ef52..170518a2 100644 --- a/io/src/openni_camera/openni_device_oni.cpp +++ b/io/src/openni_camera/openni_device_oni.cpp @@ -102,7 +102,7 @@ openni_wrapper::DeviceONI::DeviceONI ( } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -openni_wrapper::DeviceONI::~DeviceONI() throw () +openni_wrapper::DeviceONI::~DeviceONI() noexcept { if (streaming_) { @@ -221,7 +221,7 @@ openni_wrapper::DeviceONI::PlayerThreadFunction () ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void __stdcall -openni_wrapper::DeviceONI::NewONIDepthDataAvailable (xn::ProductionNode&, void* cookie) throw () +openni_wrapper::DeviceONI::NewONIDepthDataAvailable (xn::ProductionNode&, void* cookie) noexcept { DeviceONI* device = reinterpret_cast(cookie); if (device->depth_stream_running_) @@ -230,7 +230,7 @@ openni_wrapper::DeviceONI::NewONIDepthDataAvailable (xn::ProductionNode&, void* ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void __stdcall -openni_wrapper::DeviceONI::NewONIImageDataAvailable (xn::ProductionNode&, void* cookie) throw () +openni_wrapper::DeviceONI::NewONIImageDataAvailable (xn::ProductionNode&, void* cookie) noexcept { DeviceONI* device = reinterpret_cast (cookie); if (device->image_stream_running_) @@ -239,7 +239,7 @@ openni_wrapper::DeviceONI::NewONIImageDataAvailable (xn::ProductionNode&, void* ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void __stdcall -openni_wrapper::DeviceONI::NewONIIRDataAvailable (xn::ProductionNode&, void* cookie) throw () +openni_wrapper::DeviceONI::NewONIIRDataAvailable (xn::ProductionNode&, void* cookie) noexcept { DeviceONI* device = reinterpret_cast (cookie); if (device->ir_stream_running_) diff --git a/io/src/openni_camera/openni_device_primesense.cpp b/io/src/openni_camera/openni_device_primesense.cpp index b44fdc11..c0ae9eb6 100644 --- a/io/src/openni_camera/openni_device_primesense.cpp +++ b/io/src/openni_camera/openni_device_primesense.cpp @@ -83,7 +83,7 @@ openni_wrapper::DevicePrimesense::DevicePrimesense ( } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -openni_wrapper::DevicePrimesense::~DevicePrimesense () throw () +openni_wrapper::DevicePrimesense::~DevicePrimesense () noexcept { setDepthRegistration (false); setSynchronization (false); @@ -110,7 +110,7 @@ openni_wrapper::DevicePrimesense::isImageResizeSupported ( ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void -openni_wrapper::DevicePrimesense::enumAvailableModes () throw () +openni_wrapper::DevicePrimesense::enumAvailableModes () noexcept { XnMapOutputMode output_mode; available_image_modes_.clear (); diff --git a/io/src/openni_camera/openni_device_xtion.cpp b/io/src/openni_camera/openni_device_xtion.cpp index d4234824..638b6a43 100644 --- a/io/src/openni_camera/openni_device_xtion.cpp +++ b/io/src/openni_camera/openni_device_xtion.cpp @@ -65,7 +65,7 @@ openni_wrapper::DeviceXtionPro::DeviceXtionPro (xn::Context& context, const xn:: } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -openni_wrapper::DeviceXtionPro::~DeviceXtionPro () throw () +openni_wrapper::DeviceXtionPro::~DeviceXtionPro () noexcept { depth_mutex_.lock (); depth_generator_.UnregisterFromNewDataAvailable (depth_callback_handle_); @@ -81,7 +81,7 @@ openni_wrapper::DeviceXtionPro::isImageResizeSupported (unsigned, unsigned, unsi ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void -openni_wrapper::DeviceXtionPro::enumAvailableModes () throw () +openni_wrapper::DeviceXtionPro::enumAvailableModes () noexcept { XnMapOutputMode output_mode; available_image_modes_.clear(); diff --git a/io/src/openni_camera/openni_driver.cpp b/io/src/openni_camera/openni_driver.cpp index 3a54e913..a41c4d4e 100644 --- a/io/src/openni_camera/openni_driver.cpp +++ b/io/src/openni_camera/openni_driver.cpp @@ -192,7 +192,7 @@ openni_wrapper::OpenNIDriver::updateDeviceList () getDeviceType(device.device_node.GetCreationInfo (), vendor_id, product_id ); -#if _WIN32 +#ifdef _WIN32 if (vendor_id == 0x45e) { strcpy (const_cast (device_context_[device].device_node.GetDescription ().strVendor), "Microsoft"); @@ -218,7 +218,7 @@ openni_wrapper::OpenNIDriver::stopAll () THROW_OPENNI_EXCEPTION ("stopping all streams failed. Reason: %s", xnGetStatusString (status)); } -openni_wrapper::OpenNIDriver::~OpenNIDriver () throw () +openni_wrapper::OpenNIDriver::~OpenNIDriver () noexcept { // no exception during destuctor try @@ -331,7 +331,7 @@ openni_wrapper::OpenNIDriver::getDeviceByAddress (unsigned char bus, unsigned ch ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void -openni_wrapper::OpenNIDriver::getDeviceInfos () throw () +openni_wrapper::OpenNIDriver::getDeviceInfos () noexcept { libusb_context *context = nullptr; int result; @@ -428,7 +428,7 @@ openni_wrapper::OpenNIDriver::getSerialNumber (unsigned index) const throw () void openni_wrapper::OpenNIDriver::getDeviceType (const std::string& connectionString, unsigned short& vendorId, unsigned short& productId) { -#if _WIN32 +#ifdef _WIN32 // expected format: "\\?\usb#vid_[ID]&pid_[ID]#[SERIAL]#{GUID}" using tokenizer = boost::tokenizer >; boost::char_separator separators("#&_"); diff --git a/io/src/openni_camera/openni_exception.cpp b/io/src/openni_camera/openni_exception.cpp index 5beed117..540a7100 100644 --- a/io/src/openni_camera/openni_exception.cpp +++ b/io/src/openni_camera/openni_exception.cpp @@ -43,7 +43,7 @@ namespace openni_wrapper { -OpenNIException::OpenNIException (const std::string& function_name, const std::string& file_name, unsigned line_number, const std::string& message) throw () +OpenNIException::OpenNIException (const std::string& function_name, const std::string& file_name, unsigned line_number, const std::string& message) noexcept : function_name_ (function_name) , file_name_ (file_name) , line_number_ (line_number) @@ -54,11 +54,11 @@ OpenNIException::OpenNIException (const std::string& function_name, const std::s message_long_ = sstream.str(); } -OpenNIException::~OpenNIException () throw () +OpenNIException::~OpenNIException () noexcept { } -OpenNIException& OpenNIException::operator = (const OpenNIException& exception) throw () +OpenNIException& OpenNIException::operator = (const OpenNIException& exception) noexcept { message_ = exception.message_; return *this; diff --git a/io/src/openni_camera/openni_image_bayer_grbg.cpp b/io/src/openni_camera/openni_image_bayer_grbg.cpp index a648f030..1ac27ccb 100644 --- a/io/src/openni_camera/openni_image_bayer_grbg.cpp +++ b/io/src/openni_camera/openni_image_bayer_grbg.cpp @@ -51,14 +51,14 @@ using namespace std; ////////////////////////////////////////////////////////////////////////////// -openni_wrapper::ImageBayerGRBG::ImageBayerGRBG (pcl::shared_ptr image_meta_data, DebayeringMethod method) throw () +openni_wrapper::ImageBayerGRBG::ImageBayerGRBG (pcl::shared_ptr image_meta_data, DebayeringMethod method) noexcept : Image (std::move(image_meta_data)) , debayering_method_ (method) { } ////////////////////////////////////////////////////////////////////////////// -openni_wrapper::ImageBayerGRBG::~ImageBayerGRBG () throw () +openni_wrapper::ImageBayerGRBG::~ImageBayerGRBG () noexcept { } diff --git a/io/src/openni_camera/openni_image_rgb24.cpp b/io/src/openni_camera/openni_image_rgb24.cpp index 30a99e65..9d51d520 100644 --- a/io/src/openni_camera/openni_image_rgb24.cpp +++ b/io/src/openni_camera/openni_image_rgb24.cpp @@ -6,12 +6,12 @@ namespace openni_wrapper { -ImageRGB24::ImageRGB24 (pcl::shared_ptr image_meta_data) throw () +ImageRGB24::ImageRGB24 (pcl::shared_ptr image_meta_data) noexcept : Image (std::move(image_meta_data)) { } -ImageRGB24::~ImageRGB24 () throw () +ImageRGB24::~ImageRGB24 () noexcept { } diff --git a/io/src/openni_camera/openni_image_yuv_422.cpp b/io/src/openni_camera/openni_image_yuv_422.cpp index 5a8525d0..fe2e7897 100644 --- a/io/src/openni_camera/openni_image_yuv_422.cpp +++ b/io/src/openni_camera/openni_image_yuv_422.cpp @@ -48,12 +48,12 @@ using namespace std; namespace openni_wrapper { -ImageYUV422::ImageYUV422 (pcl::shared_ptr image_meta_data) throw () +ImageYUV422::ImageYUV422 (pcl::shared_ptr image_meta_data) noexcept : Image (std::move(image_meta_data)) { } -ImageYUV422::~ImageYUV422 () throw () +ImageYUV422::~ImageYUV422 () noexcept { } diff --git a/io/src/openni_grabber.cpp b/io/src/openni_grabber.cpp index 0d8642f0..39d033a0 100644 --- a/io/src/openni_grabber.cpp +++ b/io/src/openni_grabber.cpp @@ -121,7 +121,7 @@ pcl::OpenNIGrabber::OpenNIGrabber (const std::string& device_id, const Mode& dep } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -pcl::OpenNIGrabber::~OpenNIGrabber () throw () +pcl::OpenNIGrabber::~OpenNIGrabber () noexcept { try { diff --git a/io/src/pcd_grabber.cpp b/io/src/pcd_grabber.cpp index d9f69f22..28a1f0c4 100644 --- a/io/src/pcd_grabber.cpp +++ b/io/src/pcd_grabber.cpp @@ -204,7 +204,7 @@ bool pcl::PCDGrabberBase::PCDGrabberImpl::readTARHeader () { // Read in the header -#if WIN32 +#ifdef _WIN32 int result = static_cast (_read (tar_fd_, reinterpret_cast (&tar_header_), 512)); #else int result = static_cast (::read (tar_fd_, reinterpret_cast (&tar_header_), 512)); @@ -366,7 +366,7 @@ pcl::PCDGrabberBase::PCDGrabberBase (const std::vector& pcd_files, } /////////////////////////////////////////////////////////////////////////////////////////// -pcl::PCDGrabberBase::~PCDGrabberBase () throw () +pcl::PCDGrabberBase::~PCDGrabberBase () noexcept { delete impl_; } @@ -460,4 +460,3 @@ pcl::PCDGrabberBase::numFrames () const return (impl_->numFrames ()); } - diff --git a/io/src/ply_io.cpp b/io/src/ply_io.cpp index 23157ef7..4692f6af 100644 --- a/io/src/ply_io.cpp +++ b/io/src/ply_io.cpp @@ -1769,6 +1769,6 @@ pcl::io::savePLYFileBinary (const std::string &file_name, const pcl::PolygonMesh } // Close file - fs.close (); + fpout.close (); return (0); } diff --git a/io/src/real_sense_grabber.cpp b/io/src/real_sense_grabber.cpp index a0f87856..ecdf8951 100644 --- a/io/src/real_sense_grabber.cpp +++ b/io/src/real_sense_grabber.cpp @@ -130,7 +130,7 @@ pcl::RealSenseGrabber::RealSenseGrabber (const std::string& device_id, const Mod point_cloud_rgba_signal_ = createSignal (); } -pcl::RealSenseGrabber::~RealSenseGrabber () throw () +pcl::RealSenseGrabber::~RealSenseGrabber () noexcept { stop (); diff --git a/io/src/robot_eye_grabber.cpp b/io/src/robot_eye_grabber.cpp index 30b76347..7f7e71cf 100644 --- a/io/src/robot_eye_grabber.cpp +++ b/io/src/robot_eye_grabber.cpp @@ -61,7 +61,7 @@ pcl::RobotEyeGrabber::RobotEyeGrabber (const boost::asio::ip::address& ipAddress } ///////////////////////////////////////////////////////////////////////////// -pcl::RobotEyeGrabber::~RobotEyeGrabber () throw () +pcl::RobotEyeGrabber::~RobotEyeGrabber () noexcept { stop (); disconnect_all_slots (); diff --git a/io/src/vlp_grabber.cpp b/io/src/vlp_grabber.cpp index 8140bb15..ea15158d 100644 --- a/io/src/vlp_grabber.cpp +++ b/io/src/vlp_grabber.cpp @@ -58,7 +58,7 @@ pcl::VLPGrabber::VLPGrabber (const boost::asio::ip::address& ipAddress, } ///////////////////////////////////////////////////////////////////////////// -pcl::VLPGrabber::~VLPGrabber () throw () +pcl::VLPGrabber::~VLPGrabber () noexcept { } diff --git a/io/tools/CMakeLists.txt b/io/tools/CMakeLists.txt index b0452bf4..651bb00d 100644 --- a/io/tools/CMakeLists.txt +++ b/io/tools/CMakeLists.txt @@ -1,3 +1,5 @@ +set(SUBSYS_NAME tools) + if(WITH_OPENNI) PCL_ADD_EXECUTABLE(pcl_openni_grabber_example COMPONENT ${SUBSYS_NAME} SOURCES openni_grabber_example.cpp) target_link_libraries(pcl_openni_grabber_example pcl_common pcl_io) diff --git a/io/tools/openni_grabber_depth_example.cpp b/io/tools/openni_grabber_depth_example.cpp index 3dc2b3f9..61509e5e 100644 --- a/io/tools/openni_grabber_depth_example.cpp +++ b/io/tools/openni_grabber_depth_example.cpp @@ -94,15 +94,11 @@ class SimpleOpenNIProcessor do { key = static_cast (getchar ()); - switch (key) + if (key == ' ') { - case ' ': - if (interface.isRunning ()) - interface.stop (); - else - interface.start (); + interface.toggle (); } - } while (key != 27 && key != 'q' && key != 'Q'); + } while ((key != 27) && (key != 'q') && (key != 'Q')); // stop the grabber interface.stop (); diff --git a/keypoints/include/pcl/keypoints/impl/sift_keypoint.hpp b/keypoints/include/pcl/keypoints/impl/sift_keypoint.hpp index 8e471c6e..7b06de57 100644 --- a/keypoints/include/pcl/keypoints/impl/sift_keypoint.hpp +++ b/keypoints/include/pcl/keypoints/impl/sift_keypoint.hpp @@ -226,7 +226,6 @@ void pcl::SIFTKeypoint::computeScaleSpace ( // For each scale, compute the Gaussian "filter response" at the current point float filter_response = 0.0f; - float previous_filter_response; for (std::size_t i_scale = 0; i_scale < scales.size (); ++i_scale) { float sigma_sqr = powf (scales[i_scale], 2.0f); @@ -245,7 +244,7 @@ void pcl::SIFTKeypoint::computeScaleSpace ( } else break; // i.e. if dist > 3 standard deviations, then terminate early } - previous_filter_response = filter_response; + float previous_filter_response = filter_response; filter_response = numerator / denominator; // Compute the difference between adjacent scales diff --git a/ml/include/pcl/ml/impl/kmeans.hpp b/ml/include/pcl/ml/impl/kmeans.hpp index 2f1cb855..ee02c1ef 100644 --- a/ml/include/pcl/ml/impl/kmeans.hpp +++ b/ml/include/pcl/ml/impl/kmeans.hpp @@ -73,7 +73,6 @@ pcl::Kmeans::cluster(std::vector& clusters) pcl::PointCloud point; std::vector fields; - int user_index = -1; // if no cluster field name is set, check for X Y Z if (strcmp(cluster_field_name_.c_str(), "") == 0) { int x_index = -1; @@ -121,7 +120,7 @@ pcl::Kmeans::cluster(std::vector& clusters) } // if cluster field name is set, check if field name is valid else { - user_index = pcl::getFieldIndex(cluster_field_name_.c_str(), fields); + int user_index = pcl::getFieldIndex(cluster_field_name_.c_str(), fields); if (user_index == -1) { PCL_ERROR("Failed to find match for field '%s'\n", cluster_field_name_.c_str()); diff --git a/octree/include/pcl/octree/boost.h b/octree/include/pcl/octree/boost.h index 8db30618..3510b239 100644 --- a/octree/include/pcl/octree/boost.h +++ b/octree/include/pcl/octree/boost.h @@ -40,7 +40,7 @@ #pragma once #ifdef __GNUC__ -#pragma GCC system_header +#pragma GCC system_header #endif // Marking all Boost headers as system headers to remove warnings diff --git a/octree/include/pcl/octree/impl/octree2buf_base.hpp b/octree/include/pcl/octree/impl/octree2buf_base.hpp index 8abc3b5e..edaea812 100644 --- a/octree/include/pcl/octree/impl/octree2buf_base.hpp +++ b/octree/include/pcl/octree/impl/octree2buf_base.hpp @@ -39,787 +39,799 @@ #ifndef PCL_OCTREE_2BUF_BASE_HPP #define PCL_OCTREE_2BUF_BASE_HPP -namespace pcl +namespace pcl { +namespace octree { +////////////////////////////////////////////////////////////////////////////////////////////// +template +Octree2BufBase::Octree2BufBase() +: leaf_count_(0) +, branch_count_(1) +, root_node_(new BranchNode()) +, depth_mask_(0) +, buffer_selector_(0) +, tree_dirty_flag_(false) +, octree_depth_(0) +, dynamic_depth_enabled_(false) +{} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +Octree2BufBase::~Octree2BufBase() { - namespace octree - { - ////////////////////////////////////////////////////////////////////////////////////////////// - template - Octree2BufBase::Octree2BufBase () : - leaf_count_ (0), - branch_count_ (1), - root_node_ (new BranchNode ()), - depth_mask_ (0), - buffer_selector_ (0), - tree_dirty_flag_ (false), - octree_depth_ (0), - dynamic_depth_enabled_(false) - { - } + // deallocate tree structure + deleteTree(); + delete (root_node_); +} - ////////////////////////////////////////////////////////////////////////////////////////////// - template - Octree2BufBase::~Octree2BufBase () - { - // deallocate tree structure - deleteTree (); - delete (root_node_); - } +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +Octree2BufBase::setMaxVoxelIndex( + unsigned int max_voxel_index_arg) +{ + unsigned int treeDepth; - ////////////////////////////////////////////////////////////////////////////////////////////// - template void - Octree2BufBase::setMaxVoxelIndex (unsigned int max_voxel_index_arg) - { - unsigned int treeDepth; + assert(max_voxel_index_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(std::log2(max_voxel_index_arg))))), + static_cast(0)); - // tree depth == amount of bits of maxVoxels - treeDepth = std::max ((std::min (static_cast (OctreeKey::maxDepth), - static_cast (std::ceil (std::log2 (max_voxel_index_arg))))), - static_cast (0)); + // define depthMask_ by setting a single bit to 1 at bit position == tree depth + depth_mask_ = (1 << (treeDepth - 1)); +} - // define depthMask_ by setting a single bit to 1 at bit position == tree depth - depth_mask_ = (1 << (treeDepth - 1)); - } +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +Octree2BufBase::setTreeDepth(unsigned int depth_arg) +{ + assert(depth_arg > 0); - ////////////////////////////////////////////////////////////////////////////////////////////// - template void - Octree2BufBase::setTreeDepth (unsigned int depth_arg) - { - assert (depth_arg > 0); + // set octree depth + octree_depth_ = depth_arg; - // set octree depth - octree_depth_ = depth_arg; + // define depthMask_ by setting a single bit to 1 at bit position == tree depth + depth_mask_ = (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; +} - // define max. keys - max_key_.x = max_key_.y = max_key_.z = (1 << depth_arg) - 1; - } +////////////////////////////////////////////////////////////////////////////////////////////// +template +LeafContainerT* +Octree2BufBase::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); - ////////////////////////////////////////////////////////////////////////////////////////////// - template LeafContainerT* - Octree2BufBase::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)); - } - - ////////////////////////////////////////////////////////////////////////////////////////////// - template LeafContainerT* - Octree2BufBase::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 - Octree2BufBase::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 (findLeaf(key)); +} - ////////////////////////////////////////////////////////////////////////////////////////////// - template void - Octree2BufBase::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); +////////////////////////////////////////////////////////////////////////////////////////////// +template +LeafContainerT* +Octree2BufBase::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); - // free voxel at key - return (this->removeLeaf (key)); - } + // check if key exist in octree + return (createLeaf(key)); +} - ////////////////////////////////////////////////////////////////////////////////////////////// - template void - Octree2BufBase::deleteTree () - { - if (root_node_) - { - // reset octree - deleteBranch (*root_node_); - leaf_count_ = 0; - branch_count_ = 1; - - tree_dirty_flag_ = false; - depth_mask_ = 0; - octree_depth_ = 0; - } - } +////////////////////////////////////////////////////////////////////////////////////////////// +template +bool +Octree2BufBase::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); - ////////////////////////////////////////////////////////////////////////////////////////////// - template void - Octree2BufBase::switchBuffers () - { - if (tree_dirty_flag_) - { - // make sure that all unused branch nodes from previous buffer are deleted - treeCleanUpRecursive (root_node_); - } + // check if key exist in octree + return existLeaf(key); +} - // switch butter selector - buffer_selector_ = !buffer_selector_; +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +Octree2BufBase::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); - // reset flags - tree_dirty_flag_ = true; - leaf_count_ = 0; - branch_count_ = 1; + // free voxel at key + return (this->removeLeaf(key)); +} - // we can safely remove children references of root node - for (unsigned char child_idx = 0; child_idx < 8; child_idx++) - { - root_node_->setChildPtr(buffer_selector_, child_idx, nullptr); - } - } +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +Octree2BufBase::deleteTree() +{ + if (root_node_) { + // reset octree + deleteBranch(*root_node_); + leaf_count_ = 0; + branch_count_ = 1; + + tree_dirty_flag_ = false; + depth_mask_ = 0; + octree_depth_ = 0; + } +} - ////////////////////////////////////////////////////////////////////////////////////////////// - template void - Octree2BufBase::serializeTree (std::vector& binary_tree_out_arg, - bool do_XOR_encoding_arg) - { - OctreeKey new_key; - - // clear binary vector - binary_tree_out_arg.clear (); - binary_tree_out_arg.reserve (this->branch_count_); - - serializeTreeRecursive (root_node_, new_key, &binary_tree_out_arg, nullptr, do_XOR_encoding_arg, false); - - // serializeTreeRecursive cleans-up unused octree nodes in previous octree - tree_dirty_flag_ = false; - } +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +Octree2BufBase::switchBuffers() +{ + if (tree_dirty_flag_) { + // make sure that all unused branch nodes from previous buffer are deleted + treeCleanUpRecursive(root_node_); + } - ////////////////////////////////////////////////////////////////////////////////////////////// - template void - Octree2BufBase::serializeTree (std::vector& binary_tree_out_arg, - std::vector& leaf_container_vector_arg, - bool do_XOR_encoding_arg) - { - OctreeKey new_key; + // switch butter selector + buffer_selector_ = !buffer_selector_; - // clear output vectors - binary_tree_out_arg.clear (); - leaf_container_vector_arg.clear (); + // reset flags + tree_dirty_flag_ = true; + leaf_count_ = 0; + branch_count_ = 1; - leaf_container_vector_arg.reserve (leaf_count_); - binary_tree_out_arg.reserve (this->branch_count_); + // we can safely remove children references of root node + for (unsigned char child_idx = 0; child_idx < 8; child_idx++) { + root_node_->setChildPtr(buffer_selector_, child_idx, nullptr); + } +} - serializeTreeRecursive (root_node_, new_key, &binary_tree_out_arg, &leaf_container_vector_arg, do_XOR_encoding_arg, false); +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +Octree2BufBase::serializeTree( + std::vector& binary_tree_out_arg, bool do_XOR_encoding_arg) +{ + OctreeKey new_key; - // serializeTreeRecursive cleans-up unused octree nodes in previous octree - tree_dirty_flag_ = false; - } + // clear binary vector + binary_tree_out_arg.clear(); + binary_tree_out_arg.reserve(this->branch_count_); - ////////////////////////////////////////////////////////////////////////////////////////////// - template void - Octree2BufBase::serializeLeafs (std::vector& leaf_container_vector_arg) - { - OctreeKey new_key; + serializeTreeRecursive( + root_node_, new_key, &binary_tree_out_arg, nullptr, do_XOR_encoding_arg, false); - // clear output vector - leaf_container_vector_arg.clear (); + // serializeTreeRecursive cleans-up unused octree nodes in previous octree + tree_dirty_flag_ = false; +} - leaf_container_vector_arg.reserve (leaf_count_); +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +Octree2BufBase::serializeTree( + std::vector& binary_tree_out_arg, + std::vector& leaf_container_vector_arg, + bool do_XOR_encoding_arg) +{ + OctreeKey new_key; - serializeTreeRecursive (root_node_, new_key, nullptr, &leaf_container_vector_arg, false, false); + // clear output vectors + binary_tree_out_arg.clear(); + leaf_container_vector_arg.clear(); - // serializeLeafsRecursive cleans-up unused octree nodes in previous octree - tree_dirty_flag_ = false; - } + leaf_container_vector_arg.reserve(leaf_count_); + binary_tree_out_arg.reserve(this->branch_count_); - ////////////////////////////////////////////////////////////////////////////////////////////// - template void - Octree2BufBase::deserializeTree (std::vector& binary_tree_in_arg, - bool do_XOR_decoding_arg) - { - OctreeKey new_key; + serializeTreeRecursive(root_node_, + new_key, + &binary_tree_out_arg, + &leaf_container_vector_arg, + do_XOR_encoding_arg, + false); - // we will rebuild an octree -> reset leafCount - leaf_count_ = 0; + // serializeTreeRecursive cleans-up unused octree nodes in previous octree + tree_dirty_flag_ = false; +} - // iterator for binary tree structure vector - 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 (); +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +Octree2BufBase::serializeLeafs( + std::vector& leaf_container_vector_arg) +{ + OctreeKey new_key; - deserializeTreeRecursive (root_node_, depth_mask_, new_key, - binary_tree_in_it, binary_tree_in_it_end, nullptr, nullptr, false, - do_XOR_decoding_arg); + // clear output vector + leaf_container_vector_arg.clear(); - // we modified the octree structure -> clean-up/tree-reset might be required - tree_dirty_flag_ = false; - } + leaf_container_vector_arg.reserve(leaf_count_); - ////////////////////////////////////////////////////////////////////////////////////////////// - template void - Octree2BufBase::deserializeTree (std::vector& binary_tree_in_arg, - std::vector& leaf_container_vector_arg, - bool do_XOR_decoding_arg) - { - OctreeKey new_key; + serializeTreeRecursive( + root_node_, new_key, nullptr, &leaf_container_vector_arg, false, false); - // set data iterator to first element - typename std::vector::const_iterator leaf_container_vector_it = leaf_container_vector_arg.begin (); + // serializeLeafsRecursive cleans-up unused octree nodes in previous octree + tree_dirty_flag_ = false; +} - // set data iterator to last element - typename std::vector::const_iterator leaf_container_vector_it_end = leaf_container_vector_arg.end (); +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +Octree2BufBase::deserializeTree( + std::vector& binary_tree_in_arg, bool do_XOR_decoding_arg) +{ + OctreeKey new_key; + + // we will rebuild an octree -> reset leafCount + leaf_count_ = 0; + + // iterator for binary tree structure vector + 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(root_node_, + depth_mask_, + new_key, + binary_tree_in_it, + binary_tree_in_it_end, + nullptr, + nullptr, + false, + do_XOR_decoding_arg); + + // we modified the octree structure -> clean-up/tree-reset might be required + tree_dirty_flag_ = false; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +Octree2BufBase::deserializeTree( + std::vector& binary_tree_in_arg, + std::vector& leaf_container_vector_arg, + bool do_XOR_decoding_arg) +{ + OctreeKey new_key; + + // set data iterator to first element + 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 leaf_container_vector_it_end = + leaf_container_vector_arg.end(); + + // we will rebuild an octree -> reset leafCount + leaf_count_ = 0; + + // iterator for binary tree structure vector + 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(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 + tree_dirty_flag_ = false; +} - // we will rebuild an octree -> reset leafCount - leaf_count_ = 0; +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +Octree2BufBase::serializeNewLeafs( + std::vector& leaf_container_vector_arg) +{ + OctreeKey new_key; - // iterator for binary tree structure vector - 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 (); + // clear output vector + leaf_container_vector_arg.clear(); + leaf_container_vector_arg.reserve(leaf_count_); - 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); + serializeTreeRecursive( + root_node_, new_key, nullptr, &leaf_container_vector_arg, false, true); + // serializeLeafsRecursive cleans-up unused octree nodes in previous octree buffer + tree_dirty_flag_ = false; +} - // we modified the octree structure -> clean-up/tree-reset might be required - tree_dirty_flag_ = false; +////////////////////////////////////////////////////////////////////////////////////////////// +template +unsigned int +Octree2BufBase::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) +{ + // branch reset -> this branch has been taken from previous buffer + if (branch_reset_arg) { + // we can safely remove children references + for (unsigned char child_idx = 0; child_idx < 8; child_idx++) { + branch_arg->setChildPtr(buffer_selector_, child_idx, nullptr); } + } + // find branch child from key + unsigned char child_idx = key_arg.getChildIdxWithDepthMask(depth_mask_arg); - ////////////////////////////////////////////////////////////////////////////////////////////// - template void - Octree2BufBase::serializeNewLeafs (std::vector& leaf_container_vector_arg) - { - OctreeKey new_key; + if (depth_mask_arg > 1) { + // we have not reached maximum tree depth + BranchNode* child_branch; + bool doNodeReset; - // clear output vector - leaf_container_vector_arg.clear (); - leaf_container_vector_arg.reserve (leaf_count_); + doNodeReset = false; - serializeTreeRecursive (root_node_, new_key, nullptr, &leaf_container_vector_arg, false, true); + // if required branch does not exist + if (!branch_arg->hasChild(buffer_selector_, child_idx)) { + // check if we find a branch node reference in previous buffer - // serializeLeafsRecursive cleans-up unused octree nodes in previous octree buffer - tree_dirty_flag_ = false; - } + if (branch_arg->hasChild(!buffer_selector_, child_idx)) { + OctreeNode* child_node = branch_arg->getChildPtr(!buffer_selector_, child_idx); - ////////////////////////////////////////////////////////////////////////////////////////////// - template - unsigned int - Octree2BufBase::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) - { - // branch reset -> this branch has been taken from previous buffer - if (branch_reset_arg) - { - // we can safely remove children references - for (unsigned char child_idx = 0; child_idx < 8; child_idx++) - { - branch_arg->setChildPtr(buffer_selector_, child_idx, nullptr); + 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 preceding buffer is a leaf node. + deleteBranchChild(*branch_arg, !buffer_selector_, child_idx); + child_branch = createBranchChild(*branch_arg, child_idx); } - } - // find branch child from key - unsigned char child_idx = key_arg.getChildIdxWithDepthMask (depth_mask_arg); + // take child branch from previous buffer + doNodeReset = true; // reset the branch pointer array of stolen child node + } + else { + // if required branch does not exist -> create it + child_branch = createBranchChild(*branch_arg, child_idx); + } - if (depth_mask_arg > 1) - { - // we have not reached maximum tree depth - BranchNode* child_branch; - bool doNodeReset; + branch_count_++; + } + // required branch node already exists - use it + else + child_branch = static_cast( + branch_arg->getChildPtr(buffer_selector_, child_idx)); + + // recursively proceed with indexed child branch + return createLeafRecursive(key_arg, + depth_mask_arg / 2, + child_branch, + return_leaf_arg, + parent_of_leaf_arg, + doNodeReset); + } - doNodeReset = false; + // branch childs are leaf nodes + LeafNode* child_leaf; + if (!branch_arg->hasChild(buffer_selector_, child_idx)) { + // leaf node at child_idx does not exist - // if required branch does not exist - if (!branch_arg->hasChild(buffer_selector_, child_idx)) - { - // check if we find a branch node reference in previous buffer + // check if we can take copy a reference from previous buffer + if (branch_arg->hasChild(!buffer_selector_, child_idx)) { - if (branch_arg->hasChild(!buffer_selector_, child_idx)) - { - OctreeNode* child_node = branch_arg->getChildPtr(!buffer_selector_,child_idx); + 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 preceding buffer is a leaf node. + deleteBranchChild(*branch_arg, !buffer_selector_, child_idx); + child_leaf = createLeafChild(*branch_arg, child_idx); + } + leaf_count_++; + } + else { + // if required leaf does not exist -> create it + child_leaf = createLeafChild(*branch_arg, child_idx); + leaf_count_++; + } - 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 preceding buffer is a leaf node. - deleteBranchChild (*branch_arg, !buffer_selector_, child_idx); - child_branch = createBranchChild (*branch_arg, child_idx); - } + // return leaf node + return_leaf_arg = child_leaf; + parent_of_leaf_arg = branch_arg; + } + else { + // leaf node already exist + return_leaf_arg = + static_cast(branch_arg->getChildPtr(buffer_selector_, child_idx)); + parent_of_leaf_arg = branch_arg; + } - // take child branch from previous buffer - doNodeReset = true; // reset the branch pointer array of stolen child node + return depth_mask_arg; +} - } - else - { - // if required branch does not exist -> create it - child_branch = createBranchChild (*branch_arg, child_idx); - } +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +Octree2BufBase::findLeafRecursive( + const OctreeKey& key_arg, + unsigned int depth_mask_arg, + BranchNode* branch_arg, + LeafContainerT*& result_arg) const +{ + // return leaf node + unsigned char child_idx; - branch_count_++; - } - // required branch node already exists - use it - else - child_branch = static_cast (branch_arg->getChildPtr(buffer_selector_,child_idx)); - - // recursively proceed with indexed child branch - return createLeafRecursive (key_arg, depth_mask_arg / 2, child_branch, return_leaf_arg, parent_of_leaf_arg, doNodeReset); - } + // find branch child from key + child_idx = key_arg.getChildIdxWithDepthMask(depth_mask_arg); - // branch childs are leaf nodes - LeafNode* child_leaf; - if (!branch_arg->hasChild(buffer_selector_, child_idx)) - { - // leaf node at child_idx does not exist - - // check if we can take copy a reference from previous buffer - if (branch_arg->hasChild(!buffer_selector_, child_idx)) - { - - 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 preceding buffer is a leaf node. - deleteBranchChild (*branch_arg, !buffer_selector_, child_idx); - child_leaf = createLeafChild (*branch_arg, child_idx); - } - leaf_count_++; - } - else - { - // if required leaf does not exist -> create it - child_leaf = createLeafChild (*branch_arg, child_idx); - leaf_count_++; - } - - // return leaf node - return_leaf_arg = child_leaf; - parent_of_leaf_arg = branch_arg; - } - else - { - // leaf node already exist - return_leaf_arg = static_cast (branch_arg->getChildPtr(buffer_selector_,child_idx));; - parent_of_leaf_arg = branch_arg; - } + if (depth_mask_arg > 1) { + // we have not reached maximum tree depth + BranchNode* child_branch; + child_branch = + static_cast(branch_arg->getChildPtr(buffer_selector_, child_idx)); - return depth_mask_arg; + if (child_branch) + // recursively proceed with indexed child branch + findLeafRecursive(key_arg, depth_mask_arg / 2, child_branch, result_arg); + } + else { + // we reached leaf node level + if (branch_arg->hasChild(buffer_selector_, child_idx)) { + // return existing leaf node + LeafNode* leaf_node = + static_cast(branch_arg->getChildPtr(buffer_selector_, child_idx)); + result_arg = leaf_node->getContainerPtr(); } + } +} - ////////////////////////////////////////////////////////////////////////////////////////////// - template void - Octree2BufBase::findLeafRecursive (const OctreeKey& key_arg, - unsigned int depth_mask_arg, - BranchNode* branch_arg, - LeafContainerT*& result_arg) const - { - // return leaf node - unsigned char child_idx; - - // find branch child from key - child_idx = key_arg.getChildIdxWithDepthMask (depth_mask_arg); - - if (depth_mask_arg > 1) - { - // we have not reached maximum tree depth - BranchNode* child_branch; - child_branch = static_cast (branch_arg->getChildPtr(buffer_selector_,child_idx)); - - if (child_branch) - // recursively proceed with indexed child branch - findLeafRecursive (key_arg, depth_mask_arg / 2, child_branch, result_arg); +////////////////////////////////////////////////////////////////////////////////////////////// +template +bool +Octree2BufBase::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 bNoChilds; + + // find branch child from key + child_idx = key_arg.getChildIdxWithDepthMask(depth_mask_arg); + + if (depth_mask_arg > 1) { + // we have not reached maximum tree depth + BranchNode* child_branch; + + // next branch child on our path through the tree + child_branch = + static_cast(branch_arg->getChildPtr(buffer_selector_, child_idx)); + + if (child_branch) { + // recursively explore the indexed child branch + bool 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, buffer_selector_, child_idx); + branch_count_--; } - else - { - // we reached leaf node level - if (branch_arg->hasChild(buffer_selector_, child_idx)) - { - // return existing leaf node - LeafNode* leaf_node = static_cast (branch_arg->getChildPtr(buffer_selector_,child_idx)); - result_arg = leaf_node->getContainerPtr();; - } - } } + } + else { + // our child is a leaf node -> delete it + deleteBranchChild(*branch_arg, buffer_selector_, child_idx); + leaf_count_--; + } - ////////////////////////////////////////////////////////////////////////////////////////////// - template bool - Octree2BufBase::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 bNoChilds; - - // find branch child from key - child_idx = key_arg.getChildIdxWithDepthMask (depth_mask_arg); - - if (depth_mask_arg > 1) - { - // we have not reached maximum tree depth - BranchNode* child_branch; - bool bBranchOccupied; - - // next branch child on our path through the tree - child_branch = static_cast (branch_arg->getChildPtr(buffer_selector_,child_idx)); - - if (child_branch) - { - // recursively explore the indexed child branch - 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, buffer_selector_, child_idx); - branch_count_--; - } - } - } - else - { - // our child is a leaf node -> delete it - deleteBranchChild (*branch_arg, buffer_selector_, child_idx); - leaf_count_--; - } + // check if current branch still owns childs + bNoChilds = false; + for (child_idx = 0; child_idx < 8; child_idx++) { + bNoChilds = branch_arg->hasChild(buffer_selector_, child_idx); + if (bNoChilds) + break; + } - // check if current branch still owns childs - bNoChilds = false; - for (child_idx = 0; child_idx < 8; child_idx++) - { - bNoChilds = branch_arg->hasChild(buffer_selector_, child_idx); - if (bNoChilds) - break; - } + // return true if current branch can be deleted + return (bNoChilds); +} - // return true if current branch can be deleted - return (bNoChilds); +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +Octree2BufBase::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, + bool new_leafs_filter_arg) +{ + // bit pattern + 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) + 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 + node_XOR_bit_pattern = + branch_bit_pattern_curr_buffer ^ branch_bit_pattern_prev_buffer; + + if (binary_tree_out_arg) { + if (do_XOR_encoding_arg) { + // write XOR bit pattern to output vector + binary_tree_out_arg->push_back(node_XOR_bit_pattern); } + else { + // write bit pattern of current buffer to output vector + binary_tree_out_arg->push_back(branch_bit_pattern_curr_buffer); + } + } - ////////////////////////////////////////////////////////////////////////////////////////////// - template void Octree2BufBase< - LeafContainerT, BranchContainerT>::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, - bool new_leafs_filter_arg) - { - // bit pattern - 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) - 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 - node_XOR_bit_pattern = branch_bit_pattern_curr_buffer ^ branch_bit_pattern_prev_buffer; - - if (binary_tree_out_arg) - { - if (do_XOR_encoding_arg) - { - // write XOR bit pattern to output vector - binary_tree_out_arg->push_back (node_XOR_bit_pattern); - } - else - { - // write bit pattern of current buffer to output vector - binary_tree_out_arg->push_back (branch_bit_pattern_curr_buffer); - } + // iterate over all children + for (unsigned char child_idx = 0; child_idx < 8; child_idx++) { + if (branch_arg->hasChild(buffer_selector_, child_idx)) { + // add current branch voxel to key + key_arg.pushBranch(child_idx); + + OctreeNode* child_node = branch_arg->getChildPtr(buffer_selector_, child_idx); + + switch (child_node->getNodeType()) { + case BRANCH_NODE: { + // 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* child_leaf = static_cast(child_node); - // iterate over all children - for (unsigned char child_idx = 0; child_idx < 8; child_idx++) - { - if (branch_arg->hasChild(buffer_selector_, child_idx)) - { - // add current branch voxel to key - key_arg.pushBranch(child_idx); - - OctreeNode *child_node = branch_arg->getChildPtr(buffer_selector_,child_idx); - - switch (child_node->getNodeType ()) - { - case BRANCH_NODE: - { - // 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* child_leaf = static_cast (child_node); - - if (new_leafs_filter_arg) - { - if (!branch_arg->hasChild (!buffer_selector_, child_idx)) - { - if (leaf_container_vector_arg) - leaf_container_vector_arg->push_back(child_leaf->getContainerPtr()); - - serializeTreeCallback (**child_leaf, key_arg); - } - } else - { - - if (leaf_container_vector_arg) - leaf_container_vector_arg->push_back(child_leaf->getContainerPtr()); - - serializeTreeCallback (**child_leaf, key_arg); - } + if (new_leafs_filter_arg) { + if (!branch_arg->hasChild(!buffer_selector_, child_idx)) { + if (leaf_container_vector_arg) + leaf_container_vector_arg->push_back(child_leaf->getContainerPtr()); - break; - } - default: - break; + serializeTreeCallback(**child_leaf, key_arg); } - - // pop current branch voxel from key - key_arg.popBranch(); } - else if (branch_arg->hasChild (!buffer_selector_, child_idx)) - { - // delete branch, free memory - deleteBranchChild (*branch_arg, !buffer_selector_, child_idx); + else { + + if (leaf_container_vector_arg) + leaf_container_vector_arg->push_back(child_leaf->getContainerPtr()); + serializeTreeCallback(**child_leaf, key_arg); } + break; + } + default: + break; } + + // pop current branch voxel from key + key_arg.popBranch(); + } + else if (branch_arg->hasChild(!buffer_selector_, child_idx)) { + // delete branch, free memory + deleteBranchChild(*branch_arg, !buffer_selector_, child_idx); } + } +} +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +Octree2BufBase::deserializeTreeRecursive( + BranchNode* branch_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 branch_reset_arg, + bool do_XOR_decoding_arg) +{ - ////////////////////////////////////////////////////////////////////////////////////////////// - template void - Octree2BufBase::deserializeTreeRecursive (BranchNode* branch_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 branch_reset_arg, bool do_XOR_decoding_arg) - { - // node bits - char nodeBits; - char recoveredNodeBits; - - // branch reset -> this branch has been taken from previous buffer - if (branch_reset_arg) - { - // we can safely remove children references - for (unsigned char child_idx = 0; child_idx < 8; child_idx++) - { - branch_arg->setChildPtr(buffer_selector_, child_idx, nullptr); - } - } + // branch reset -> this branch has been taken from previous buffer + if (branch_reset_arg) { + // we can safely remove children references + for (unsigned char child_idx = 0; child_idx < 8; child_idx++) { + branch_arg->setChildPtr(buffer_selector_, child_idx, nullptr); + } + } - if (binaryTreeIT_arg!=binaryTreeIT_End_arg) { - // read branch occupancy bit pattern from vector - nodeBits = *binaryTreeIT_arg++; + if (binaryTreeIT_arg != binaryTreeIT_End_arg) { + // read branch occupancy bit pattern from vector + char nodeBits = *binaryTreeIT_arg++; + // recover branch occupancy bit pattern + char recoveredNodeBits; + if (do_XOR_decoding_arg) { + recoveredNodeBits = + getBranchBitPattern(*branch_arg, !buffer_selector_) ^ nodeBits; + } + else { + recoveredNodeBits = nodeBits; + } - // recover branch occupancy bit pattern - if (do_XOR_decoding_arg) - { - recoveredNodeBits = getBranchBitPattern (*branch_arg, !buffer_selector_) ^ nodeBits; - } - else - { - recoveredNodeBits = nodeBits; - } + // iterate over all children + for (unsigned char child_idx = 0; child_idx < 8; child_idx++) { + // if occupancy bit for child_idx is set.. + if (recoveredNodeBits & (1 << child_idx)) { + // add current branch voxel to key + key_arg.pushBranch(child_idx); - // iterate over all children - for (unsigned char child_idx = 0; child_idx < 8; child_idx++) - { - // if occupancy bit for child_idx is set.. - if (recoveredNodeBits & (1 << child_idx)) - { - // add current branch voxel to key - key_arg.pushBranch(child_idx); - - bool doNodeReset; - - doNodeReset = false; - - if (depth_mask_arg > 1) - { - // we have not reached maximum tree depth - - BranchNode* child_branch; - - // check if we find a branch node reference in previous buffer - if (!branch_arg->hasChild(buffer_selector_, child_idx)) - { - - if (branch_arg->hasChild(!buffer_selector_, child_idx)) - { - OctreeNode* child_node = branch_arg->getChildPtr(!buffer_selector_,child_idx); - - 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 preceding buffer is a leaf node. - deleteBranchChild (*branch_arg, !buffer_selector_, child_idx); - child_branch = createBranchChild (*branch_arg, child_idx); - } - - // take child branch from previous buffer - doNodeReset = true; // reset the branch pointer array of stolen child node - } - else - { - // if required branch does not exist -> create it - child_branch = createBranchChild (*branch_arg, child_idx); - } - - branch_count_++; + if (depth_mask_arg > 1) { + // we have not reached maximum tree depth - } - else - { - // required branch node already exists - use it - child_branch = static_cast (branch_arg->getChildPtr(buffer_selector_,child_idx)); - } + bool doNodeReset = false; - // recursively proceed with indexed child branch - deserializeTreeRecursive (child_branch, depth_mask_arg / 2, key_arg, - binaryTreeIT_arg, binaryTreeIT_End_arg, - dataVectorIterator_arg, dataVectorEndIterator_arg, - doNodeReset, do_XOR_decoding_arg); + BranchNode* child_branch; - } - else - { - // branch childs are leaf nodes - LeafNode* child_leaf; - - // check if we can take copy a reference pointer from previous buffer - if (branch_arg->hasChild(!buffer_selector_, child_idx)) - { - // take child leaf node from previous buffer - 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 preceding buffer is a leaf node. - deleteBranchChild (*branch_arg, !buffer_selector_, child_idx); - child_leaf = createLeafChild (*branch_arg, child_idx); - } - } - else - { - // if required leaf does not exist -> create it - child_leaf = createLeafChild (*branch_arg, child_idx); - } + // check if we find a branch node reference in previous buffer + if (!branch_arg->hasChild(buffer_selector_, child_idx)) { - // we reached leaf node level + if (branch_arg->hasChild(!buffer_selector_, child_idx)) { + OctreeNode* child_node = + branch_arg->getChildPtr(!buffer_selector_, child_idx); - if (dataVectorIterator_arg - && (*dataVectorIterator_arg != *dataVectorEndIterator_arg)) - { - LeafContainerT& container = **child_leaf; - container = ***dataVectorIterator_arg; - ++*dataVectorIterator_arg; + 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 preceding buffer is a leaf node. + deleteBranchChild(*branch_arg, !buffer_selector_, child_idx); + child_branch = createBranchChild(*branch_arg, child_idx); } - leaf_count_++; + // take child branch from previous buffer + doNodeReset = true; // reset the branch pointer array of stolen child node + } + else { + // if required branch does not exist -> create it + child_branch = createBranchChild(*branch_arg, child_idx); + } - // execute deserialization callback - deserializeTreeCallback (**child_leaf, key_arg); + branch_count_++; + } + else { + // required branch node already exists - use it + child_branch = static_cast( + branch_arg->getChildPtr(buffer_selector_, child_idx)); + } + // recursively proceed with indexed child branch + deserializeTreeRecursive(child_branch, + depth_mask_arg / 2, + key_arg, + binaryTreeIT_arg, + binaryTreeIT_End_arg, + dataVectorIterator_arg, + dataVectorEndIterator_arg, + doNodeReset, + do_XOR_decoding_arg); + } + else { + // branch childs are leaf nodes + LeafNode* child_leaf; + + // check if we can take copy a reference pointer from previous buffer + if (branch_arg->hasChild(!buffer_selector_, child_idx)) { + // take child leaf node from previous buffer + 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); } - - // pop current branch voxel from key - key_arg.popBranch(); + else { + // depth has changed.. child in preceding buffer is a leaf node. + deleteBranchChild(*branch_arg, !buffer_selector_, child_idx); + child_leaf = createLeafChild(*branch_arg, child_idx); + } + } + else { + // if required leaf does not exist -> create it + child_leaf = createLeafChild(*branch_arg, child_idx); } - else if (branch_arg->hasChild (!buffer_selector_, child_idx)) - { - // remove old branch pointer information in current branch - branch_arg->setChildPtr(buffer_selector_, child_idx, nullptr); - - // remove unused branches in previous buffer - deleteBranchChild (*branch_arg, !buffer_selector_, child_idx); + + // we reached leaf node level + + if (dataVectorIterator_arg && + (*dataVectorIterator_arg != *dataVectorEndIterator_arg)) { + LeafContainerT& container = **child_leaf; + container = ***dataVectorIterator_arg; + ++*dataVectorIterator_arg; } + + leaf_count_++; + + // execute deserialization callback + deserializeTreeCallback(**child_leaf, key_arg); } + + // pop current branch voxel from key + key_arg.popBranch(); } + else if (branch_arg->hasChild(!buffer_selector_, child_idx)) { + // remove old branch pointer information in current branch + branch_arg->setChildPtr(buffer_selector_, child_idx, nullptr); + // remove unused branches in previous buffer + deleteBranchChild(*branch_arg, !buffer_selector_, child_idx); + } } - - ////////////////////////////////////////////////////////////////////////////////////////////// - template void - Octree2BufBase::treeCleanUpRecursive (BranchNode* branch_arg) - { - // occupancy bit pattern of branch node (previous octree buffer) - char occupied_children_bit_pattern_prev_buffer = getBranchBitPattern (*branch_arg, !buffer_selector_); - - // XOR of current and previous occupancy bit patterns - char node_XOR_bit_pattern = getBranchXORBitPattern (*branch_arg); - - // bit pattern indicating unused octree nodes in previous branch - char unused_branches_bit_pattern = node_XOR_bit_pattern & occupied_children_bit_pattern_prev_buffer; - - // iterate over all children - for (unsigned char child_idx = 0; child_idx < 8; child_idx++) - { - if (branch_arg->hasChild(buffer_selector_, child_idx)) - { - OctreeNode *child_node = branch_arg->getChildPtr(buffer_selector_,child_idx); - - switch (child_node->getNodeType ()) - { - case BRANCH_NODE: - { - // recursively proceed with indexed child branch - treeCleanUpRecursive (static_cast (child_node)); - break; - } - case LEAF_NODE: - // leaf level - nothing to do.. - break; - default: - break; - } - } + } +} - // check for unused branches in previous buffer - if (unused_branches_bit_pattern & (1 << child_idx)) - { - // delete branch, free memory - deleteBranchChild (*branch_arg, !buffer_selector_, child_idx); - } +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +Octree2BufBase::treeCleanUpRecursive( + BranchNode* branch_arg) +{ + // occupancy bit pattern of branch node (previous octree buffer) + char occupied_children_bit_pattern_prev_buffer = + getBranchBitPattern(*branch_arg, !buffer_selector_); + + // XOR of current and previous occupancy bit patterns + char node_XOR_bit_pattern = getBranchXORBitPattern(*branch_arg); + + // bit pattern indicating unused octree nodes in previous branch + char unused_branches_bit_pattern = + node_XOR_bit_pattern & occupied_children_bit_pattern_prev_buffer; + + // iterate over all children + for (unsigned char child_idx = 0; child_idx < 8; child_idx++) { + if (branch_arg->hasChild(buffer_selector_, child_idx)) { + OctreeNode* child_node = branch_arg->getChildPtr(buffer_selector_, child_idx); + + switch (child_node->getNodeType()) { + case BRANCH_NODE: { + // recursively proceed with indexed child branch + treeCleanUpRecursive(static_cast(child_node)); + break; + } + case LEAF_NODE: + // leaf level - nothing to do.. + break; + default: + break; } } + + // check for unused branches in previous buffer + if (unused_branches_bit_pattern & (1 << child_idx)) { + // delete branch, free memory + deleteBranchChild(*branch_arg, !buffer_selector_, child_idx); + } } } +} // namespace octree +} // namespace pcl -#define PCL_INSTANTIATE_Octree2BufBase(T) template class PCL_EXPORTS pcl::octree::Octree2BufBase; +#define PCL_INSTANTIATE_Octree2BufBase(T) \ + template class PCL_EXPORTS pcl::octree::Octree2BufBase; #endif - diff --git a/octree/include/pcl/octree/impl/octree_base.hpp b/octree/include/pcl/octree/impl/octree_base.hpp index c2a5bf7f..d7521bc6 100644 --- a/octree/include/pcl/octree/impl/octree_base.hpp +++ b/octree/include/pcl/octree/impl/octree_base.hpp @@ -43,521 +43,521 @@ #include -namespace pcl +namespace pcl { +namespace octree { +////////////////////////////////////////////////////////////////////////////////////////////// +template +OctreeBase::OctreeBase() +: leaf_count_(0) +, branch_count_(1) +, root_node_(new BranchNode()) +, depth_mask_(0) +, octree_depth_(0) +, dynamic_depth_enabled_(false) +{} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +OctreeBase::~OctreeBase() { - namespace octree - { - ////////////////////////////////////////////////////////////////////////////////////////////// - template - OctreeBase::OctreeBase () : - leaf_count_ (0), - branch_count_ (1), - root_node_ (new BranchNode ()), - depth_mask_ (0), - octree_depth_ (0), - dynamic_depth_enabled_ (false) - { - } + // deallocate tree structure + deleteTree(); + delete (root_node_); +} - ////////////////////////////////////////////////////////////////////////////////////////////// - template - OctreeBase::~OctreeBase () - { - // deallocate tree structure - deleteTree (); - delete (root_node_); - } +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +OctreeBase::setMaxVoxelIndex( + unsigned int max_voxel_index_arg) +{ + unsigned int tree_depth; - ////////////////////////////////////////////////////////////////////////////////////////////// - template - void - OctreeBase::setMaxVoxelIndex (unsigned int max_voxel_index_arg) - { - unsigned int tree_depth; + assert(max_voxel_index_arg > 0); - assert(max_voxel_index_arg>0); + // tree depth == bitlength of maxVoxels + tree_depth = + std::min(static_cast(OctreeKey::maxDepth), + static_cast(std::ceil(std::log2(max_voxel_index_arg)))); - // tree depth == bitlength of maxVoxels - tree_depth = std::min (static_cast (OctreeKey::maxDepth), static_cast (std::ceil (std::log2 (max_voxel_index_arg)))); + // define depthMask_ by setting a single bit to 1 at bit position == tree depth + depth_mask_ = (1 << (tree_depth - 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); - ////////////////////////////////////////////////////////////////////////////////////////////// - template - void - OctreeBase::setTreeDepth (unsigned int depth_arg) - { - assert(depth_arg>0); + // set octree depth + octree_depth_ = depth_arg; - // set octree depth - octree_depth_ = depth_arg; + // define depthMask_ by setting a single bit to 1 at bit position == tree depth + depth_mask_ = (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; +} - // define max. keys - max_key_.x = max_key_.y = max_key_.z = (1 << depth_arg) - 1; - } +////////////////////////////////////////////////////////////////////////////////////////////// +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); - ////////////////////////////////////////////////////////////////////////////////////////////// - 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 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 +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); - ////////////////////////////////////////////////////////////////////////////////////////////// - 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 (createLeaf(key)); +} - ////////////////////////////////////////////////////////////////////////////////////////////// - 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, depth_mask_, root_node_); - } +////////////////////////////////////////////////////////////////////////////////////////////// +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); - ////////////////////////////////////////////////////////////////////////////////////////////// - template - void - OctreeBase::deleteTree () - { - - if (root_node_) - { - // reset octree - deleteBranch (*root_node_); - leaf_count_ = 0; - branch_count_ = 1; - } + // check if key exist in octree + return (existLeaf(key)); +} - } +////////////////////////////////////////////////////////////////////////////////////////////// +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); - ////////////////////////////////////////////////////////////////////////////////////////////// - template - void - OctreeBase::serializeTree (std::vector& binary_tree_out_arg) - { + // check if key exist in octree + deleteLeafRecursive(key, depth_mask_, root_node_); +} - OctreeKey new_key; +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +OctreeBase::deleteTree() +{ - // clear binary vector - binary_tree_out_arg.clear (); - binary_tree_out_arg.reserve (this->branch_count_); + if (root_node_) { + // reset octree + deleteBranch(*root_node_); + leaf_count_ = 0; + branch_count_ = 1; + } +} - serializeTreeRecursive (root_node_, new_key, &binary_tree_out_arg, nullptr); - } +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +OctreeBase::serializeTree( + std::vector& binary_tree_out_arg) +{ - ////////////////////////////////////////////////////////////////////////////////////////////// - template - void - OctreeBase::serializeTree (std::vector& binary_tree_out_arg, - std::vector& leaf_container_vector_arg) - { + OctreeKey new_key; - OctreeKey new_key; + // clear binary vector + binary_tree_out_arg.clear(); + binary_tree_out_arg.reserve(this->branch_count_); - // clear output vectors - binary_tree_out_arg.clear (); - leaf_container_vector_arg.clear (); + serializeTreeRecursive(root_node_, new_key, &binary_tree_out_arg, nullptr); +} - binary_tree_out_arg.reserve (this->branch_count_); - leaf_container_vector_arg.reserve (this->leaf_count_); +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +OctreeBase::serializeTree( + std::vector& binary_tree_out_arg, + std::vector& leaf_container_vector_arg) +{ - serializeTreeRecursive (root_node_, new_key, &binary_tree_out_arg, &leaf_container_vector_arg); - } + OctreeKey new_key; - ////////////////////////////////////////////////////////////////////////////////////////////// - template - void - OctreeBase::serializeLeafs (std::vector& leaf_container_vector_arg) - { - OctreeKey new_key; + // clear output vectors + binary_tree_out_arg.clear(); + leaf_container_vector_arg.clear(); - // clear output vector - leaf_container_vector_arg.clear (); + binary_tree_out_arg.reserve(this->branch_count_); + leaf_container_vector_arg.reserve(this->leaf_count_); - leaf_container_vector_arg.reserve (this->leaf_count_); + serializeTreeRecursive( + root_node_, new_key, &binary_tree_out_arg, &leaf_container_vector_arg); +} - serializeTreeRecursive (root_node_, new_key, nullptr, &leaf_container_vector_arg); - } +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +OctreeBase::serializeLeafs( + std::vector& leaf_container_vector_arg) +{ + OctreeKey new_key; - ////////////////////////////////////////////////////////////////////////////////////////////// - template - void - OctreeBase::deserializeTree (std::vector& binary_tree_out_arg) - { - OctreeKey new_key; + // clear output vector + leaf_container_vector_arg.clear(); - // free existing tree before tree rebuild - deleteTree (); + leaf_container_vector_arg.reserve(this->leaf_count_); - //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 (); + serializeTreeRecursive(root_node_, new_key, nullptr, &leaf_container_vector_arg); +} - deserializeTreeRecursive (root_node_, - depth_mask_, - new_key, - binary_tree_out_it, - binary_tree_out_it_end, - nullptr, - nullptr); +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +OctreeBase::deserializeTree( + std::vector& binary_tree_out_arg) +{ + OctreeKey new_key; + + // free existing tree before tree rebuild + deleteTree(); + + // 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(root_node_, + depth_mask_, + new_key, + binary_tree_out_it, + binary_tree_out_it_end, + nullptr, + nullptr); +} - } +////////////////////////////////////////////////////////////////////////////////////////////// +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 leaf_vector_it = + leaf_container_vector_arg.begin(); + + // 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(); + + // 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(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 depth_mask_arg, + BranchNode* branch_arg, + LeafNode*& return_leaf_arg, + BranchNode*& parent_of_leaf_arg) +{ + // index to branch child + unsigned char child_idx; + + // find branch child from key + child_idx = key_arg.getChildIdxWithDepthMask(depth_mask_arg); + + OctreeNode* child_node = (*branch_arg)[child_idx]; + + 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); + + branch_count_++; + + // recursively proceed with indexed child branch + return createLeafRecursive(key_arg, + depth_mask_arg / 2, + childBranch, + return_leaf_arg, + parent_of_leaf_arg); + } + // 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; + + case LEAF_NODE: + return_leaf_arg = static_cast(child_node); + parent_of_leaf_arg = branch_arg; + break; + } + } + + return (depth_mask_arg >> 1); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +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; + + // find branch child from key + child_idx = key_arg.getChildIdxWithDepthMask(depth_mask_arg); + + OctreeNode* child_node = (*branch_arg)[child_idx]; + + 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; + + case LEAF_NODE: + // return existing leaf node + LeafNode* child_leaf; + child_leaf = static_cast(child_node); + + result_arg = child_leaf->getContainerPtr(); + break; + } + } +} - ////////////////////////////////////////////////////////////////////////////////////////////// - template - void - OctreeBase::deserializeTree (std::vector& binary_tree_in_arg, - std::vector& leaf_container_vector_arg) - { - OctreeKey new_key; +////////////////////////////////////////////////////////////////////////////////////////////// +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; - // set data iterator to first element - typename std::vector::const_iterator leaf_vector_it = leaf_container_vector_arg.begin (); + // find branch child from key + child_idx = key_arg.getChildIdxWithDepthMask(depth_mask_arg); - // set data iterator to last element - typename std::vector::const_iterator leaf_vector_it_end = leaf_container_vector_arg.end (); + OctreeNode* child_node = (*branch_arg)[child_idx]; - // free existing tree before tree rebuild - deleteTree (); + if (child_node) { + switch (child_node->getNodeType()) { - //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 (); + case BRANCH_NODE: + BranchNode* child_branch; + child_branch = static_cast(child_node); - deserializeTreeRecursive (root_node_, - depth_mask_, - new_key, - binary_tree_input_it, - binary_tree_input_it_end, - &leaf_vector_it, - &leaf_vector_it_end); + // recursively explore the indexed child branch + b_no_children = deleteLeafRecursive(key_arg, depth_mask_arg / 2, child_branch); + 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; - ////////////////////////////////////////////////////////////////////////////////////////////// - template - unsigned int - OctreeBase::createLeafRecursive (const OctreeKey& key_arg, - unsigned int depth_mask_arg, - BranchNode* branch_arg, - LeafNode*& return_leaf_arg, - BranchNode*& parent_of_leaf_arg) - { - // index to branch child - unsigned char child_idx; + case LEAF_NODE: + // return existing leaf node - // find branch child from key - child_idx = key_arg.getChildIdxWithDepthMask (depth_mask_arg); + // our child is a leaf node -> delete it + deleteBranchChild(*branch_arg, child_idx); + this->leaf_count_--; + break; + } + } - OctreeNode* child_node = (*branch_arg)[child_idx]; + // 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); +} - 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); +////////////////////////////////////////////////////////////////////////////////////////////// +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 +{ + char node_bit_pattern; - branch_count_++; + // branch occupancy bit pattern + node_bit_pattern = getBranchBitPattern(*branch_arg); - // recursively proceed with indexed child branch - return createLeafRecursive (key_arg, depth_mask_arg / 2, childBranch, return_leaf_arg, parent_of_leaf_arg); + // write bit pattern to output vector + if (binary_tree_out_arg) + binary_tree_out_arg->push_back(node_bit_pattern); - } - // 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; - - case LEAF_NODE: - return_leaf_arg = static_cast (child_node);; - parent_of_leaf_arg = branch_arg; - break; - } + // iterate over all children + for (unsigned char child_idx = 0; child_idx < 8; child_idx++) { - } + // if child exist + if (branch_arg->hasChild(child_idx)) { + // add current branch voxel to key + key_arg.pushBranch(child_idx); - return (depth_mask_arg >> 1); - } + OctreeNode* childNode = branch_arg->getChildPtr(child_idx); - ////////////////////////////////////////////////////////////////////////////////////////////// - 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; - - // find branch child from key - child_idx = key_arg.getChildIdxWithDepthMask (depth_mask_arg); - - OctreeNode* child_node = (*branch_arg)[child_idx]; - - 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; - - case LEAF_NODE: - // return existing leaf node - LeafNode* child_leaf; - child_leaf = static_cast (child_node); - - result_arg = child_leaf->getContainerPtr (); - break; - } - } + switch (childNode->getNodeType()) { + 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); - ////////////////////////////////////////////////////////////////////////////////////////////// - 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 - child_idx = key_arg.getChildIdxWithDepthMask (depth_mask_arg); - - OctreeNode* child_node = (*branch_arg)[child_idx]; - - if (child_node) - { - switch (child_node->getNodeType ()) - { - - case BRANCH_NODE: - BranchNode* child_branch; - child_branch = static_cast (child_node); - - // recursively explore the indexed child branch - b_no_children = deleteLeafRecursive (key_arg, depth_mask_arg / 2, child_branch); - - 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 - - // our child is a leaf node -> delete it - deleteBranchChild (*branch_arg, child_idx); - this->leaf_count_--; - break; - } - } + if (leaf_container_vector_arg) + leaf_container_vector_arg->push_back(child_leaf->getContainerPtr()); - // 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); + // we reached a leaf node -> execute serialization callback + serializeTreeCallback(**child_leaf, key_arg); + break; + } + default: + break; } - ////////////////////////////////////////////////////////////////////////////////////////////// - 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 - { - char node_bit_pattern; - - // branch occupancy bit pattern - node_bit_pattern = getBranchBitPattern (*branch_arg); - - // 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 (unsigned char child_idx = 0; child_idx < 8; child_idx++) - { - - // if child exist - if (branch_arg->hasChild (child_idx)) - { - // add current branch voxel to key - key_arg.pushBranch (child_idx); - - OctreeNode *childNode = branch_arg->getChildPtr (child_idx); - - switch (childNode->getNodeType ()) - { - 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 (leaf_container_vector_arg) - leaf_container_vector_arg->push_back (child_leaf->getContainerPtr ()); - - // we reached a leaf node -> execute serialization callback - serializeTreeCallback (**child_leaf, key_arg); - 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 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) +{ + + if (binary_tree_input_it_arg != binary_tree_input_it_end_arg) { + // read branch occupancy bit pattern from input vector + char node_bits = (*binary_tree_input_it_arg); + binary_tree_input_it_arg++; + + // iterate over all children + for (unsigned char child_idx = 0; child_idx < 8; child_idx++) { + // if occupancy bit for child_idx is set.. + if (node_bits & (1 << child_idx)) { + // add current branch voxel to key + key_arg.pushBranch(child_idx); + + if (depth_mask_arg > 1) { + // we have not reached maximum tree depth + BranchNode* newBranch = createBranchChild(*branch_arg, child_idx); + + branch_count_++; + + // 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 - ////////////////////////////////////////////////////////////////////////////////////////////// - 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) - { - char node_bits; - - if (binary_tree_input_it_arg != binary_tree_input_it_end_arg) - { - // read branch occupancy bit pattern from input vector - node_bits = (*binary_tree_input_it_arg); - binary_tree_input_it_arg++; - - // iterate over all children - for (unsigned char child_idx = 0; child_idx < 8; child_idx++) - { - // if occupancy bit for child_idx is set.. - if (node_bits & (1 << child_idx)) - { - // add current branch voxel to key - key_arg.pushBranch (child_idx); - - if (depth_mask_arg > 1) - { - // we have not reached maximum tree depth - BranchNode * newBranch = createBranchChild (*branch_arg, child_idx); - - branch_count_++; - - // 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 - - LeafNode* child_leaf = createLeafChild (*branch_arg, child_idx); - - 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; - } - - leaf_count_++; - - // execute deserialization callback - deserializeTreeCallback (**child_leaf, key_arg); - } - - // pop current branch voxel from key - key_arg.popBranch (); - } + LeafNode* child_leaf = createLeafChild(*branch_arg, child_idx); + + 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; } + + leaf_count_++; + + // execute deserialization callback + deserializeTreeCallback(**child_leaf, key_arg); } - } + // pop current branch voxel from key + key_arg.popBranch(); + } + } } } -#define PCL_INSTANTIATE_OctreeBase(T) template class PCL_EXPORTS pcl::octree::OctreeBase; +} // namespace octree +} // namespace pcl -#endif +#define PCL_INSTANTIATE_OctreeBase(T) \ + template class PCL_EXPORTS pcl::octree::OctreeBase; +#endif diff --git a/octree/include/pcl/octree/impl/octree_iterator.hpp b/octree/include/pcl/octree/impl/octree_iterator.hpp index 3248e4c8..76e33fab 100644 --- a/octree/include/pcl/octree/impl/octree_iterator.hpp +++ b/octree/include/pcl/octree/impl/octree_iterator.hpp @@ -41,344 +41,337 @@ #include -namespace pcl +namespace pcl { +namespace octree { +////////////////////////////////////////////////////////////////////////////////////////////// +template +OctreeDepthFirstIterator::OctreeDepthFirstIterator(unsigned int max_depth_arg) +: OctreeIteratorBase(max_depth_arg), stack_() { - namespace octree - { - ////////////////////////////////////////////////////////////////////////////////////////////// - template - OctreeDepthFirstIterator::OctreeDepthFirstIterator (unsigned int max_depth_arg) : - OctreeIteratorBase (max_depth_arg), stack_ () - { - // initialize iterator - this->reset (); - } + // initialize iterator + this->reset(); +} - ////////////////////////////////////////////////////////////////////////////////////////////// - template - OctreeDepthFirstIterator::OctreeDepthFirstIterator (OctreeT* octree_arg, unsigned int max_depth_arg) : - OctreeIteratorBase (octree_arg, max_depth_arg), stack_ () - { - // initialize iterator - this->reset (); - } +////////////////////////////////////////////////////////////////////////////////////////////// +template +OctreeDepthFirstIterator::OctreeDepthFirstIterator(OctreeT* octree_arg, + unsigned int max_depth_arg) +: OctreeIteratorBase(octree_arg, max_depth_arg), stack_() +{ + // initialize iterator + this->reset(); +} - ////////////////////////////////////////////////////////////////////////////////////////////// - template - void OctreeDepthFirstIterator::reset () - { - OctreeIteratorBase::reset (); +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +OctreeDepthFirstIterator::reset() +{ + OctreeIteratorBase::reset(); - if (this->octree_) - { - // allocate stack - stack_.reserve (this->max_octree_depth_); + if (this->octree_) { + // allocate stack + stack_.reserve(this->max_octree_depth_); - // empty stack - stack_.clear (); + // empty stack + stack_.clear(); - // pushing root node to stack - 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; + // pushing root node to stack + 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(stack_entry); + stack_.push_back(stack_entry); - this->current_state_ = &stack_.back(); - } + this->current_state_ = &stack_.back(); + } +} - } +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +OctreeDepthFirstIterator::skipChildVoxels() +{ - ////////////////////////////////////////////////////////////////////////////////////////////// - template - void OctreeDepthFirstIterator::skipChildVoxels () - { - - if (stack_.size ()) - { - // current depth - unsigned char current_depth = stack_.back ().depth_; - - // pop from stack as long as we find stack elements with depth >= current depth - while (stack_.size () && (stack_.back ().depth_ >= current_depth)) - stack_.pop_back (); - - if (stack_.size ()) - { - this->current_state_ = &stack_.back(); - } else - { - this->current_state_ = 0; - } - } + if (stack_.size()) { + // current depth + unsigned char current_depth = stack_.back().depth_; + + // pop from stack as long as we find stack elements with depth >= current depth + while (stack_.size() && (stack_.back().depth_ >= current_depth)) + stack_.pop_back(); + if (stack_.size()) { + this->current_state_ = &stack_.back(); } + else { + this->current_state_ = 0; + } + } +} - ////////////////////////////////////////////////////////////////////////////////////////////// - template - OctreeDepthFirstIterator& - OctreeDepthFirstIterator::operator++ () - { - - if (stack_.size ()) - { - // get stack element - IteratorState stack_entry = stack_.back (); - stack_.pop_back (); - - stack_entry.depth_ ++; - OctreeKey& current_key = stack_entry.key_; - - if ( (this->max_octree_depth_>=stack_entry.depth_) && - (stack_entry.node_->getNodeType () == BRANCH_NODE) ) - { - // current node is a branch node - BranchNode* current_branch = - static_cast (stack_entry.node_); - - // add all children to stack - for (std::int8_t i = 7; i >= 0; --i) - { - const unsigned char child_idx = (unsigned char) i; - - // if child exist - if (this->octree_->branchHasChild(*current_branch, child_idx)) - { - // add child to stack - current_key.pushBranch (child_idx); - - stack_entry.node_ = this->octree_->getBranchChildPtr(*current_branch, child_idx); - - stack_.push_back(stack_entry); - - current_key.popBranch(); - } - } - } +////////////////////////////////////////////////////////////////////////////////////////////// +template +OctreeDepthFirstIterator& +OctreeDepthFirstIterator::operator++() +{ + + if (stack_.size()) { + // get stack element + IteratorState stack_entry = stack_.back(); + stack_.pop_back(); + + stack_entry.depth_++; - if (stack_.size ()) - { - this->current_state_ = &stack_.back(); - } else - { - this->current_state_ = 0; + if ((this->max_octree_depth_ >= stack_entry.depth_) && + (stack_entry.node_->getNodeType() == BRANCH_NODE)) { + // current node is a branch node + BranchNode* current_branch = static_cast(stack_entry.node_); + + OctreeKey& current_key = stack_entry.key_; + + // add all children to stack + for (std::int8_t i = 7; i >= 0; --i) { + const unsigned char child_idx = (unsigned char)i; + + // if child exist + if (this->octree_->branchHasChild(*current_branch, child_idx)) { + // add child to stack + current_key.pushBranch(child_idx); + + stack_entry.node_ = + this->octree_->getBranchChildPtr(*current_branch, child_idx); + + stack_.push_back(stack_entry); + + current_key.popBranch(); } } - - return (*this); } - ////////////////////////////////////////////////////////////////////////////////////////////// - template - OctreeBreadthFirstIterator::OctreeBreadthFirstIterator (unsigned int max_depth_arg) : - OctreeIteratorBase (max_depth_arg), FIFO_ () - { - OctreeIteratorBase::reset (); - - // initialize iterator - this->reset (); + if (stack_.size()) { + this->current_state_ = &stack_.back(); + } + else { + this->current_state_ = 0; } + } - ////////////////////////////////////////////////////////////////////////////////////////////// - template - OctreeBreadthFirstIterator::OctreeBreadthFirstIterator (OctreeT* octree_arg, unsigned int max_depth_arg) : - OctreeIteratorBase (octree_arg, max_depth_arg), FIFO_ () - { - OctreeIteratorBase::reset (); + return (*this); +} - // initialize iterator - this->reset (); - } +////////////////////////////////////////////////////////////////////////////////////////////// +template +OctreeBreadthFirstIterator::OctreeBreadthFirstIterator( + unsigned int max_depth_arg) +: OctreeIteratorBase(max_depth_arg), FIFO_() +{ + OctreeIteratorBase::reset(); - ////////////////////////////////////////////////////////////////////////////////////////////// - template - void OctreeBreadthFirstIterator::reset () - { - OctreeIteratorBase::reset (); + // initialize iterator + this->reset(); +} - // init FIFO - FIFO_.clear (); +////////////////////////////////////////////////////////////////////////////////////////////// +template +OctreeBreadthFirstIterator::OctreeBreadthFirstIterator( + OctreeT* octree_arg, unsigned int max_depth_arg) +: OctreeIteratorBase(octree_arg, max_depth_arg), FIFO_() +{ + OctreeIteratorBase::reset(); - if (this->octree_) - { - // pushing root node to stack - 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; + // initialize iterator + this->reset(); +} - FIFO_.push_back(FIFO_entry); +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +OctreeBreadthFirstIterator::reset() +{ + OctreeIteratorBase::reset(); - this->current_state_ = &FIFO_.front(); - } - } + // init FIFO + FIFO_.clear(); - ////////////////////////////////////////////////////////////////////////////////////////////// - template - OctreeBreadthFirstIterator& - OctreeBreadthFirstIterator::operator++ () - { - - if (FIFO_.size ()) - { - // get stack element - IteratorState FIFO_entry = FIFO_.front (); - FIFO_.pop_front (); - - FIFO_entry.depth_ ++; - OctreeKey& current_key = FIFO_entry.key_; - - if ( (this->max_octree_depth_>=FIFO_entry.depth_) && - (FIFO_entry.node_->getNodeType () == BRANCH_NODE) ) - { - // current node is a branch node - BranchNode* current_branch = - static_cast (FIFO_entry.node_); - - // iterate over all children - for (unsigned char child_idx = 0; child_idx < 8 ; ++child_idx) - { - - // if child exist - if (this->octree_->branchHasChild(*current_branch, child_idx)) - { - // add child to stack - current_key.pushBranch (static_cast (child_idx)); - - FIFO_entry.node_ = this->octree_->getBranchChildPtr(*current_branch, child_idx); - - FIFO_.push_back(FIFO_entry); - - current_key.popBranch(); - } - } - } + if (this->octree_) { + // pushing root node to stack + 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; - if (FIFO_.size ()) - { - this->current_state_ = &FIFO_.front(); - } else - { - this->current_state_ = 0; - } + FIFO_.push_back(FIFO_entry); - } + this->current_state_ = &FIFO_.front(); + } +} - return (*this); - } +////////////////////////////////////////////////////////////////////////////////////////////// +template +OctreeBreadthFirstIterator& +OctreeBreadthFirstIterator::operator++() +{ - ////////////////////////////////////////////////////////////////////////////////////////////// - template - OctreeFixedDepthIterator::OctreeFixedDepthIterator () : - OctreeBreadthFirstIterator (0u), fixed_depth_ (0u) - {} - - ////////////////////////////////////////////////////////////////////////////////////////////// - template - OctreeFixedDepthIterator::OctreeFixedDepthIterator (OctreeT* octree_arg, unsigned int fixed_depth_arg) : - OctreeBreadthFirstIterator (octree_arg, fixed_depth_arg), fixed_depth_ (fixed_depth_arg) - { - this->reset (fixed_depth_arg); - } + if (FIFO_.size()) { + // get stack element + IteratorState FIFO_entry = FIFO_.front(); + FIFO_.pop_front(); - ////////////////////////////////////////////////////////////////////////////////////////////// - template - void OctreeFixedDepthIterator::reset (unsigned int fixed_depth_arg) - { - // Set the desired depth to walk through - fixed_depth_ = fixed_depth_arg; + FIFO_entry.depth_++; - if (!this->octree_) - { - return; - } + if ((this->max_octree_depth_ >= FIFO_entry.depth_) && + (FIFO_entry.node_->getNodeType() == BRANCH_NODE)) { + // current node is a branch node + BranchNode* current_branch = static_cast(FIFO_entry.node_); - // If I'm nowhere, reset - // If I'm somewhere and I can't guarantee I'm before the first node, reset - if ((!this->current_state_) || (fixed_depth_ <= this->getCurrentOctreeDepth ())) - OctreeBreadthFirstIterator::reset (); + // iterate over all children + for (unsigned char child_idx = 0; child_idx < 8; ++child_idx) { - if (this->octree_->getTreeDepth () < fixed_depth_) - { - PCL_WARN ("[pcl::octree::FixedDepthIterator] The requested fixed depth was bigger than the octree's depth.\n"); - PCL_WARN ("[pcl::octree::FixedDepthIterator] fixed_depth = %d (instead of %d)\n", this->octree_->getTreeDepth (), fixed_depth_); - } + // if child exist + if (this->octree_->branchHasChild(*current_branch, child_idx)) { + // add child to stack + OctreeKey& current_key = FIFO_entry.key_; + current_key.pushBranch(static_cast(child_idx)); - // By default for the parent class OctreeBreadthFirstIterator, if the - // depth argument is equal to 0, the iterator would run over every node. - // For the OctreeFixedDepthIterator, whatever the depth ask, set the - // max_octree_depth_ accordingly - this->max_octree_depth_ = std::min (fixed_depth_, this->octree_->getTreeDepth ()); + FIFO_entry.node_ = + this->octree_->getBranchChildPtr(*current_branch, child_idx); - // Restore previous state in case breath first iterator had child nodes already set up - if (FIFO_.size ()) - this->current_state_ = &FIFO_.front (); + FIFO_.push_back(FIFO_entry); - // Iterate all the way to the desired level - while (this->current_state_ && (this->getCurrentOctreeDepth () != fixed_depth_)) - OctreeBreadthFirstIterator::operator++ (); + current_key.popBranch(); + } + } } - ////////////////////////////////////////////////////////////////////////////////////////////// - template - OctreeLeafNodeBreadthFirstIterator::OctreeLeafNodeBreadthFirstIterator (unsigned int max_depth_arg) : - OctreeBreadthFirstIterator (max_depth_arg) - { - reset (); + if (FIFO_.size()) { + this->current_state_ = &FIFO_.front(); } - - ////////////////////////////////////////////////////////////////////////////////////////////// - template - OctreeLeafNodeBreadthFirstIterator::OctreeLeafNodeBreadthFirstIterator (OctreeT* octree_arg, unsigned int max_depth_arg) : - OctreeBreadthFirstIterator (octree_arg, max_depth_arg) - { - reset (); + else { + this->current_state_ = 0; } + } - ////////////////////////////////////////////////////////////////////////////////////////////// - template - OctreeLeafNodeBreadthFirstIterator::OctreeLeafNodeBreadthFirstIterator (OctreeT* octree_arg, - unsigned int max_depth_arg, - IteratorState* current_state, - const std::deque& fifo) - : OctreeBreadthFirstIterator (octree_arg, - max_depth_arg, - current_state, - fifo) - {} - - ////////////////////////////////////////////////////////////////////////////////////////////// - template - void OctreeLeafNodeBreadthFirstIterator::reset () - { - OctreeBreadthFirstIterator::reset (); - ++*this; - } + return (*this); +} - ////////////////////////////////////////////////////////////////////////////////////////////// - template - OctreeLeafNodeBreadthFirstIterator& - OctreeLeafNodeBreadthFirstIterator::operator++ () - { - do - { - OctreeBreadthFirstIterator::operator++ (); - } while ((this->current_state_) && (this->current_state_->node_->getNodeType () != LEAF_NODE)); - - return (*this); - } +////////////////////////////////////////////////////////////////////////////////////////////// +template +OctreeFixedDepthIterator::OctreeFixedDepthIterator() +: OctreeBreadthFirstIterator(0u), fixed_depth_(0u) +{} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +OctreeFixedDepthIterator::OctreeFixedDepthIterator( + OctreeT* octree_arg, unsigned int fixed_depth_arg) +: OctreeBreadthFirstIterator(octree_arg, fixed_depth_arg) +, fixed_depth_(fixed_depth_arg) +{ + this->reset(fixed_depth_arg); +} - ////////////////////////////////////////////////////////////////////////////////////////////// - template - OctreeLeafNodeBreadthFirstIterator - OctreeLeafNodeBreadthFirstIterator::operator++ (int) - { - OctreeLeafNodeBreadthFirstIterator _Tmp = *this; - ++*this; - return (_Tmp); - } +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +OctreeFixedDepthIterator::reset(unsigned int fixed_depth_arg) +{ + // Set the desired depth to walk through + fixed_depth_ = fixed_depth_arg; + + if (!this->octree_) { + return; } + + // If I'm nowhere, reset + // If I'm somewhere and I can't guarantee I'm before the first node, reset + if ((!this->current_state_) || (fixed_depth_ <= this->getCurrentOctreeDepth())) + OctreeBreadthFirstIterator::reset(); + + if (this->octree_->getTreeDepth() < fixed_depth_) { + PCL_WARN("[pcl::octree::FixedDepthIterator] The requested fixed depth was bigger " + "than the octree's depth.\n"); + PCL_WARN("[pcl::octree::FixedDepthIterator] fixed_depth = %d (instead of %d)\n", + this->octree_->getTreeDepth(), + fixed_depth_); + } + + // By default for the parent class OctreeBreadthFirstIterator, if the + // depth argument is equal to 0, the iterator would run over every node. + // For the OctreeFixedDepthIterator, whatever the depth ask, set the + // max_octree_depth_ accordingly + this->max_octree_depth_ = std::min(fixed_depth_, this->octree_->getTreeDepth()); + + // Restore previous state in case breath first iterator had child nodes already set up + if (FIFO_.size()) + this->current_state_ = &FIFO_.front(); + + // Iterate all the way to the desired level + while (this->current_state_ && (this->getCurrentOctreeDepth() != fixed_depth_)) + OctreeBreadthFirstIterator::operator++(); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +OctreeLeafNodeBreadthFirstIterator::OctreeLeafNodeBreadthFirstIterator( + unsigned int max_depth_arg) +: OctreeBreadthFirstIterator(max_depth_arg) +{ + reset(); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +OctreeLeafNodeBreadthFirstIterator::OctreeLeafNodeBreadthFirstIterator( + OctreeT* octree_arg, unsigned int max_depth_arg) +: OctreeBreadthFirstIterator(octree_arg, max_depth_arg) +{ + reset(); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +OctreeLeafNodeBreadthFirstIterator::OctreeLeafNodeBreadthFirstIterator( + OctreeT* octree_arg, + unsigned int max_depth_arg, + IteratorState* current_state, + const std::deque& fifo) +: OctreeBreadthFirstIterator(octree_arg, max_depth_arg, current_state, fifo) +{} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +void +OctreeLeafNodeBreadthFirstIterator::reset() +{ + OctreeBreadthFirstIterator::reset(); + ++*this; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +OctreeLeafNodeBreadthFirstIterator& +OctreeLeafNodeBreadthFirstIterator::operator++() +{ + do { + OctreeBreadthFirstIterator::operator++(); + } while ((this->current_state_) && + (this->current_state_->node_->getNodeType() != LEAF_NODE)); + + return (*this); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template +OctreeLeafNodeBreadthFirstIterator +OctreeLeafNodeBreadthFirstIterator::operator++(int) +{ + OctreeLeafNodeBreadthFirstIterator _Tmp = *this; + ++*this; + return (_Tmp); } +} // namespace octree +} // namespace pcl #endif diff --git a/octree/include/pcl/octree/impl/octree_pointcloud.hpp b/octree/include/pcl/octree/impl/octree_pointcloud.hpp index 16569c31..d2ee3733 100644 --- a/octree/include/pcl/octree/impl/octree_pointcloud.hpp +++ b/octree/include/pcl/octree/impl/octree_pointcloud.hpp @@ -45,111 +45,156 @@ #include ////////////////////////////////////////////////////////////////////////////////////////////// -template -pcl::octree::OctreePointCloud::OctreePointCloud (const double resolution) : - OctreeT (), input_ (PointCloudConstPtr ()), indices_ (IndicesConstPtr ()), - 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) +template +pcl::octree::OctreePointCloud:: + OctreePointCloud(const double resolution) +: OctreeT() +, input_(PointCloudConstPtr()) +, indices_(IndicesConstPtr()) +, 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); + assert(resolution > 0.0f); } ////////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::octree::OctreePointCloud::addPointsFromInputCloud () +template +void +pcl::octree::OctreePointCloud:: + addPointsFromInputCloud() { - if (indices_) - { - for (const int &index : *indices_) - { - assert( (index >= 0) && (index < static_cast (input_->points.size ()))); - - if (isFinite (input_->points[index])) - { + if (indices_) { + for (const int& index : *indices_) { + assert((index >= 0) && (index < static_cast(input_->points.size()))); + + if (isFinite(input_->points[index])) { // add points to octree - this->addPointIdx (index); + this->addPointIdx(index); } } } - else - { - for (std::size_t i = 0; i < input_->points.size (); i++) - { - if (isFinite (input_->points[i])) - { + else { + for (std::size_t i = 0; i < input_->points.size(); i++) { + if (isFinite(input_->points[i])) { // add points to octree - this->addPointIdx (static_cast (i)); + this->addPointIdx(static_cast(i)); } } } } ////////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::octree::OctreePointCloud::addPointFromCloud (const int point_idx_arg, IndicesPtr indices_arg) +template +void +pcl::octree::OctreePointCloud:: + addPointFromCloud(const int point_idx_arg, IndicesPtr indices_arg) { - this->addPointIdx (point_idx_arg); + this->addPointIdx(point_idx_arg); if (indices_arg) - indices_arg->push_back (point_idx_arg); + indices_arg->push_back(point_idx_arg); } ////////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::octree::OctreePointCloud::addPointToCloud (const PointT& point_arg, PointCloudPtr cloud_arg) +template +void +pcl::octree::OctreePointCloud:: + addPointToCloud(const PointT& point_arg, PointCloudPtr cloud_arg) { - assert (cloud_arg==input_); + assert(cloud_arg == input_); - cloud_arg->push_back (point_arg); + cloud_arg->push_back(point_arg); - this->addPointIdx (static_cast (cloud_arg->points.size ()) - 1); + this->addPointIdx(static_cast(cloud_arg->points.size()) - 1); } ////////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::octree::OctreePointCloud::addPointToCloud (const PointT& point_arg, PointCloudPtr cloud_arg, - IndicesPtr indices_arg) +template +void +pcl::octree::OctreePointCloud:: + addPointToCloud(const PointT& point_arg, + PointCloudPtr cloud_arg, + IndicesPtr indices_arg) { - assert (cloud_arg==input_); - assert (indices_arg==indices_); + assert(cloud_arg == input_); + assert(indices_arg == indices_); - cloud_arg->push_back (point_arg); + cloud_arg->push_back(point_arg); - this->addPointFromCloud (static_cast (cloud_arg->points.size ()) - 1, indices_arg); + this->addPointFromCloud(static_cast(cloud_arg->points.size()) - 1, + indices_arg); } ////////////////////////////////////////////////////////////////////////////////////////////// -template bool -pcl::octree::OctreePointCloud::isVoxelOccupiedAtPoint (const PointT& point_arg) const +template +bool +pcl::octree::OctreePointCloud:: + isVoxelOccupiedAtPoint(const PointT& point_arg) const { - if (!isPointWithinBoundingBox (point_arg)) - { + if (!isPointWithinBoundingBox(point_arg)) { return false; } OctreeKey key; // generate key for point - this->genOctreeKeyforPoint (point_arg, key); + this->genOctreeKeyforPoint(point_arg, key); // search for key in octree - return (this->existLeaf (key)); + return (this->existLeaf(key)); } ////////////////////////////////////////////////////////////////////////////////////////////// -template bool -pcl::octree::OctreePointCloud::isVoxelOccupiedAtPoint (const int& point_idx_arg) const +template +bool +pcl::octree::OctreePointCloud:: + isVoxelOccupiedAtPoint(const int& point_idx_arg) const { // retrieve point from input cloud const PointT& point = this->input_->points[point_idx_arg]; // search for voxel at point in octree - return (this->isVoxelOccupiedAtPoint (point)); + return (this->isVoxelOccupiedAtPoint(point)); } ////////////////////////////////////////////////////////////////////////////////////////////// -template bool -pcl::octree::OctreePointCloud::isVoxelOccupiedAtPoint ( - const double point_x_arg, const double point_y_arg, const double point_z_arg) const +template +bool +pcl::octree::OctreePointCloud:: + isVoxelOccupiedAtPoint(const double point_x_arg, + const double point_y_arg, + const double point_z_arg) const { // create a new point with the argument coordinates PointT point; @@ -158,115 +203,134 @@ pcl::octree::OctreePointCloud point.z = point_z_arg; // search for voxel at point in octree - return (this->isVoxelOccupiedAtPoint (point)); + return (this->isVoxelOccupiedAtPoint(point)); } ////////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::octree::OctreePointCloud::deleteVoxelAtPoint (const PointT& point_arg) +template +void +pcl::octree::OctreePointCloud:: + deleteVoxelAtPoint(const PointT& point_arg) { - if (!isPointWithinBoundingBox (point_arg)) - { + if (!isPointWithinBoundingBox(point_arg)) { return; } OctreeKey key; // generate key for point - this->genOctreeKeyforPoint (point_arg, key); + this->genOctreeKeyforPoint(point_arg, key); - this->removeLeaf (key); + this->removeLeaf(key); } ////////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::octree::OctreePointCloud::deleteVoxelAtPoint (const int& point_idx_arg) +template +void +pcl::octree::OctreePointCloud:: + deleteVoxelAtPoint(const int& point_idx_arg) { // retrieve point from input cloud const PointT& point = this->input_->points[point_idx_arg]; // delete leaf at point - this->deleteVoxelAtPoint (point); + this->deleteVoxelAtPoint(point); } ////////////////////////////////////////////////////////////////////////////////////////////// -template int -pcl::octree::OctreePointCloud::getOccupiedVoxelCenters ( - AlignedPointTVector &voxel_center_list_arg) const +template +int +pcl::octree::OctreePointCloud:: + getOccupiedVoxelCenters(AlignedPointTVector& voxel_center_list_arg) const { OctreeKey key; key.x = key.y = key.z = 0; - voxel_center_list_arg.clear (); - - return getOccupiedVoxelCentersRecursive (this->root_node_, key, voxel_center_list_arg); + voxel_center_list_arg.clear(); + return getOccupiedVoxelCentersRecursive(this->root_node_, key, voxel_center_list_arg); } ////////////////////////////////////////////////////////////////////////////////////////////// -template int -pcl::octree::OctreePointCloud::getApproxIntersectedVoxelCentersBySegment ( - const Eigen::Vector3f& origin, - const Eigen::Vector3f& end, - AlignedPointTVector &voxel_center_list, - float precision) +template +int +pcl::octree::OctreePointCloud:: + getApproxIntersectedVoxelCentersBySegment(const Eigen::Vector3f& origin, + const Eigen::Vector3f& end, + AlignedPointTVector& voxel_center_list, + float precision) { Eigen::Vector3f direction = end - origin; - float norm = direction.norm (); - direction.normalize (); + float norm = direction.norm(); + direction.normalize(); - const float step_size = static_cast (resolution_) * precision; + const float step_size = static_cast(resolution_) * precision; // Ensure we get at least one step for the first voxel. - const int nsteps = std::max (1, static_cast (norm / step_size)); + const int nsteps = std::max(1, static_cast(norm / step_size)); OctreeKey prev_key; bool bkeyDefined = false; // Walk along the line segment with small steps. - for (int i = 0; i < nsteps; ++i) - { - Eigen::Vector3f p = origin + (direction * step_size * static_cast (i)); + for (int i = 0; i < nsteps; ++i) { + Eigen::Vector3f p = origin + (direction * step_size * static_cast(i)); PointT octree_p; - octree_p.x = p.x (); - octree_p.y = p.y (); - octree_p.z = p.z (); + octree_p.x = p.x(); + octree_p.y = p.y(); + octree_p.z = p.z(); OctreeKey key; - this->genOctreeKeyforPoint (octree_p, key); + this->genOctreeKeyforPoint(octree_p, key); // Not a new key, still the same voxel. - if ((key == prev_key) && (bkeyDefined) ) + if ((key == prev_key) && (bkeyDefined)) continue; prev_key = key; bkeyDefined = true; PointT center; - genLeafNodeCenterFromOctreeKey (key, center); - voxel_center_list.push_back (center); + genLeafNodeCenterFromOctreeKey(key, center); + voxel_center_list.push_back(center); } OctreeKey end_key; PointT end_p; - end_p.x = end.x (); - end_p.y = end.y (); - end_p.z = end.z (); - this->genOctreeKeyforPoint (end_p, end_key); - if (!(end_key == prev_key)) - { + end_p.x = end.x(); + end_p.y = end.y(); + end_p.z = end.z(); + this->genOctreeKeyforPoint(end_p, end_key); + if (!(end_key == prev_key)) { PointT center; - genLeafNodeCenterFromOctreeKey (end_key, center); - voxel_center_list.push_back (center); + genLeafNodeCenterFromOctreeKey(end_key, center); + voxel_center_list.push_back(center); } - return (static_cast (voxel_center_list.size ())); + return (static_cast(voxel_center_list.size())); } ////////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::octree::OctreePointCloud::defineBoundingBox () +template +void +pcl::octree::OctreePointCloud:: + defineBoundingBox() { double minX, minY, minZ, maxX, maxY, maxZ; @@ -275,11 +339,11 @@ pcl::octree::OctreePointCloud PointT max_pt; // bounding box cannot be changed once the octree contains elements - assert (this->leaf_count_ == 0); + assert(this->leaf_count_ == 0); - pcl::getMinMax3D (*input_, min_pt, max_pt); + pcl::getMinMax3D(*input_, min_pt, max_pt); - float minValue = std::numeric_limits::epsilon () * 512.0f; + float minValue = std::numeric_limits::epsilon() * 512.0f; minX = min_pt.x; minY = min_pt.y; @@ -290,24 +354,29 @@ pcl::octree::OctreePointCloud maxZ = max_pt.z + minValue; // generate bit masks for octree - defineBoundingBox (minX, minY, minZ, maxX, maxY, maxZ); + defineBoundingBox(minX, minY, minZ, maxX, maxY, maxZ); } ////////////////////////////////////////////////////////////////////////////////////////////// -template void -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) +template +void +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->leaf_count_ == 0); + assert(this->leaf_count_ == 0); - assert (max_x_arg >= min_x_arg); - assert (max_y_arg >= min_y_arg); - assert (max_z_arg >= min_z_arg); + assert(max_x_arg >= min_x_arg); + assert(max_y_arg >= min_y_arg); + assert(max_z_arg >= min_z_arg); min_x_ = min_x_arg; max_x_ = max_x_arg; @@ -318,31 +387,37 @@ pcl::octree::OctreePointCloud min_z_ = min_z_arg; max_z_ = max_z_arg; - min_x_ = std::min (min_x_, max_x_); - min_y_ = std::min (min_y_, max_y_); - min_z_ = std::min (min_z_, max_z_); + min_x_ = std::min(min_x_, max_x_); + min_y_ = std::min(min_y_, max_y_); + min_z_ = std::min(min_z_, max_z_); - max_x_ = std::max (min_x_, max_x_); - max_y_ = std::max (min_y_, max_y_); - max_z_ = std::max (min_z_, max_z_); + max_x_ = std::max(min_x_, max_x_); + max_y_ = std::max(min_y_, max_y_); + max_z_ = std::max(min_z_, max_z_); // generate bit masks for octree - getKeyBitSize (); + getKeyBitSize(); bounding_box_defined_ = true; } ////////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::octree::OctreePointCloud::defineBoundingBox ( - const double max_x_arg, const double max_y_arg, const double max_z_arg) +template +void +pcl::octree::OctreePointCloud:: + defineBoundingBox(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->leaf_count_ == 0); + assert(this->leaf_count_ == 0); - assert (max_x_arg >= 0.0f); - assert (max_y_arg >= 0.0f); - assert (max_z_arg >= 0.0f); + assert(max_x_arg >= 0.0f); + assert(max_y_arg >= 0.0f); + assert(max_z_arg >= 0.0f); min_x_ = 0.0f; max_x_ = max_x_arg; @@ -353,28 +428,33 @@ pcl::octree::OctreePointCloud min_z_ = 0.0f; max_z_ = max_z_arg; - min_x_ = std::min (min_x_, max_x_); - min_y_ = std::min (min_y_, max_y_); - min_z_ = std::min (min_z_, max_z_); + min_x_ = std::min(min_x_, max_x_); + min_y_ = std::min(min_y_, max_y_); + min_z_ = std::min(min_z_, max_z_); - max_x_ = std::max (min_x_, max_x_); - max_y_ = std::max (min_y_, max_y_); - max_z_ = std::max (min_z_, max_z_); + max_x_ = std::max(min_x_, max_x_); + max_y_ = std::max(min_y_, max_y_); + max_z_ = std::max(min_z_, max_z_); // generate bit masks for octree - getKeyBitSize (); + getKeyBitSize(); bounding_box_defined_ = true; } ////////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::octree::OctreePointCloud::defineBoundingBox (const double cubeLen_arg) +template +void +pcl::octree::OctreePointCloud:: + defineBoundingBox(const double cubeLen_arg) { // bounding box cannot be changed once the octree contains elements - assert (this->leaf_count_ == 0); + assert(this->leaf_count_ == 0); - assert (cubeLen_arg >= 0.0f); + assert(cubeLen_arg >= 0.0f); min_x_ = 0.0f; max_x_ = cubeLen_arg; @@ -385,25 +465,33 @@ pcl::octree::OctreePointCloud min_z_ = 0.0f; max_z_ = cubeLen_arg; - min_x_ = std::min (min_x_, max_x_); - min_y_ = std::min (min_y_, max_y_); - min_z_ = std::min (min_z_, max_z_); + min_x_ = std::min(min_x_, max_x_); + min_y_ = std::min(min_y_, max_y_); + min_z_ = std::min(min_z_, max_z_); - max_x_ = std::max (min_x_, max_x_); - max_y_ = std::max (min_y_, max_y_); - max_z_ = std::max (min_z_, max_z_); + max_x_ = std::max(min_x_, max_x_); + max_y_ = std::max(min_y_, max_y_); + max_z_ = std::max(min_z_, max_z_); // generate bit masks for octree - getKeyBitSize (); + getKeyBitSize(); bounding_box_defined_ = true; } ////////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::octree::OctreePointCloud::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 +template +void +pcl::octree::OctreePointCloud:: + 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 { min_x_arg = min_x_; min_y_arg = min_y_; @@ -414,18 +502,20 @@ pcl::octree::OctreePointCloud max_z_arg = max_z_; } - ////////////////////////////////////////////////////////////////////////////////////////////// -template +template void -pcl::octree::OctreePointCloud::adoptBoundingBoxToPoint (const PointT& point_idx_arg) +pcl::octree::OctreePointCloud:: + adoptBoundingBoxToPoint(const PointT& point_idx_arg) { - const float minValue = std::numeric_limits::epsilon (); + const float minValue = std::numeric_limits::epsilon(); // increase octree size until point fits into bounding box - while (true) - { + while (true) { bool bLowerBoundViolationX = (point_idx_arg.x < min_x_); bool bLowerBoundViolationY = (point_idx_arg.y < min_y_); bool bLowerBoundViolationZ = (point_idx_arg.z < min_z_); @@ -435,30 +525,31 @@ pcl::octree::OctreePointCloud bool bUpperBoundViolationZ = (point_idx_arg.z >= max_z_); // do we violate any bounds? - if (bLowerBoundViolationX || bLowerBoundViolationY || bLowerBoundViolationZ || bUpperBoundViolationX - || bUpperBoundViolationY || bUpperBoundViolationZ || (!bounding_box_defined_) ) - { + if (bLowerBoundViolationX || bLowerBoundViolationY || bLowerBoundViolationZ || + bUpperBoundViolationX || bUpperBoundViolationY || bUpperBoundViolationZ || + (!bounding_box_defined_)) { - if (bounding_box_defined_) - { + if (bounding_box_defined_) { double octreeSideLen; unsigned char child_idx; - // octree not empty - we add another tree level and thus increase its size by a factor of 2*2*2 - child_idx = static_cast (((!bUpperBoundViolationX) << 2) | ((!bUpperBoundViolationY) << 1) - | ((!bUpperBoundViolationZ))); + // octree not empty - we add another tree level and thus increase its size by a + // factor of 2*2*2 + child_idx = static_cast(((!bUpperBoundViolationX) << 2) | + ((!bUpperBoundViolationY) << 1) | + ((!bUpperBoundViolationZ))); BranchNode* newRootBranch; newRootBranch = new BranchNode(); this->branch_count_++; - this->setBranchChildPtr (*newRootBranch, child_idx, this->root_node_); + this->setBranchChildPtr(*newRootBranch, child_idx, this->root_node_); this->root_node_ = newRootBranch; - octreeSideLen = static_cast (1 << this->octree_depth_) * resolution_; + octreeSideLen = static_cast(1 << this->octree_depth_) * resolution_; if (!bUpperBoundViolationX) min_x_ -= octreeSideLen; @@ -471,20 +562,19 @@ pcl::octree::OctreePointCloud // configure tree depth of octree this->octree_depth_++; - this->setTreeDepth (this->octree_depth_); + this->setTreeDepth(this->octree_depth_); // recalculate bounding box width - octreeSideLen = static_cast (1 << this->octree_depth_) * resolution_ - minValue; + octreeSideLen = + static_cast(1 << this->octree_depth_) * resolution_ - minValue; // increase octree bounding box 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 - { + else { // octree is empty - we set the center of the bounding box to our first pixel this->min_x_ = point_idx_arg.x - this->resolution_ / 2; this->min_y_ = point_idx_arg.y - this->resolution_ / 2; @@ -494,11 +584,10 @@ pcl::octree::OctreePointCloud this->max_y_ = point_idx_arg.y + this->resolution_ / 2; this->max_z_ = point_idx_arg.z + this->resolution_ / 2; - getKeyBitSize (); + getKeyBitSize(); bounding_box_defined_ = true; } - } else // no bound violations anymore - leave while loop @@ -507,16 +596,23 @@ pcl::octree::OctreePointCloud } ////////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::octree::OctreePointCloud::expandLeafNode (LeafNode* leaf_node, BranchNode* parent_branch, unsigned char child_idx, unsigned int depth_mask) +template +void +pcl::octree::OctreePointCloud:: + expandLeafNode(LeafNode* leaf_node, + BranchNode* parent_branch, + unsigned char child_idx, + unsigned int depth_mask) { - if (depth_mask) - { + if (depth_mask) { // get amount of objects in leaf container - std::size_t leaf_obj_count = (*leaf_node)->getSize (); + std::size_t leaf_obj_count = (*leaf_node)->getSize(); - // copy leaf data + // copy leaf data std::vector leafIndices; leafIndices.reserve(leaf_obj_count); @@ -524,90 +620,101 @@ pcl::octree::OctreePointCloud // delete current leaf node this->deleteBranchChild(*parent_branch, child_idx); - this->leaf_count_ --; + this->leaf_count_--; // create new branch node - BranchNode* childBranch = this->createBranchChild (*parent_branch, child_idx); - this->branch_count_ ++; + BranchNode* childBranch = this->createBranchChild(*parent_branch, child_idx); + this->branch_count_++; // add data to new branch OctreeKey new_index_key; - for (const int &leafIndex : leafIndices) - { + for (const int& leafIndex : leafIndices) { const PointT& point_from_index = input_->points[leafIndex]; // generate key - genOctreeKeyforPoint (point_from_index, new_index_key); + genOctreeKeyforPoint(point_from_index, new_index_key); LeafNode* newLeaf; BranchNode* newBranchParent; - this->createLeafRecursive (new_index_key, depth_mask, childBranch, newLeaf, newBranchParent); + this->createLeafRecursive( + new_index_key, depth_mask, childBranch, newLeaf, newBranchParent); (*newLeaf)->addPointIndex(leafIndex); } } - - } - ////////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::octree::OctreePointCloud::addPointIdx (const int point_idx_arg) +template +void +pcl::octree::OctreePointCloud:: + addPointIdx(const int point_idx_arg) { OctreeKey key; - assert (point_idx_arg < static_cast (input_->points.size ())); + assert(point_idx_arg < static_cast(input_->points.size())); const PointT& point = input_->points[point_idx_arg]; // make sure bounding box is big enough - adoptBoundingBoxToPoint (point); + adoptBoundingBoxToPoint(point); // generate key - genOctreeKeyforPoint (point, key); + genOctreeKeyforPoint(point, key); LeafNode* leaf_node; BranchNode* 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); + 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) - { + if (this->dynamic_depth_enabled_ && depth_mask) { // get amount of objects in leaf container - std::size_t leaf_obj_count = (*leaf_node)->getSize (); + std::size_t leaf_obj_count = (*leaf_node)->getSize(); - while (leaf_obj_count>=max_objs_per_leaf_ && depth_mask) - { + while (leaf_obj_count >= max_objs_per_leaf_ && depth_mask) { // index to branch child - unsigned char child_idx = key.getChildIdxWithDepthMask (depth_mask*2); + unsigned char child_idx = key.getChildIdxWithDepthMask(depth_mask * 2); - expandLeafNode (leaf_node, - parent_branch_of_leaf_node, - child_idx, - depth_mask); + expandLeafNode(leaf_node, parent_branch_of_leaf_node, child_idx, depth_mask); - depth_mask = this->createLeafRecursive (key, this->depth_mask_ ,this->root_node_, leaf_node, parent_branch_of_leaf_node); - leaf_obj_count = (*leaf_node)->getSize (); + depth_mask = this->createLeafRecursive(key, + this->depth_mask_, + this->root_node_, + leaf_node, + parent_branch_of_leaf_node); + leaf_obj_count = (*leaf_node)->getSize(); } - } - (*leaf_node)->addPointIndex (point_idx_arg); + (*leaf_node)->addPointIndex(point_idx_arg); } ////////////////////////////////////////////////////////////////////////////////////////////// -template const PointT& -pcl::octree::OctreePointCloud::getPointByIndex (const unsigned int index_arg) const +template +const PointT& +pcl::octree::OctreePointCloud:: + getPointByIndex(const unsigned int index_arg) const { // retrieve point from input cloud - assert (index_arg < static_cast (input_->points.size ())); + assert(index_arg < static_cast(input_->points.size())); return (this->input_->points[index_arg]); } ////////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::octree::OctreePointCloud::getKeyBitSize () +template +void +pcl::octree::OctreePointCloud:: + getKeyBitSize() { unsigned int max_voxels; @@ -620,22 +727,27 @@ pcl::octree::OctreePointCloud const float minValue = std::numeric_limits::epsilon(); // find maximum key values for x, y, z - max_key_x = static_cast (std::ceil ((max_x_ - min_x_ - minValue) / resolution_)); - max_key_y = static_cast (std::ceil ((max_y_ - min_y_ - minValue) / resolution_)); - max_key_z = static_cast (std::ceil ((max_z_ - min_z_ - minValue) / resolution_)); + max_key_x = + static_cast(std::ceil((max_x_ - min_x_ - minValue) / resolution_)); + max_key_y = + static_cast(std::ceil((max_y_ - min_y_ - minValue) / resolution_)); + max_key_z = + static_cast(std::ceil((max_z_ - min_z_ - minValue) / resolution_)); // find maximum amount of keys - max_voxels = std::max (std::max (std::max (max_key_x, max_key_y), max_key_z), static_cast (2)); - + max_voxels = std::max(std::max(std::max(max_key_x, max_key_y), max_key_z), + static_cast(2)); // tree depth == amount of bits of max_voxels - this->octree_depth_ = std::max ((std::min (static_cast (OctreeKey::maxDepth), static_cast (std::ceil (std::log2 (max_voxels) - minValue)))), - static_cast (0)); + this->octree_depth_ = + std::max((std::min(static_cast(OctreeKey::maxDepth), + static_cast( + std::ceil(std::log2(max_voxels) - minValue)))), + static_cast(0)); - octree_side_len = static_cast (1 << this->octree_depth_) * resolution_; + octree_side_len = static_cast(1 << this->octree_depth_) * resolution_; - if (this->leaf_count_ == 0) - { + if (this->leaf_count_ == 0) { double octree_oversize_x; double octree_oversize_y; double octree_oversize_z; @@ -644,133 +756,188 @@ pcl::octree::OctreePointCloud octree_oversize_y = (octree_side_len - (max_y_ - min_y_)) / 2.0; octree_oversize_z = (octree_side_len - (max_z_ - min_z_)) / 2.0; - assert (octree_oversize_x > -minValue); - assert (octree_oversize_y > -minValue); - assert (octree_oversize_z > -minValue); + assert(octree_oversize_x > -minValue); + assert(octree_oversize_y > -minValue); + assert(octree_oversize_z > -minValue); - if (octree_oversize_x > minValue) - { + if (octree_oversize_x > minValue) { min_x_ -= octree_oversize_x; max_x_ += octree_oversize_x; } - if (octree_oversize_y > minValue) - { + if (octree_oversize_y > minValue) { min_y_ -= octree_oversize_y; max_y_ += octree_oversize_y; } - if (octree_oversize_z > minValue) - { + if (octree_oversize_z > minValue) { min_z_ -= octree_oversize_z; max_z_ += octree_oversize_z; } } - else - { + else { 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->octree_depth_); - + // configure tree depth of octree + this->setTreeDepth(this->octree_depth_); } ////////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::octree::OctreePointCloud::genOctreeKeyforPoint (const PointT& point_arg, - OctreeKey & key_arg) const - { - // calculate integer key for point coordinates - 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_); - - assert (key_arg.x <= this->max_key_.x); - assert (key_arg.y <= this->max_key_.y); - assert (key_arg.z <= this->max_key_.z); - } +template +void +pcl::octree::OctreePointCloud:: + genOctreeKeyforPoint(const PointT& point_arg, OctreeKey& key_arg) const +{ + // calculate integer key for point coordinates + 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_); + + assert(key_arg.x <= this->max_key_.x); + assert(key_arg.y <= this->max_key_.y); + assert(key_arg.z <= this->max_key_.z); +} ////////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::octree::OctreePointCloud::genOctreeKeyforPoint ( - const double point_x_arg, const double point_y_arg, - const double point_z_arg, OctreeKey & key_arg) const +template +void +pcl::octree::OctreePointCloud:: + genOctreeKeyforPoint(const double point_x_arg, + const double point_y_arg, + const double point_z_arg, + OctreeKey& key_arg) const { PointT temp_point; - temp_point.x = static_cast (point_x_arg); - temp_point.y = static_cast (point_y_arg); - temp_point.z = static_cast (point_z_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 (temp_point, key_arg); + genOctreeKeyforPoint(temp_point, key_arg); } ////////////////////////////////////////////////////////////////////////////////////////////// -template bool -pcl::octree::OctreePointCloud::genOctreeKeyForDataT (const int& data_arg, OctreeKey & key_arg) const +template +bool +pcl::octree::OctreePointCloud:: + genOctreeKeyForDataT(const int& data_arg, OctreeKey& key_arg) const { - const PointT temp_point = getPointByIndex (data_arg); + const PointT temp_point = getPointByIndex(data_arg); // generate key for point - genOctreeKeyforPoint (temp_point, key_arg); + genOctreeKeyforPoint(temp_point, key_arg); return (true); } ////////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::octree::OctreePointCloud::genLeafNodeCenterFromOctreeKey (const OctreeKey & key, PointT & point) const +template +void +pcl::octree::OctreePointCloud:: + 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->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_); + 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 tree_depth_arg, - PointT& point_arg) const +template +void +pcl::octree::OctreePointCloud:: + genVoxelCenterFromOctreeKey(const OctreeKey& key_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->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_); + 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 tree_depth_arg, - Eigen::Vector3f &min_pt, - Eigen::Vector3f &max_pt) const +template +void +pcl::octree::OctreePointCloud:: + genVoxelBoundsFromOctreeKey(const OctreeKey& key_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->octree_depth_ - tree_depth_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->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->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_); + 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->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 tree_depth_arg) const +template +double +pcl::octree::OctreePointCloud:: + getVoxelSquaredSideLen(unsigned int tree_depth_arg) const { double side_len; // side length of the voxel cube increases exponentially with the octree depth - side_len = this->resolution_ * static_cast(1 << (this->octree_depth_ - tree_depth_arg)); + side_len = this->resolution_ * + static_cast(1 << (this->octree_depth_ - tree_depth_arg)); // squared voxel side length side_len *= side_len; @@ -779,30 +946,38 @@ pcl::octree::OctreePointCloud } ////////////////////////////////////////////////////////////////////////////////////////////// -template double -pcl::octree::OctreePointCloud::getVoxelSquaredDiameter (unsigned int tree_depth_arg) const +template +double +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 (tree_depth_arg) * 3); + return (getVoxelSquaredSideLen(tree_depth_arg) * 3); } ////////////////////////////////////////////////////////////////////////////////////////////// -template int -pcl::octree::OctreePointCloud::getOccupiedVoxelCentersRecursive ( - const BranchNode* node_arg, - const OctreeKey& key_arg, - AlignedPointTVector &voxel_center_list_arg) const +template +int +pcl::octree::OctreePointCloud:: + getOccupiedVoxelCentersRecursive(const BranchNode* node_arg, + const OctreeKey& key_arg, + AlignedPointTVector& voxel_center_list_arg) const { int voxel_count = 0; // iterate over all children - for (unsigned char child_idx = 0; child_idx < 8; child_idx++) - { - if (!this->branchHasChild (*node_arg, child_idx)) + for (unsigned char child_idx = 0; child_idx < 8; child_idx++) { + if (!this->branchHasChild(*node_arg, child_idx)) continue; - const OctreeNode * child_node; - child_node = this->getBranchChildPtr (*node_arg, child_idx); + const OctreeNode* child_node; + child_node = this->getBranchChildPtr(*node_arg, child_idx); // generate new key for current branch voxel OctreeKey new_key; @@ -810,38 +985,72 @@ pcl::octree::OctreePointCloud new_key.y = (key_arg.y << 1) | (!!(child_idx & (1 << 1))); new_key.z = (key_arg.z << 1) | (!!(child_idx & (1 << 0))); - switch (child_node->getNodeType ()) - { - case BRANCH_NODE: - { - // recursively proceed with indexed child branch - voxel_count += getOccupiedVoxelCentersRecursive (static_cast (child_node), new_key, voxel_center_list_arg); - break; - } - case LEAF_NODE: - { - PointT new_point; + switch (child_node->getNodeType()) { + case BRANCH_NODE: { + // recursively proceed with indexed child branch + voxel_count += getOccupiedVoxelCentersRecursive( + static_cast(child_node), new_key, voxel_center_list_arg); + break; + } + case LEAF_NODE: { + PointT new_point; - genLeafNodeCenterFromOctreeKey (new_key, new_point); - voxel_center_list_arg.push_back (new_point); + genLeafNodeCenterFromOctreeKey(new_key, new_point); + voxel_center_list_arg.push_back(new_point); - voxel_count++; - break; - } - default: - break; + voxel_count++; + break; + } + default: + break; } } return (voxel_count); } -#define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithLeafDataTVector(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud >; -#define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithLeafDataTVector(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud >; - -#define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithLeafDataT(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud >; -#define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithLeafDataT(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud >; - -#define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithEmptyLeaf(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud >; -#define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithEmptyLeaf(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud >; +#define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithLeafDataTVector(T) \ + template class PCL_EXPORTS pcl::octree::OctreePointCloud< \ + T, \ + pcl::octree::OctreeContainerPointIndices, \ + pcl::octree::OctreeContainerEmpty, \ + pcl::octree::OctreeBase>; +#define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithLeafDataTVector(T) \ + template class PCL_EXPORTS pcl::octree::OctreePointCloud< \ + T, \ + pcl::octree::OctreeContainerPointIndices, \ + pcl::octree::OctreeContainerEmpty, \ + pcl::octree::Octree2BufBase>; + +#define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithLeafDataT(T) \ + template class PCL_EXPORTS pcl::octree::OctreePointCloud< \ + T, \ + pcl::octree::OctreeContainerPointIndex, \ + pcl::octree::OctreeContainerEmpty, \ + pcl::octree::OctreeBase>; +#define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithLeafDataT(T) \ + template class PCL_EXPORTS pcl::octree::OctreePointCloud< \ + T, \ + pcl::octree::OctreeContainerPointIndex, \ + pcl::octree::OctreeContainerEmpty, \ + pcl::octree::Octree2BufBase>; + +#define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithEmptyLeaf(T) \ + template class PCL_EXPORTS pcl::octree::OctreePointCloud< \ + T, \ + pcl::octree::OctreeContainerEmpty, \ + pcl::octree::OctreeContainerEmpty, \ + pcl::octree::OctreeBase>; +#define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithEmptyLeaf(T) \ + template class PCL_EXPORTS pcl::octree::OctreePointCloud< \ + T, \ + pcl::octree::OctreeContainerEmpty, \ + pcl::octree::OctreeContainerEmpty, \ + pcl::octree::Octree2BufBase>; #endif /* OCTREE_POINTCLOUD_HPP_ */ diff --git a/octree/include/pcl/octree/impl/octree_pointcloud_adjacency.hpp b/octree/include/pcl/octree/impl/octree_pointcloud_adjacency.hpp index ecdc37f7..adf3ac5c 100644 --- a/octree/include/pcl/octree/impl/octree_pointcloud_adjacency.hpp +++ b/octree/include/pcl/octree/impl/octree_pointcloud_adjacency.hpp @@ -38,8 +38,8 @@ #ifndef PCL_OCTREE_POINTCLOUD_ADJACENCY_HPP_ #define PCL_OCTREE_POINTCLOUD_ADJACENCY_HPP_ -#include #include +#include /* * OctreePointCloudAdjacency is not precompiled, since it's used in other * parts of PCL with custom LeafContainers. So if PCL_NO_PRECOMPILE is NOT @@ -50,28 +50,35 @@ #include ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -template -pcl::octree::OctreePointCloudAdjacency::OctreePointCloudAdjacency (const double resolution_arg) -: OctreePointCloud > (resolution_arg) -{ - -} +template +pcl::octree::OctreePointCloudAdjacency:: + OctreePointCloudAdjacency(const double resolution_arg) +: OctreePointCloud>(resolution_arg) +{} //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::octree::OctreePointCloudAdjacency::addPointsFromInputCloud () +template +void +pcl::octree::OctreePointCloudAdjacency:: + addPointsFromInputCloud() { - //double t1,t2; - float minX = std::numeric_limits::max (), minY = std::numeric_limits::max (), minZ = std::numeric_limits::max (); - float maxX = -std::numeric_limits::max(), maxY = -std::numeric_limits::max(), maxZ = -std::numeric_limits::max(); - - for (std::size_t i = 0; i < input_->size (); ++i) - { - PointT temp (input_->points[i]); - if (transform_func_) //Search for point with - transform_func_ (temp); - if (!pcl::isFinite (temp)) //Check to make sure transform didn't make point not finite + // double t1,t2; + float minX = std::numeric_limits::max(), + minY = std::numeric_limits::max(), + minZ = std::numeric_limits::max(); + float maxX = -std::numeric_limits::max(), + maxY = -std::numeric_limits::max(), + maxZ = -std::numeric_limits::max(); + + for (std::size_t i = 0; i < input_->size(); ++i) { + PointT temp(input_->points[i]); + if (transform_func_) // Search for point with + transform_func_(temp); + if (!pcl::isFinite( + temp)) // Check to make sure transform didn't make point not finite continue; if (temp.x < minX) minX = temp.x; @@ -86,86 +93,97 @@ pcl::octree::OctreePointCloudAdjacency if (temp.z > maxZ) maxZ = temp.z; } - this->defineBoundingBox (minX, minY, minZ, maxX, maxY, maxZ); - - OctreePointCloud::addPointsFromInputCloud (); - - leaf_vector_.reserve (this->getLeafCount ()); - for (auto leaf_itr = this->leaf_depth_begin () ; leaf_itr != this->leaf_depth_end (); ++leaf_itr) - { - OctreeKey leaf_key = leaf_itr.getCurrentOctreeKey (); - LeafContainerT *leaf_container = &(leaf_itr.getLeafContainer ()); - - //Run the leaf's compute function - leaf_container->computeData (); - - computeNeighbors (leaf_key, leaf_container); - - leaf_vector_.push_back (leaf_container); + this->defineBoundingBox(minX, minY, minZ, maxX, maxY, maxZ); + + OctreePointCloud::addPointsFromInputCloud(); + + leaf_vector_.reserve(this->getLeafCount()); + for (auto leaf_itr = this->leaf_depth_begin(); leaf_itr != this->leaf_depth_end(); + ++leaf_itr) { + OctreeKey leaf_key = leaf_itr.getCurrentOctreeKey(); + LeafContainerT* leaf_container = &(leaf_itr.getLeafContainer()); + + // Run the leaf's compute function + leaf_container->computeData(); + + computeNeighbors(leaf_key, leaf_container); + + leaf_vector_.push_back(leaf_container); } - //Make sure our leaf vector is correctly sized - assert (leaf_vector_.size () == this->getLeafCount ()); + // Make sure our leaf vector is correctly sized + assert(leaf_vector_.size() == this->getLeafCount()); } ////////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::octree::OctreePointCloudAdjacency::genOctreeKeyforPoint (const PointT& point_arg,OctreeKey & key_arg) const +template +void +pcl::octree::OctreePointCloudAdjacency:: + genOctreeKeyforPoint(const PointT& point_arg, OctreeKey& key_arg) const { - if (transform_func_) - { - PointT temp (point_arg); - transform_func_ (temp); - // calculate integer key for transformed point coordinates - if (pcl::isFinite (temp)) //Make sure transformed point is finite - if it is not, it gets default key + if (transform_func_) { + PointT temp(point_arg); + transform_func_(temp); + // calculate integer key for transformed point coordinates + if (pcl::isFinite(temp)) // Make sure transformed point is finite - if it is not, it + // gets default key { - key_arg.x = static_cast ((temp.x - this->min_x_) / this->resolution_); - key_arg.y = static_cast ((temp.y - this->min_y_) / this->resolution_); - key_arg.z = static_cast ((temp.z - this->min_z_) / this->resolution_); + key_arg.x = + static_cast((temp.x - this->min_x_) / this->resolution_); + key_arg.y = + static_cast((temp.y - this->min_y_) / this->resolution_); + key_arg.z = + static_cast((temp.z - this->min_z_) / this->resolution_); } - else - { - key_arg = OctreeKey (); + else { + key_arg = OctreeKey(); } } - else - { + else { // calculate integer key for point coordinates - 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_); + 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::OctreePointCloudAdjacency::addPointIdx (const int pointIdx_arg) +template +void +pcl::octree::OctreePointCloudAdjacency:: + addPointIdx(const int pointIdx_arg) { OctreeKey key; - - assert (pointIdx_arg < static_cast (this->input_->points.size ())); - + + assert(pointIdx_arg < static_cast(this->input_->points.size())); + const PointT& point = this->input_->points[pointIdx_arg]; - if (!pcl::isFinite (point)) + if (!pcl::isFinite(point)) return; - + // generate key - this->genOctreeKeyforPoint (point, key); + this->genOctreeKeyforPoint(point, key); // add point to octree at key LeafContainerT* container = this->createLeaf(key); - container->addPoint (point); + container->addPoint(point); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::octree::OctreePointCloudAdjacency::computeNeighbors (OctreeKey &key_arg, LeafContainerT* leaf_container) -{ - //Make sure requested key is valid - if (key_arg.x > this->max_key_.x || key_arg.y > this->max_key_.y || key_arg.z > this->max_key_.z) - { - PCL_ERROR ("OctreePointCloudAdjacency::computeNeighbors Requested neighbors for invalid octree key\n"); +template +void +pcl::octree::OctreePointCloudAdjacency:: + computeNeighbors(OctreeKey& key_arg, LeafContainerT* leaf_container) +{ + // Make sure requested key is valid + if (key_arg.x > this->max_key_.x || key_arg.y > this->max_key_.y || + key_arg.z > this->max_key_.z) { + PCL_ERROR("OctreePointCloudAdjacency::computeNeighbors Requested neighbors for " + "invalid octree key\n"); return; } - + OctreeKey neighbor_key; int dx_min = (key_arg.x > 0) ? -1 : 0; int dy_min = (key_arg.y > 0) ? -1 : 0; @@ -173,145 +191,144 @@ pcl::octree::OctreePointCloudAdjacency int dx_max = (key_arg.x == this->max_key_.x) ? 0 : 1; int dy_max = (key_arg.y == this->max_key_.y) ? 0 : 1; int dz_max = (key_arg.z == this->max_key_.z) ? 0 : 1; - - for (int dx = dx_min; dx <= dx_max; ++dx) - { - for (int dy = dy_min; dy <= dy_max; ++dy) - { - for (int dz = dz_min; dz <= dz_max; ++dz) - { - neighbor_key.x = static_cast (key_arg.x + dx); - neighbor_key.y = static_cast (key_arg.y + dy); - neighbor_key.z = static_cast (key_arg.z + dz); - LeafContainerT *neighbor = this->findLeaf (neighbor_key); - if (neighbor) - { - leaf_container->addNeighbor (neighbor); + + for (int dx = dx_min; dx <= dx_max; ++dx) { + for (int dy = dy_min; dy <= dy_max; ++dy) { + for (int dz = dz_min; dz <= dz_max; ++dz) { + neighbor_key.x = static_cast(key_arg.x + dx); + neighbor_key.y = static_cast(key_arg.y + dy); + neighbor_key.z = static_cast(key_arg.z + dz); + LeafContainerT* neighbor = this->findLeaf(neighbor_key); + if (neighbor) { + leaf_container->addNeighbor(neighbor); } } } } } - - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -template LeafContainerT* -pcl::octree::OctreePointCloudAdjacency::getLeafContainerAtPoint ( - const PointT& point_arg) const +template +LeafContainerT* +pcl::octree::OctreePointCloudAdjacency:: + getLeafContainerAtPoint(const PointT& point_arg) const { OctreeKey key; LeafContainerT* leaf = nullptr; // generate key - this->genOctreeKeyforPoint (point_arg, key); - - leaf = this->findLeaf (key); - + this->genOctreeKeyforPoint(point_arg, key); + + leaf = this->findLeaf(key); + return leaf; } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::octree::OctreePointCloudAdjacency::computeVoxelAdjacencyGraph (VoxelAdjacencyList &voxel_adjacency_graph) +template +void +pcl::octree::OctreePointCloudAdjacency:: + computeVoxelAdjacencyGraph(VoxelAdjacencyList& voxel_adjacency_graph) { - //TODO Change this to use leaf centers, not centroids! - - voxel_adjacency_graph.clear (); - //Add a vertex for each voxel, store ids in map - std::map leaf_vertex_id_map; - for (typename OctreeAdjacencyT::LeafNodeDepthFirstIterator leaf_itr = this->leaf_depth_begin () ; leaf_itr != this->leaf_depth_end (); ++leaf_itr) - { - OctreeKey leaf_key = leaf_itr.getCurrentOctreeKey (); + // TODO Change this to use leaf centers, not centroids! + + voxel_adjacency_graph.clear(); + // Add a vertex for each voxel, store ids in map + std::map leaf_vertex_id_map; + for (typename OctreeAdjacencyT::LeafNodeDepthFirstIterator leaf_itr = + this->leaf_depth_begin(); + leaf_itr != this->leaf_depth_end(); + ++leaf_itr) { + OctreeKey leaf_key = leaf_itr.getCurrentOctreeKey(); PointT centroid_point; - this->genLeafNodeCenterFromOctreeKey (leaf_key, centroid_point); - VoxelID node_id = add_vertex (voxel_adjacency_graph); - + this->genLeafNodeCenterFromOctreeKey(leaf_key, centroid_point); + VoxelID node_id = add_vertex(voxel_adjacency_graph); + voxel_adjacency_graph[node_id] = centroid_point; - LeafContainerT* leaf_container = &(leaf_itr.getLeafContainer ()); + LeafContainerT* leaf_container = &(leaf_itr.getLeafContainer()); leaf_vertex_id_map[leaf_container] = node_id; } - - //Iterate through and add edges to adjacency graph - for ( typename std::vector::iterator leaf_itr = leaf_vector_.begin (); leaf_itr != leaf_vector_.end (); ++leaf_itr) - { - VoxelID u = (leaf_vertex_id_map.find (*leaf_itr))->second; + + // Iterate through and add edges to adjacency graph + for (typename std::vector::iterator leaf_itr = leaf_vector_.begin(); + leaf_itr != leaf_vector_.end(); + ++leaf_itr) { + VoxelID u = (leaf_vertex_id_map.find(*leaf_itr))->second; PointT p_u = voxel_adjacency_graph[u]; - for (auto neighbor_itr = (*leaf_itr)->cbegin (), neighbor_end = (*leaf_itr)->cend (); neighbor_itr != neighbor_end; ++neighbor_itr) - { + for (auto neighbor_itr = (*leaf_itr)->cbegin(), neighbor_end = (*leaf_itr)->cend(); + neighbor_itr != neighbor_end; + ++neighbor_itr) { LeafContainerT* neighbor_container = *neighbor_itr; EdgeID edge; bool edge_added; - VoxelID v = (leaf_vertex_id_map.find (neighbor_container))->second; - boost::tie (edge, edge_added) = add_edge (u,v,voxel_adjacency_graph); - + VoxelID v = (leaf_vertex_id_map.find(neighbor_container))->second; + boost::tie(edge, edge_added) = add_edge(u, v, voxel_adjacency_graph); + PointT p_v = voxel_adjacency_graph[v]; - float dist = (p_v.getVector3fMap () - p_u.getVector3fMap ()).norm (); + float dist = (p_v.getVector3fMap() - p_u.getVector3fMap()).norm(); voxel_adjacency_graph[edge] = dist; - } - } - } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -template bool -pcl::octree::OctreePointCloudAdjacency::testForOcclusion (const PointT& point_arg, const PointXYZ &camera_pos) +template +bool +pcl::octree::OctreePointCloudAdjacency:: + testForOcclusion(const PointT& point_arg, const PointXYZ& camera_pos) { OctreeKey key; - this->genOctreeKeyforPoint (point_arg, key); + this->genOctreeKeyforPoint(point_arg, key); // This code follows the method in Octree::PointCloud - Eigen::Vector3f sensor(camera_pos.x, - camera_pos.y, - camera_pos.z); - - Eigen::Vector3f leaf_centroid(static_cast ((static_cast (key.x) + 0.5f) * this->resolution_ + this->min_x_), - static_cast ((static_cast (key.y) + 0.5f) * this->resolution_ + this->min_y_), - static_cast ((static_cast (key.z) + 0.5f) * this->resolution_ + this->min_z_)); + Eigen::Vector3f sensor(camera_pos.x, camera_pos.y, camera_pos.z); + + Eigen::Vector3f leaf_centroid( + static_cast((static_cast(key.x) + 0.5f) * this->resolution_ + + this->min_x_), + static_cast((static_cast(key.y) + 0.5f) * this->resolution_ + + this->min_y_), + static_cast((static_cast(key.z) + 0.5f) * this->resolution_ + + this->min_z_)); Eigen::Vector3f direction = sensor - leaf_centroid; - - float norm = direction.norm (); - direction.normalize (); + + float norm = direction.norm(); + direction.normalize(); float precision = 1.0f; - const float step_size = static_cast (resolution_) * precision; - const int nsteps = std::max (1, static_cast (norm / step_size)); - + const float step_size = static_cast(resolution_) * precision; + const int nsteps = std::max(1, static_cast(norm / step_size)); + OctreeKey prev_key = key; // Walk along the line segment with small steps. Eigen::Vector3f p = leaf_centroid; PointT octree_p; - for (int i = 0; i < nsteps; ++i) - { - //Start at the leaf voxel, and move back towards sensor. + for (int i = 0; i < nsteps; ++i) { + // Start at the leaf voxel, and move back towards sensor. p += (direction * step_size); - - octree_p.x = p.x (); - octree_p.y = p.y (); - octree_p.z = p.z (); + + octree_p.x = p.x(); + octree_p.y = p.y(); + octree_p.z = p.z(); // std::cout << octree_p<< "\n"; OctreeKey key; - this->genOctreeKeyforPoint (octree_p, key); - + this->genOctreeKeyforPoint(octree_p, key); + // Not a new key, still the same voxel (starts at self). if ((key == prev_key)) continue; - + prev_key = key; - - LeafContainerT *leaf = this->findLeaf (key); - //If the voxel is occupied, there is a possible occlusion - if (leaf) - { - return true; + + LeafContainerT* leaf = this->findLeaf(key); + // If the voxel is occupied, there is a possible occlusion + if (leaf) { + return true; } } - - //If we didn't run into a voxel on the way to this camera, it can't be occluded. + + // If we didn't run into a voxel on the way to this camera, it can't be occluded. return false; - } -#define PCL_INSTANTIATE_OctreePointCloudAdjacency(T) template class PCL_EXPORTS pcl::octree::OctreePointCloudAdjacency; +#define PCL_INSTANTIATE_OctreePointCloudAdjacency(T) \ + template class PCL_EXPORTS pcl::octree::OctreePointCloudAdjacency; #endif - diff --git a/octree/include/pcl/octree/impl/octree_pointcloud_voxelcentroid.hpp b/octree/include/pcl/octree/impl/octree_pointcloud_voxelcentroid.hpp index 99a1afd7..ed3f132f 100644 --- a/octree/include/pcl/octree/impl/octree_pointcloud_voxelcentroid.hpp +++ b/octree/include/pcl/octree/impl/octree_pointcloud_voxelcentroid.hpp @@ -50,88 +50,92 @@ #include ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -template bool -pcl::octree::OctreePointCloudVoxelCentroid::getVoxelCentroidAtPoint ( - const PointT& point_arg, PointT& voxel_centroid_arg) const +template +bool +pcl::octree::OctreePointCloudVoxelCentroid:: + getVoxelCentroidAtPoint(const PointT& point_arg, PointT& voxel_centroid_arg) const { OctreeKey key; LeafContainerT* leaf = NULL; // generate key - genOctreeKeyforPoint (point_arg, key); + genOctreeKeyforPoint(point_arg, key); - leaf = this->findLeaf (key); + leaf = this->findLeaf(key); if (leaf) - leaf->getCentroid (voxel_centroid_arg); + leaf->getCentroid(voxel_centroid_arg); return (leaf != NULL); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -template std::size_t -pcl::octree::OctreePointCloudVoxelCentroid::getVoxelCentroids ( - typename OctreePointCloud::AlignedPointTVector &voxel_centroid_list_arg) const +template +std::size_t +pcl::octree::OctreePointCloudVoxelCentroid:: + getVoxelCentroids( + typename OctreePointCloud:: + AlignedPointTVector& voxel_centroid_list_arg) const { OctreeKey new_key; // reset output vector - voxel_centroid_list_arg.clear (); - voxel_centroid_list_arg.reserve (this->leaf_count_); + voxel_centroid_list_arg.clear(); + voxel_centroid_list_arg.reserve(this->leaf_count_); - getVoxelCentroidsRecursive (this->root_node_, 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 ()); + return (voxel_centroid_list_arg.size()); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::octree::OctreePointCloudVoxelCentroid::getVoxelCentroidsRecursive ( - const BranchNode* branch_arg, OctreeKey& key_arg, - typename OctreePointCloud::AlignedPointTVector &voxel_centroid_list_arg) const +template +void +pcl::octree::OctreePointCloudVoxelCentroid:: + getVoxelCentroidsRecursive( + const BranchNode* branch_arg, + OctreeKey& key_arg, + typename OctreePointCloud:: + AlignedPointTVector& voxel_centroid_list_arg) const { // iterate over all children - for (unsigned char child_idx = 0; child_idx < 8; child_idx++) - { + for (unsigned char child_idx = 0; child_idx < 8; child_idx++) { // if child exist - if (branch_arg->hasChild (child_idx)) - { + if (branch_arg->hasChild(child_idx)) { // add current branch voxel to key - key_arg.pushBranch (child_idx); + key_arg.pushBranch(child_idx); - OctreeNode *child_node = branch_arg->getChildPtr (child_idx); + OctreeNode* child_node = branch_arg->getChildPtr(child_idx); - switch (child_node->getNodeType ()) - { - case BRANCH_NODE: - { - // recursively proceed with indexed child branch - getVoxelCentroidsRecursive (static_cast (child_node), key_arg, voxel_centroid_list_arg); - break; - } - case LEAF_NODE: - { - PointT new_centroid; + switch (child_node->getNodeType()) { + case BRANCH_NODE: { + // recursively proceed with indexed child branch + getVoxelCentroidsRecursive(static_cast(child_node), + key_arg, + voxel_centroid_list_arg); + break; + } + case LEAF_NODE: { + PointT new_centroid; - LeafNode* container = static_cast (child_node); + LeafNode* container = static_cast(child_node); - container->getContainer().getCentroid (new_centroid); + container->getContainer().getCentroid(new_centroid); - voxel_centroid_list_arg.push_back (new_centroid); - break; - } - default: - break; - } + voxel_centroid_list_arg.push_back(new_centroid); + break; + } + default: + break; + } // pop current branch voxel from key - key_arg.popBranch (); + key_arg.popBranch(); } } } - -#define PCL_INSTANTIATE_OctreePointCloudVoxelCentroid(T) template class PCL_EXPORTS pcl::octree::OctreePointCloudVoxelCentroid; +#define PCL_INSTANTIATE_OctreePointCloudVoxelCentroid(T) \ + template class PCL_EXPORTS pcl::octree::OctreePointCloudVoxelCentroid; #endif - diff --git a/octree/include/pcl/octree/impl/octree_search.hpp b/octree/include/pcl/octree/impl/octree_search.hpp index 264bf71f..9a72e3dc 100644 --- a/octree/include/pcl/octree/impl/octree_search.hpp +++ b/octree/include/pcl/octree/impl/octree_search.hpp @@ -42,22 +42,23 @@ #include ////////////////////////////////////////////////////////////////////////////////////////////// -template bool -pcl::octree::OctreePointCloudSearch::voxelSearch (const PointT& point, - std::vector& point_idx_data) +template +bool +pcl::octree::OctreePointCloudSearch:: + voxelSearch(const PointT& point, std::vector& point_idx_data) { - assert (isFinite (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"); + assert(isFinite(point) && + "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"); OctreeKey key; bool b_success = false; // generate key - this->genOctreeKeyforPoint (point, key); + this->genOctreeKeyforPoint(point, key); - LeafContainerT* leaf = this->findLeaf (key); + LeafContainerT* leaf = this->findLeaf(key); - if (leaf) - { - (*leaf).getPointIndices (point_idx_data); + if (leaf) { + (*leaf).getPointIndices(point_idx_data); b_success = true; } @@ -65,29 +66,34 @@ pcl::octree::OctreePointCloudSearch::v } ////////////////////////////////////////////////////////////////////////////////////////////// -template bool -pcl::octree::OctreePointCloudSearch::voxelSearch (const int index, - std::vector& point_idx_data) +template +bool +pcl::octree::OctreePointCloudSearch:: + voxelSearch(const int index, std::vector& point_idx_data) { - const PointT search_point = this->getPointByIndex (index); - return (this->voxelSearch (search_point, point_idx_data)); + const PointT search_point = this->getPointByIndex(index); + return (this->voxelSearch(search_point, point_idx_data)); } ////////////////////////////////////////////////////////////////////////////////////////////// -template int -pcl::octree::OctreePointCloudSearch::nearestKSearch (const PointT &p_q, int k, - std::vector &k_indices, - std::vector &k_sqr_distances) +template +int +pcl::octree::OctreePointCloudSearch:: + nearestKSearch(const PointT& p_q, + int k, + std::vector& k_indices, + std::vector& k_sqr_distances) { - assert(this->leaf_count_>0); - assert (isFinite (p_q) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"); + assert(this->leaf_count_ > 0); + assert(isFinite(p_q) && + "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"); - k_indices.clear (); - k_sqr_distances.clear (); + k_indices.clear(); + k_sqr_distances.clear(); if (k < 1) return 0; - + prioPointQueueEntry point_entry; std::vector point_candidates; @@ -95,132 +101,157 @@ pcl::octree::OctreePointCloudSearch::n key.x = key.y = key.z = 0; // initialize smallest point distance in search with high value - double smallest_dist = std::numeric_limits::max (); + double smallest_dist = std::numeric_limits::max(); + + getKNearestNeighborRecursive( + p_q, k, this->root_node_, key, 1, smallest_dist, point_candidates); - getKNearestNeighborRecursive (p_q, k, this->root_node_, key, 1, smallest_dist, point_candidates); + unsigned int result_count = static_cast(point_candidates.size()); - unsigned int result_count = static_cast (point_candidates.size ()); + k_indices.resize(result_count); + k_sqr_distances.resize(result_count); - k_indices.resize (result_count); - k_sqr_distances.resize (result_count); - - for (unsigned int i = 0; i < result_count; ++i) - { - k_indices [i] = point_candidates [i].point_idx_; - k_sqr_distances [i] = point_candidates [i].point_distance_; + for (unsigned int i = 0; i < result_count; ++i) { + k_indices[i] = point_candidates[i].point_idx_; + k_sqr_distances[i] = point_candidates[i].point_distance_; } - return static_cast (k_indices.size ()); + return static_cast(k_indices.size()); } ////////////////////////////////////////////////////////////////////////////////////////////// -template int -pcl::octree::OctreePointCloudSearch::nearestKSearch (int index, int k, - std::vector &k_indices, - std::vector &k_sqr_distances) +template +int +pcl::octree::OctreePointCloudSearch:: + nearestKSearch(int index, + int k, + std::vector& k_indices, + std::vector& k_sqr_distances) { - const PointT search_point = this->getPointByIndex (index); - return (nearestKSearch (search_point, k, k_indices, k_sqr_distances)); + const PointT search_point = this->getPointByIndex(index); + return (nearestKSearch(search_point, k, k_indices, k_sqr_distances)); } ////////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::octree::OctreePointCloudSearch::approxNearestSearch (const PointT &p_q, - int &result_index, - float &sqr_distance) +template +void +pcl::octree::OctreePointCloudSearch:: + approxNearestSearch(const PointT& p_q, int& result_index, float& sqr_distance) { - assert(this->leaf_count_>0); - assert (isFinite (p_q) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"); - + 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->root_node_, key, 1, result_index, sqr_distance); + approxNearestSearchRecursive( + p_q, this->root_node_, key, 1, result_index, sqr_distance); return; } ////////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::octree::OctreePointCloudSearch::approxNearestSearch (int query_index, int &result_index, - float &sqr_distance) +template +void +pcl::octree::OctreePointCloudSearch:: + approxNearestSearch(int query_index, int& result_index, float& sqr_distance) { - const PointT search_point = this->getPointByIndex (query_index); + const PointT search_point = this->getPointByIndex(query_index); - return (approxNearestSearch (search_point, result_index, sqr_distance)); + return (approxNearestSearch(search_point, result_index, sqr_distance)); } ////////////////////////////////////////////////////////////////////////////////////////////// -template int -pcl::octree::OctreePointCloudSearch::radiusSearch (const PointT &p_q, const double radius, - std::vector &k_indices, - std::vector &k_sqr_distances, - unsigned int max_nn) const +template +int +pcl::octree::OctreePointCloudSearch:: + radiusSearch(const PointT& p_q, + const double radius, + std::vector& k_indices, + std::vector& k_sqr_distances, + unsigned int max_nn) const { - assert (isFinite (p_q) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"); + assert(isFinite(p_q) && + "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"); OctreeKey key; key.x = key.y = key.z = 0; - k_indices.clear (); - k_sqr_distances.clear (); + k_indices.clear(); + k_sqr_distances.clear(); - getNeighborsWithinRadiusRecursive (p_q, radius * radius, this->root_node_, key, 1, k_indices, k_sqr_distances, - max_nn); + getNeighborsWithinRadiusRecursive(p_q, + radius * radius, + this->root_node_, + key, + 1, + k_indices, + k_sqr_distances, + max_nn); - return (static_cast (k_indices.size ())); + return (static_cast(k_indices.size())); } ////////////////////////////////////////////////////////////////////////////////////////////// -template int -pcl::octree::OctreePointCloudSearch::radiusSearch (int index, const double radius, - std::vector &k_indices, - std::vector &k_sqr_distances, - unsigned int max_nn) const +template +int +pcl::octree::OctreePointCloudSearch:: + radiusSearch(int index, + const double radius, + std::vector& k_indices, + std::vector& k_sqr_distances, + unsigned int max_nn) const { - const PointT search_point = this->getPointByIndex (index); + const PointT search_point = this->getPointByIndex(index); - return (radiusSearch (search_point, radius, k_indices, k_sqr_distances, max_nn)); + return (radiusSearch(search_point, radius, k_indices, k_sqr_distances, max_nn)); } ////////////////////////////////////////////////////////////////////////////////////////////// -template int -pcl::octree::OctreePointCloudSearch::boxSearch (const Eigen::Vector3f &min_pt, - const Eigen::Vector3f &max_pt, - std::vector &k_indices) const +template +int +pcl::octree::OctreePointCloudSearch:: + boxSearch(const Eigen::Vector3f& min_pt, + const Eigen::Vector3f& max_pt, + std::vector& k_indices) const { OctreeKey key; key.x = key.y = key.z = 0; - k_indices.clear (); - - boxSearchRecursive (min_pt, max_pt, this->root_node_, key, 1, k_indices); + k_indices.clear(); - return (static_cast (k_indices.size ())); + boxSearchRecursive(min_pt, max_pt, this->root_node_, key, 1, k_indices); + return (static_cast(k_indices.size())); } ////////////////////////////////////////////////////////////////////////////////////////////// -template double -pcl::octree::OctreePointCloudSearch::getKNearestNeighborRecursive ( - 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 +template +double +pcl::octree::OctreePointCloudSearch:: + getKNearestNeighborRecursive( + 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 search_heap; - search_heap.resize (8); + search_heap.resize(8); OctreeKey new_key; double smallest_squared_dist = squared_search_radius; // get spatial voxel information - double voxelSquaredDiameter = this->getVoxelSquaredDiameter (tree_depth); + double voxelSquaredDiameter = this->getVoxelSquaredDiameter(tree_depth); // iterate over all children - for (unsigned char child_idx = 0; child_idx < 8; child_idx++) - { - if (this->branchHasChild (*node, child_idx)) - { + for (unsigned char child_idx = 0; child_idx < 8; child_idx++) { + if (this->branchHasChild(*node, child_idx)) { PointT voxel_center; search_heap[child_idx].key.x = (key.x << 1) + (!!(child_idx & (1 << 2))); @@ -228,100 +259,109 @@ pcl::octree::OctreePointCloudSearch::g search_heap[child_idx].key.z = (key.z << 1) + (!!(child_idx & (1 << 0))); // generate voxel center point for voxel at key - this->genVoxelCenterFromOctreeKey (search_heap[child_idx].key, tree_depth, voxel_center); + this->genVoxelCenterFromOctreeKey( + search_heap[child_idx].key, tree_depth, voxel_center); // generate new priority queue element - search_heap[child_idx].node = this->getBranchChildPtr (*node, child_idx); - search_heap[child_idx].point_distance = pointSquaredDist (voxel_center, point); + search_heap[child_idx].node = this->getBranchChildPtr(*node, child_idx); + search_heap[child_idx].point_distance = pointSquaredDist(voxel_center, point); } - else - { - search_heap[child_idx].point_distance = std::numeric_limits::infinity (); + else { + search_heap[child_idx].point_distance = std::numeric_limits::infinity(); } } - std::sort (search_heap.begin (), search_heap.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 (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_)) - { + // 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* child_node; // read from priority queue element - child_node = search_heap.back ().node; - new_key = search_heap.back ().key; + child_node = search_heap.back().node; + new_key = search_heap.back().key; - if (tree_depth < this->octree_depth_) - { + if (tree_depth < this->octree_depth_) { // we have not reached maximum tree depth - smallest_squared_dist = getKNearestNeighborRecursive (point, K, static_cast (child_node), new_key, tree_depth + 1, - smallest_squared_dist, point_candidates); + smallest_squared_dist = + getKNearestNeighborRecursive(point, + K, + static_cast(child_node), + new_key, + tree_depth + 1, + smallest_squared_dist, + point_candidates); } - else - { + else { // we reached leaf node level std::vector decoded_point_vector; - const LeafNode* child_leaf = static_cast (child_node); + const LeafNode* child_leaf = static_cast(child_node); // decode leaf node into decoded_point_vector - (*child_leaf)->getPointIndices (decoded_point_vector); + (*child_leaf)->getPointIndices(decoded_point_vector); // Linearly iterate over all decoded (unsorted) points - for (const int &point_index : decoded_point_vector) - { + for (const int& point_index : decoded_point_vector) { - const PointT& candidate_point = this->getPointByIndex (point_index); + const PointT& candidate_point = this->getPointByIndex(point_index); // calculate point distance to search point - float squared_dist = pointSquaredDist (candidate_point, point); + float squared_dist = pointSquaredDist(candidate_point, point); // check if a closer match is found - if (squared_dist < smallest_squared_dist) - { + if (squared_dist < smallest_squared_dist) { prioPointQueueEntry point_entry; point_entry.point_distance_ = squared_dist; point_entry.point_idx_ = point_index; - point_candidates.push_back (point_entry); + point_candidates.push_back(point_entry); } } - std::sort (point_candidates.begin (), point_candidates.end ()); + std::sort(point_candidates.begin(), point_candidates.end()); - if (point_candidates.size () > K) - point_candidates.resize (K); + if (point_candidates.size() > K) + point_candidates.resize(K); - if (point_candidates.size () == K) - smallest_squared_dist = point_candidates.back ().point_distance_; + if (point_candidates.size() == K) + smallest_squared_dist = point_candidates.back().point_distance_; } // pop element from priority queue - search_heap.pop_back (); + search_heap.pop_back(); } return (smallest_squared_dist); } ////////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::octree::OctreePointCloudSearch::getNeighborsWithinRadiusRecursive ( - const PointT & point, const double radiusSquared, const BranchNode* node, const OctreeKey& key, - unsigned int tree_depth, std::vector& k_indices, std::vector& k_sqr_distances, - unsigned int max_nn) const +template +void +pcl::octree::OctreePointCloudSearch:: + getNeighborsWithinRadiusRecursive(const PointT& point, + const double radiusSquared, + const BranchNode* node, + const OctreeKey& key, + unsigned int tree_depth, + std::vector& k_indices, + std::vector& k_sqr_distances, + unsigned int max_nn) const { // get spatial voxel information - double voxel_squared_diameter = this->getVoxelSquaredDiameter (tree_depth); + double voxel_squared_diameter = this->getVoxelSquaredDiameter(tree_depth); // iterate over all children - for (unsigned char child_idx = 0; child_idx < 8; child_idx++) - { - if (!this->branchHasChild (*node, child_idx)) + for (unsigned char child_idx = 0; child_idx < 8; child_idx++) { + if (!this->branchHasChild(*node, child_idx)) continue; const OctreeNode* child_node; - child_node = this->getBranchChildPtr (*node, child_idx); + child_node = this->getBranchChildPtr(*node, child_idx); OctreeKey new_key; PointT voxel_center; @@ -333,50 +373,53 @@ pcl::octree::OctreePointCloudSearch::g new_key.z = (key.z << 1) + (!!(child_idx & (1 << 0))); // generate voxel center point for voxel at key - this->genVoxelCenterFromOctreeKey (new_key, tree_depth, voxel_center); + this->genVoxelCenterFromOctreeKey(new_key, tree_depth, voxel_center); // calculate distance to search point - squared_dist = pointSquaredDist (static_cast (voxel_center), point); + squared_dist = pointSquaredDist(static_cast(voxel_center), point); // if distance is smaller than search radius - if (squared_dist + this->epsilon_ - <= voxel_squared_diameter / 4.0 + radiusSquared + sqrt (voxel_squared_diameter * radiusSquared)) - { + if (squared_dist + this->epsilon_ <= + voxel_squared_diameter / 4.0 + radiusSquared + + sqrt(voxel_squared_diameter * radiusSquared)) { - if (tree_depth < this->octree_depth_) - { + if (tree_depth < this->octree_depth_) { // we have not reached maximum tree depth - 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)) + 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; } - else - { + else { // we reached leaf node level - const LeafNode* child_leaf = static_cast (child_node); + const LeafNode* child_leaf = static_cast(child_node); std::vector decoded_point_vector; // decode leaf node into decoded_point_vector - (*child_leaf)->getPointIndices (decoded_point_vector); + (*child_leaf)->getPointIndices(decoded_point_vector); // Linearly iterate over all decoded (unsorted) points - for (const int &index : decoded_point_vector) - { - const PointT& candidate_point = this->getPointByIndex (index); + for (const int& index : decoded_point_vector) { + const PointT& candidate_point = this->getPointByIndex(index); // calculate point distance to search point - squared_dist = pointSquaredDist (candidate_point, point); + squared_dist = pointSquaredDist(candidate_point, point); // check if a match is found if (squared_dist > radiusSquared) continue; // add point to result vector - k_indices.push_back (index); - k_sqr_distances.push_back (squared_dist); + k_indices.push_back(index); + k_sqr_distances.push_back(squared_dist); - if (max_nn != 0 && k_indices.size () == static_cast (max_nn)) + if (max_nn != 0 && k_indices.size() == static_cast(max_nn)) return; } } @@ -385,13 +428,15 @@ pcl::octree::OctreePointCloudSearch::g } ////////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::octree::OctreePointCloudSearch::approxNearestSearchRecursive (const PointT & point, - const BranchNode* node, - const OctreeKey& key, - unsigned int tree_depth, - int& result_index, - float& sqr_distance) +template +void +pcl::octree::OctreePointCloudSearch:: + approxNearestSearchRecursive(const PointT& point, + const BranchNode* node, + const OctreeKey& key, + unsigned int tree_depth, + int& result_index, + float& sqr_distance) { OctreeKey minChildKey; OctreeKey new_key; @@ -399,14 +444,13 @@ pcl::octree::OctreePointCloudSearch::a const OctreeNode* child_node; // set minimum voxel distance to maximum value - double min_voxel_center_distance = std::numeric_limits::max (); + double min_voxel_center_distance = std::numeric_limits::max(); unsigned char min_child_idx = 0xFF; // iterate over all children - for (unsigned char child_idx = 0; child_idx < 8; child_idx++) - { - if (!this->branchHasChild (*node, child_idx)) + for (unsigned char child_idx = 0; child_idx < 8; child_idx++) { + if (!this->branchHasChild(*node, child_idx)) continue; PointT voxel_center; @@ -417,9 +461,9 @@ pcl::octree::OctreePointCloudSearch::a new_key.z = (key.z << 1) + (!!(child_idx & (1 << 0))); // generate voxel center point for voxel at key - this->genVoxelCenterFromOctreeKey (new_key, tree_depth, voxel_center); + this->genVoxelCenterFromOctreeKey(new_key, tree_depth, voxel_center); - voxelPointDist = pointSquaredDist (voxel_center, point); + voxelPointDist = pointSquaredDist(voxel_center, point); // search for child voxel with shortest distance to search point if (voxelPointDist >= min_voxel_center_distance) @@ -431,35 +475,36 @@ pcl::octree::OctreePointCloudSearch::a } // make sure we found at least one branch child - assert(min_child_idx<8); + assert(min_child_idx < 8); - child_node = this->getBranchChildPtr (*node, min_child_idx); + child_node = this->getBranchChildPtr(*node, min_child_idx); - if (tree_depth < this->octree_depth_) - { + if (tree_depth < this->octree_depth_) { // we have not reached maximum tree depth - approxNearestSearchRecursive (point, static_cast (child_node), minChildKey, tree_depth + 1, result_index, - sqr_distance); + approxNearestSearchRecursive(point, + static_cast(child_node), + minChildKey, + tree_depth + 1, + result_index, + sqr_distance); } - else - { + else { // we reached leaf node level std::vector decoded_point_vector; - const LeafNode* child_leaf = static_cast (child_node); + const LeafNode* child_leaf = static_cast(child_node); - double smallest_squared_dist = std::numeric_limits::max (); + double smallest_squared_dist = std::numeric_limits::max(); // decode leaf node into decoded_point_vector - (**child_leaf).getPointIndices (decoded_point_vector); + (**child_leaf).getPointIndices(decoded_point_vector); // Linearly iterate over all decoded (unsorted) points - for (const int &index : decoded_point_vector) - { - const PointT& candidate_point = this->getPointByIndex (index); + for (const int& index : decoded_point_vector) { + const PointT& candidate_point = this->getPointByIndex(index); // calculate point distance to search point - double squared_dist = pointSquaredDist (candidate_point, point); + double squared_dist = pointSquaredDist(candidate_point, point); // check if a closer match is found if (squared_dist >= smallest_squared_dist) @@ -467,34 +512,36 @@ pcl::octree::OctreePointCloudSearch::a result_index = index; smallest_squared_dist = squared_dist; - sqr_distance = static_cast (squared_dist); + sqr_distance = static_cast(squared_dist); } } } ////////////////////////////////////////////////////////////////////////////////////////////// -template float -pcl::octree::OctreePointCloudSearch::pointSquaredDist (const PointT & point_a, - const PointT & point_b) const +template +float +pcl::octree::OctreePointCloudSearch:: + pointSquaredDist(const PointT& point_a, const PointT& point_b) const { - return (point_a.getVector3fMap () - point_b.getVector3fMap ()).squaredNorm (); + return (point_a.getVector3fMap() - point_b.getVector3fMap()).squaredNorm(); } ////////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::octree::OctreePointCloudSearch::boxSearchRecursive (const Eigen::Vector3f &min_pt, - const Eigen::Vector3f &max_pt, - const BranchNode* node, - const OctreeKey& key, - unsigned int tree_depth, - std::vector& k_indices) const +template +void +pcl::octree::OctreePointCloudSearch:: + boxSearchRecursive(const Eigen::Vector3f& min_pt, + const Eigen::Vector3f& max_pt, + const BranchNode* node, + const OctreeKey& key, + unsigned int tree_depth, + std::vector& k_indices) const { // iterate over all children - for (unsigned char child_idx = 0; child_idx < 8; child_idx++) - { + for (unsigned char child_idx = 0; child_idx < 8; child_idx++) { const OctreeNode* child_node; - child_node = this->getBranchChildPtr (*node, child_idx); + child_node = this->getBranchChildPtr(*node, child_idx); if (!child_node) continue; @@ -509,44 +556,46 @@ pcl::octree::OctreePointCloudSearch::b Eigen::Vector3f lower_voxel_corner; Eigen::Vector3f upper_voxel_corner; // get voxel coordinates - this->genVoxelBoundsFromOctreeKey (new_key, tree_depth, lower_voxel_corner, upper_voxel_corner); + this->genVoxelBoundsFromOctreeKey( + new_key, tree_depth, lower_voxel_corner, upper_voxel_corner); // test if search region overlap with voxel space - 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 (!((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 (tree_depth < this->octree_depth_) - { + if (tree_depth < this->octree_depth_) { // we have not reached maximum tree depth - boxSearchRecursive (min_pt, max_pt, static_cast (child_node), new_key, tree_depth + 1, k_indices); + boxSearchRecursive(min_pt, + max_pt, + static_cast(child_node), + new_key, + tree_depth + 1, + k_indices); } - else - { + else { // we reached leaf node level std::vector decoded_point_vector; - bool bInBox; - const LeafNode* child_leaf = static_cast (child_node); + const LeafNode* child_leaf = static_cast(child_node); // decode leaf node into decoded_point_vector - (**child_leaf).getPointIndices (decoded_point_vector); + (**child_leaf).getPointIndices(decoded_point_vector); // Linearly iterate over all decoded (unsorted) points - for (const int &index : decoded_point_vector) - { - const PointT& candidate_point = this->getPointByIndex (index); + for (const int& index : decoded_point_vector) { + const PointT& candidate_point = this->getPointByIndex(index); // check if point falls within search box - 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)) ); + bool 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 (index); + k_indices.push_back(index); } } } @@ -554,70 +603,103 @@ pcl::octree::OctreePointCloudSearch::b } ////////////////////////////////////////////////////////////////////////////////////////////// -template int -pcl::octree::OctreePointCloudSearch::getIntersectedVoxelCenters ( - Eigen::Vector3f origin, Eigen::Vector3f direction, AlignedPointTVector &voxel_center_list, - int max_voxel_count) const +template +int +pcl::octree::OctreePointCloudSearch:: + getIntersectedVoxelCenters(Eigen::Vector3f origin, + Eigen::Vector3f direction, + AlignedPointTVector& voxel_center_list, + int max_voxel_count) const { OctreeKey key; key.x = key.y = key.z = 0; - voxel_center_list.clear (); + voxel_center_list.clear(); // Voxel child_idx remapping unsigned char a = 0; double min_x, min_y, min_z, max_x, max_y, max_z; - initIntersectedVoxel (origin, direction, min_x, min_y, min_z, max_x, max_y, max_z, a); - - if (std::max (std::max (min_x, min_y), min_z) < std::min (std::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); + initIntersectedVoxel(origin, direction, min_x, min_y, min_z, max_x, max_y, max_z, a); + + if (std::max(std::max(min_x, min_y), min_z) < std::min(std::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); } ////////////////////////////////////////////////////////////////////////////////////////////// -template int -pcl::octree::OctreePointCloudSearch::getIntersectedVoxelIndices ( - Eigen::Vector3f origin, Eigen::Vector3f direction, std::vector &k_indices, - int max_voxel_count) const +template +int +pcl::octree::OctreePointCloudSearch:: + getIntersectedVoxelIndices(Eigen::Vector3f origin, + Eigen::Vector3f direction, + std::vector& k_indices, + int max_voxel_count) const { OctreeKey key; key.x = key.y = key.z = 0; - k_indices.clear (); + k_indices.clear(); // Voxel child_idx remapping unsigned char a = 0; double min_x, min_y, min_z, max_x, max_y, max_z; - initIntersectedVoxel (origin, direction, min_x, min_y, min_z, max_x, max_y, max_z, a); - - if (std::max (std::max (min_x, min_y), min_z) < std::min (std::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); + initIntersectedVoxel(origin, direction, min_x, min_y, min_z, max_x, max_y, max_z, a); + + if (std::max(std::max(min_x, min_y), min_z) < std::min(std::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 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 +template +int +pcl::octree::OctreePointCloudSearch:: + 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 { 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 - if (node->getNodeType () == LEAF_NODE) - { + if (node->getNodeType() == LEAF_NODE) { PointT newPoint; - this->genLeafNodeCenterFromOctreeKey (key, newPoint); + this->genLeafNodeCenterFromOctreeKey(key, newPoint); - voxel_center_list.push_back (newPoint); + voxel_center_list.push_back(newPoint); return (1); } @@ -631,22 +713,21 @@ pcl::octree::OctreePointCloudSearch::g double mid_z = 0.5 * (min_z + max_z); // First voxel node ray will intersect - int curr_node = getFirstIntersectedNode (min_x, min_y, min_z, mid_x, mid_y, mid_z); + int curr_node = getFirstIntersectedNode(min_x, min_y, min_z, mid_x, mid_y, mid_z); // Child index, node and key unsigned char child_idx; - const OctreeNode *child_node; OctreeKey child_key; - do - { + do { if (curr_node != 0) - child_idx = static_cast (curr_node ^ a); + child_idx = static_cast(curr_node ^ a); else child_idx = a; // child_node == 0 if child_node doesn't exist - child_node = this->getBranchChildPtr (static_cast (*node), child_idx); + const OctreeNode* child_node = + this->getBranchChildPtr(static_cast(*node), child_idx); // Generate new key for current branch voxel child_key.x = (key.x << 1) | (!!(child_idx & (1 << 2))); @@ -657,84 +738,164 @@ pcl::octree::OctreePointCloudSearch::g // node intersected by the ray. Children that do not intersect will // not be traversed. - switch (curr_node) - { - case 0: - 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 (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 (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 (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 (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 (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 (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 (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; + switch (curr_node) { + case 0: + 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 (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 (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 (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 (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 (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 (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 (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 ((curr_node < 8) && (max_voxel_count <= 0 || voxel_count < max_voxel_count)); return (voxel_count); } ////////////////////////////////////////////////////////////////////////////////////////////// -template int -pcl::octree::OctreePointCloudSearch::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 max_voxel_count) const +template +int +pcl::octree::OctreePointCloudSearch:: + 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 max_voxel_count) const { 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 - if (node->getNodeType () == LEAF_NODE) - { - const LeafNode* leaf = static_cast (node); + if (node->getNodeType() == LEAF_NODE) { + const LeafNode* leaf = static_cast(node); // decode leaf node into k_indices - (*leaf)->getPointIndices (k_indices); + (*leaf)->getPointIndices(k_indices); return (1); } @@ -748,21 +909,20 @@ pcl::octree::OctreePointCloudSearch::g double mid_z = 0.5 * (min_z + max_z); // First voxel node ray will intersect - int curr_node = getFirstIntersectedNode (min_x, min_y, min_z, mid_x, mid_y, mid_z); + int curr_node = getFirstIntersectedNode(min_x, min_y, min_z, mid_x, mid_y, mid_z); // Child index, node and key unsigned char child_idx; - const OctreeNode *child_node; OctreeKey child_key; - do - { + do { if (curr_node != 0) - child_idx = static_cast (curr_node ^ a); + child_idx = static_cast(curr_node ^ a); else child_idx = a; // child_node == 0 if child_node doesn't exist - child_node = this->getBranchChildPtr (static_cast (*node), child_idx); + const OctreeNode* child_node = + this->getBranchChildPtr(static_cast(*node), child_idx); // Generate new key for current branch voxel child_key.x = (key.x << 1) | (!!(child_idx & (1 << 2))); child_key.y = (key.y << 1) | (!!(child_idx & (1 << 1))); @@ -771,69 +931,141 @@ pcl::octree::OctreePointCloudSearch::g // Recursively call each intersected child node, selecting the next // node intersected by the ray. Children that do not intersect will // not be traversed. - switch (curr_node) - { - case 0: - 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 (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 (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 (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 (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 (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 (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 (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; + switch (curr_node) { + case 0: + 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 (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 (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 (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 (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 (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 (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 (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 ((curr_node < 8) && (max_voxel_count <= 0 || voxel_count < max_voxel_count)); return (voxel_count); } -#define PCL_INSTANTIATE_OctreePointCloudSearch(T) template class PCL_EXPORTS pcl::octree::OctreePointCloudSearch; +#define PCL_INSTANTIATE_OctreePointCloudSearch(T) \ + template class PCL_EXPORTS pcl::octree::OctreePointCloudSearch; -#endif // PCL_OCTREE_SEARCH_IMPL_H_ +#endif // PCL_OCTREE_SEARCH_IMPL_H_ diff --git a/octree/include/pcl/octree/octree.h b/octree/include/pcl/octree/octree.h index b3578176..074ffbf9 100644 --- a/octree/include/pcl/octree/octree.h +++ b/octree/include/pcl/octree/octree.h @@ -38,17 +38,17 @@ #pragma once -#include #include +#include #include #include +#include +#include #include #include -#include #include -#include +#include #include -#include #include diff --git a/octree/include/pcl/octree/octree2buf_base.h b/octree/include/pcl/octree/octree2buf_base.h index eb5b4873..39157230 100644 --- a/octree/include/pcl/octree/octree2buf_base.h +++ b/octree/include/pcl/octree/octree2buf_base.h @@ -40,916 +40,969 @@ #include -#include #include -#include #include +#include +#include +#include + +namespace pcl { +namespace octree { + +template +class BufferedBranchNode : public OctreeNode { + +public: + /** \brief Empty constructor. */ + BufferedBranchNode() : OctreeNode() { reset(); } + + /** \brief Copy constructor. */ + BufferedBranchNode(const BufferedBranchNode& source) : OctreeNode() + { + *this = source; + } + + /** \brief Copy operator. */ + inline BufferedBranchNode& + operator=(const BufferedBranchNode& source_arg) + { + memset(child_node_array_, 0, sizeof(child_node_array_)); + + for (unsigned char b = 0; b < 2; ++b) + for (unsigned char 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); + } + + /** \brief Empty constructor. */ + ~BufferedBranchNode() {} + + /** \brief Method to perform a deep copy of the octree */ + BufferedBranchNode* + deepCopy() const override + { + return new BufferedBranchNode(*this); + } + + /** \brief Get child pointer in current branch node + * \param buffer_arg: buffer selector + * \param index_arg: index of child in node + * \return pointer to child node + */ + inline OctreeNode* + getChildPtr(unsigned char buffer_arg, unsigned char index_arg) const + { + assert((buffer_arg < 2) && (index_arg < 8)); + return child_node_array_[buffer_arg][index_arg]; + } + + /** \brief Set child pointer in current branch node + * \param buffer_arg: buffer selector + * \param index_arg: index of child in node + * \param newNode_arg: pointer to new child node + */ + inline void + setChildPtr(unsigned char buffer_arg, + unsigned char index_arg, + OctreeNode* newNode_arg) + { + assert((buffer_arg < 2) && (index_arg < 8)); + child_node_array_[buffer_arg][index_arg] = newNode_arg; + } + + /** \brief Check if branch is pointing to a particular child node + * \param buffer_arg: buffer selector + * \param index_arg: index of child in node + * \return "true" if pointer to child node exists; "false" otherwise + */ + inline bool + hasChild(unsigned char buffer_arg, unsigned char index_arg) const + { + assert((buffer_arg < 2) && (index_arg < 8)); + return (child_node_array_[buffer_arg][index_arg] != nullptr); + } + + /** \brief Get the type of octree node. Returns LEAVE_NODE type */ + node_type_t + getNodeType() const override + { + return BRANCH_NODE; + } + + /** \brief Reset branch node container for every branch buffer. */ + inline void + reset() + { + memset(&child_node_array_[0][0], 0, sizeof(OctreeNode*) * 8 * 2); + } + + /** \brief Get const pointer to container */ + const ContainerT* operator->() const { return &container_; } + + /** \brief Get pointer to container */ + ContainerT* operator->() { return &container_; } + + /** \brief Get const reference to container */ + const ContainerT& operator*() const { return container_; } + + /** \brief Get reference to container */ + ContainerT& operator*() { return container_; } + + /** \brief Get const reference to container */ + const ContainerT& + getContainer() const + { + return container_; + } + + /** \brief Get reference to container */ + ContainerT& + getContainer() + { + return container_; + } + + /** \brief Get const pointer to container */ + const ContainerT* + getContainerPtr() const + { + return &container_; + } + + /** \brief Get pointer to container */ + ContainerT* + getContainerPtr() + { + return &container_; + } + +protected: + ContainerT container_; + + OctreeNode* child_node_array_[2][8]; +}; + +/** \brief @b Octree double buffer class + * + * This octree implementation keeps two separate octree structures in memory + * which allows for differentially comparison of 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 The tree depth equates to the bit length of the voxel indices. + * \ingroup octree + * \author Julius Kammerl (julius@kammerl.de) + */ +template +class Octree2BufBase { + +public: + using OctreeT = Octree2BufBase; + + // iterators are friends + friend class OctreeIteratorBase; + friend class OctreeDepthFirstIterator; + friend class OctreeBreadthFirstIterator; + friend class OctreeLeafNodeDepthFirstIterator; + friend class OctreeLeafNodeBreadthFirstIterator; + + using BranchNode = BufferedBranchNode; + using LeafNode = OctreeLeafNode; + + using BranchContainer = BranchContainerT; + using LeafContainer = LeafContainerT; + + // Octree default iterators + using Iterator = OctreeDepthFirstIterator; + using ConstIterator = const OctreeDepthFirstIterator; + Iterator + begin(unsigned int max_depth_arg = 0) + { + return Iterator(this, max_depth_arg); + }; + const Iterator + end() + { + return Iterator(); + }; + + // Octree leaf node iterators + // The previous deprecated names + // LeafNodeIterator and ConstLeafNodeIterator are deprecated. + // Please use LeafNodeDepthFirstIterator and ConstLeafNodeDepthFirstIterator instead. + using LeafNodeIterator = OctreeLeafNodeDepthFirstIterator; + using ConstLeafNodeIterator = const OctreeLeafNodeDepthFirstIterator; + + PCL_DEPRECATED("use leaf_depth_begin() instead") + LeafNodeIterator + leaf_begin(unsigned int max_depth_arg = 0) + { + return LeafNodeIterator(this, max_depth_arg); + }; + + PCL_DEPRECATED("use leaf_depth_end() instead") const LeafNodeIterator leaf_end() + { + return LeafNodeIterator(); + }; + + // The currently valide names + using LeafNodeDepthFirstIterator = OctreeLeafNodeDepthFirstIterator; + using ConstLeafNodeDepthFirstIterator = + const OctreeLeafNodeDepthFirstIterator; + LeafNodeDepthFirstIterator + leaf_depth_begin(unsigned int max_depth_arg = 0) + { + return LeafNodeDepthFirstIterator(this, max_depth_arg); + }; + + const LeafNodeDepthFirstIterator + leaf_depth_end() + { + return LeafNodeDepthFirstIterator(); + }; + + // Octree depth-first iterators + using DepthFirstIterator = OctreeDepthFirstIterator; + using ConstDepthFirstIterator = const OctreeDepthFirstIterator; + DepthFirstIterator + depth_begin(unsigned int maxDepth_arg = 0) + { + return DepthFirstIterator(this, maxDepth_arg); + }; + const DepthFirstIterator + depth_end() + { + return DepthFirstIterator(); + }; + + // Octree breadth-first iterators + using BreadthFirstIterator = OctreeBreadthFirstIterator; + using ConstBreadthFirstIterator = const OctreeBreadthFirstIterator; + BreadthFirstIterator + breadth_begin(unsigned int max_depth_arg = 0) + { + return BreadthFirstIterator(this, max_depth_arg); + }; + const BreadthFirstIterator + breadth_end() + { + return BreadthFirstIterator(); + }; + + // Octree leaf node iterators + using LeafNodeBreadthIterator = OctreeLeafNodeBreadthFirstIterator; + using ConstLeafNodeBreadthIterator = + const OctreeLeafNodeBreadthFirstIterator; + + LeafNodeBreadthIterator + leaf_breadth_begin(unsigned int max_depth_arg = 0u) + { + return LeafNodeBreadthIterator(this, + max_depth_arg ? max_depth_arg : this->octree_depth_); + }; + + const LeafNodeBreadthIterator + leaf_breadth_end() + { + return LeafNodeBreadthIterator(this, 0, nullptr); + }; + + /** \brief Empty constructor. */ + Octree2BufBase(); + + /** \brief Empty deconstructor. */ + virtual ~Octree2BufBase(); + + /** \brief Copy constructor. */ + Octree2BufBase(const Octree2BufBase& source) + : 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_) + {} + + /** \brief Copy constructor. */ + inline Octree2BufBase& + operator=(const Octree2BufBase& source) + { + 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 max_voxel_index_arg: maximum amount of voxels per dimension + */ + void + setMaxVoxelIndex(unsigned int max_voxel_index_arg); + + /** \brief Set the maximum depth of the octree. + * \param depth_arg: maximum depth of octree + */ + void + setTreeDepth(unsigned int depth_arg); + + /** \brief Get the maximum depth of the octree. + * \return depth_arg: maximum depth of octree + */ + inline unsigned int + getTreeDepth() const + { + return this->octree_depth_; + } + + /** \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 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 idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg); + + /** \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 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 idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg); + + /** \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 idx_x_arg, + unsigned int idx_y_arg, + unsigned int idx_z_arg) const; + + /** \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 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 (leaf_count_); + } + + /** \brief Return the amount of existing branches in the octree. + * \return amount of branch nodes. + */ + inline std::size_t + getBranchCount() const + { + return (branch_count_); + } + + /** \brief Delete the octree structure and its leaf nodes. + */ + void + deleteTree(); + + /** \brief Delete octree structure of previous buffer. */ + inline void + deletePreviousBuffer() + { + treeCleanUpRecursive(root_node_); + } + + /** \brief Delete the octree structure in the current buffer. */ + inline void + deleteCurrentBuffer() + { + buffer_selector_ = !buffer_selector_; + treeCleanUpRecursive(root_node_); + leaf_count_ = 0; + } + + /** \brief Switch buffers and reset current octree structure. */ + void + switchBuffers(); + + /** \brief Serialize octree into a binary output vector describing its branch node + * structure. + * \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& 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 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& 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 leaf_container_vector_arg: vector of pointers to all LeafContainerT objects + * in the octree + */ + void + 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 leaf_container_vector_arg: vector of pointers to all LeafContainerT objects + * in the octree + */ + void + 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 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& 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 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& binary_tree_in_arg, + std::vector& leaf_container_vector_arg, + bool do_XOR_decoding_arg = false); + +protected: + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + // Protected octree methods based on octree keys + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + /** \brief Retrieve root node */ + OctreeNode* + getRootNode() const + { + return (this->root_node_); + } + + /** \brief Find leaf node + * \param key_arg: octree key addressing a leaf node. + * \return pointer to leaf container. If leaf node is not found, this pointer returns + * 0. + */ + inline LeafContainerT* + findLeaf(const OctreeKey& key_arg) const + { + LeafContainerT* result = nullptr; + findLeafRecursive(key_arg, depth_mask_, root_node_, result); + return result; + } + + /** \brief Create a leaf node. + * \note If the leaf node at the given octree node does not exist, it will be created + * and added to the tree. + * \param key_arg: octree key addressing a leaf node. + * \return pointer to an existing or created leaf container. + */ + inline LeafContainerT* + createLeaf(const OctreeKey& key_arg) + { + LeafNode* leaf_node; + BranchNode* leaf_node_parent; + + createLeafRecursive( + key_arg, depth_mask_, root_node_, leaf_node, leaf_node_parent, false); + + LeafContainerT* ret = leaf_node->getContainerPtr(); + + return ret; + } + + /** \brief Check if leaf doesn't exist in the octree + * \param key_arg: octree key addressing a leaf node. + * \return "true" if leaf node is found; "false" otherwise + */ + inline bool + existLeaf(const OctreeKey& key_arg) const + { + return (findLeaf(key_arg) != nullptr); + } + + /** \brief Remove leaf node from octree + * \param key_arg: octree key addressing a leaf node. + */ + inline void + removeLeaf(const OctreeKey& key_arg) + { + if (key_arg <= max_key_) { + deleteLeafRecursive(key_arg, depth_mask_, root_node_); + + // we changed the octree structure -> dirty + tree_dirty_flag_ = true; + } + } + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + // Branch node accessor inline functions + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + /** \brief Check if branch is pointing to a particular child node + * \param branch_arg: reference to octree branch class + * \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 child_idx_arg) const + { + // test occupancyByte for child existence + return (branch_arg.getChildPtr(buffer_selector_, child_idx_arg) != nullptr); + } + + /** \brief Retrieve a child node pointer for child node at child_idx. + * \param branch_arg: reference to octree branch class + * \param child_idx_arg: index to child node + * \return pointer to octree child node class + */ + inline OctreeNode* + getBranchChildPtr(const BranchNode& branch_arg, unsigned char child_idx_arg) const + { + 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 child_idx_arg: index to child node + * \param new_child_arg: pointer to new child node + */ + inline void + setBranchChildPtr(BranchNode& branch_arg, + unsigned char child_idx_arg, + OctreeNode* new_child_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 + * \param branch_arg: reference to octree branch class + * \return a single byte with 8 bits of child node information + */ + inline char + getBranchBitPattern(const BranchNode& branch_arg) const + { + char node_bits; + + // create bit pattern + node_bits = 0; + for (unsigned char i = 0; i < 8; i++) { + const OctreeNode* child = branch_arg.getChildPtr(buffer_selector_, i); + node_bits |= static_cast((!!child) << i); + } + + return (node_bits); + } + + /** \brief Generate bit pattern reflecting the existence of child node pointers in + * specific buffer + * \param branch_arg: reference to octree branch class + * \param bufferSelector_arg: buffer selector + * \return a single byte with 8 bits of child node information + */ + inline char + getBranchBitPattern(const BranchNode& branch_arg, + unsigned char bufferSelector_arg) const + { + char node_bits; + + // create bit pattern + node_bits = 0; + for (unsigned char i = 0; i < 8; i++) { + const OctreeNode* child = branch_arg.getChildPtr(bufferSelector_arg, i); + node_bits |= static_cast((!!child) << i); + } + + return (node_bits); + } + + /** \brief Generate XOR bit pattern reflecting differences between the two octree + * buffers + * \param branch_arg: reference to octree branch class + * \return a single byte with 8 bits of child node XOR difference information + */ + inline char + getBranchXORBitPattern(const BranchNode& branch_arg) const + { + char node_bits[2]; + + // create bit pattern for both buffers + node_bits[0] = node_bits[1] = 0; + + for (unsigned char i = 0; i < 8; i++) { + const OctreeNode* childA = branch_arg.getChildPtr(0, i); + const OctreeNode* childB = branch_arg.getChildPtr(1, i); + + node_bits[0] |= static_cast((!!childA) << i); + node_bits[1] |= static_cast((!!childB) << i); + } + + return node_bits[0] ^ node_bits[1]; + } + + /** \brief Test if branch changed between previous and current buffer + * \param branch_arg: reference to octree branch class + * \return "true", if child node information differs between current and previous + * octree buffer + */ + inline bool + hasBranchChanges(const BranchNode& branch_arg) const + { + return (getBranchXORBitPattern(branch_arg) > 0); + } + + /** \brief Delete child node and all its subchilds from octree in specific buffer + * \param branch_arg: reference to octree branch class + * \param buffer_selector_arg: buffer selector + * \param child_idx_arg: index to child node + */ + inline void + deleteBranchChild(BranchNode& branch_arg, + unsigned char buffer_selector_arg, + unsigned char child_idx_arg) + { + if (branch_arg.hasChild(buffer_selector_arg, child_idx_arg)) { + OctreeNode* branchChild = + branch_arg.getChildPtr(buffer_selector_arg, child_idx_arg); + + switch (branchChild->getNodeType()) { + case BRANCH_NODE: { + // free child branch recursively + deleteBranch(*static_cast(branchChild)); + + // delete unused branch + delete (branchChild); + break; + } + + case LEAF_NODE: { + // push unused leaf to branch pool + delete (branchChild); + break; + } + default: + break; + } + + // set branch child pointer to 0 + branch_arg.setChildPtr(buffer_selector_arg, child_idx_arg, nullptr); + } + } + + /** \brief Delete child node and all its subchilds from octree in current buffer + * \param branch_arg: reference to octree branch class + * \param child_idx_arg: index to child node + */ + inline void + deleteBranchChild(BranchNode& branch_arg, unsigned char child_idx_arg) + { + deleteBranchChild(branch_arg, buffer_selector_, child_idx_arg); + } + + /** \brief Delete branch and all its subchilds from octree (both buffers) + * \param branch_arg: reference to octree branch class + */ + inline void + deleteBranch(BranchNode& branch_arg) + { + // delete all branch node children + for (char i = 0; i < 8; i++) { + + if (branch_arg.getChildPtr(0, i) == branch_arg.getChildPtr(1, i)) { + // reference was copied - there is only one child instance to be deleted + deleteBranchChild(branch_arg, 0, i); + + // remove pointers from both buffers + branch_arg.setChildPtr(0, i, nullptr); + branch_arg.setChildPtr(1, i, nullptr); + } + else { + deleteBranchChild(branch_arg, 0, i); + deleteBranchChild(branch_arg, 1, i); + } + } + } + + /** \brief Fetch and add a new branch child to a branch class in current buffer + * \param branch_arg: reference to octree branch class + * \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 child_idx_arg) + { + BranchNode* new_branch_child = new BranchNode(); + + branch_arg.setChildPtr( + buffer_selector_, child_idx_arg, static_cast(new_branch_child)); + + 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 child_idx_arg: index to child node + * \return pointer of new leaf child to this reference + */ + inline LeafNode* + createLeafChild(BranchNode& branch_arg, unsigned char child_idx_arg) + { + LeafNode* new_leaf_child = new LeafNode(); + + branch_arg.setChildPtr(buffer_selector_, child_idx_arg, new_leaf_child); + + return new_leaf_child; + } + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + // Recursive octree methods + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + /** \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 depth_mask_arg: depth mask used for octree key analysis and for branch depth + * indicator + * \param branch_arg: current branch node + * \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 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 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 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 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 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 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: 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* 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 branch_arg: current branch node + * \param depth_mask_arg: depth mask used for octree key analysis and branch depth + * indicator + * \param key_arg: reference to an octree key + * \param binary_tree_in_it_arg iterator of binary input data + * \param binary_tree_in_it_end_arg + * \param leaf_container_vector_it_arg: iterator pointing to leaf container pointers + * to be added to a leaf node + * \param leaf_container_vector_it_end_arg: iterator pointing to leaf container + * 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 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); + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + // Serialization callbacks + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + /** \brief Callback executed for every leaf node data during serialization + **/ + virtual void + serializeTreeCallback(LeafContainerT&, const OctreeKey&) + {} + + /** \brief Callback executed for every leaf node data during deserialization + **/ + virtual void + deserializeTreeCallback(LeafContainerT&, const OctreeKey&) + {} + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + // Helpers + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + /** \brief Recursively explore the octree and remove unused branch and leaf nodes + * \param branch_arg: current branch node + **/ + void + treeCleanUpRecursive(BranchNode* branch_arg); + + /** \brief Helper function to calculate the binary logarithm + * \param n_arg: some value + * \return binary logarithm (log2) of argument n_arg + */ + PCL_DEPRECATED("use std::log2 instead") inline double Log2(double n_arg) + { + return std::log2(n_arg); + } + + /** \brief Test if octree is able to dynamically change its depth. This is required + * for adaptive bounding box adjustment. + * \return "false" - not resizeable due to XOR serialization + **/ + inline bool + octreeCanResize() + { + return (false); + } + + /** \brief Prints binary representation of a byte - used for debugging + * \param data_arg - byte to be printed to stdout + **/ + inline void + printBinary(char data_arg) + { + unsigned char mask = 1; // Bit mask + + // Extract the bits + for (int i = 0; i < 8; i++) { + // Mask each bit in the byte and print it + std::cout << ((data_arg & (mask << i)) ? "1" : "0"); + } + std::cout << std::endl; + } + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + // Globals + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + /** \brief Amount of leaf nodes **/ + std::size_t leaf_count_; + + /** \brief Amount of branch nodes **/ + std::size_t branch_count_; + + /** \brief Pointer to root branch node of octree **/ + BranchNode* root_node_; + + /** \brief Depth mask based on octree depth **/ + unsigned int depth_mask_; + + /** \brief key range */ + OctreeKey max_key_; + + /** \brief Currently active octree buffer **/ + unsigned char buffer_selector_; + + // flags indicating if unused branches and leafs might exist in previous buffer + bool tree_dirty_flag_; + /** \brief Octree depth */ + unsigned int octree_depth_; -namespace pcl -{ - namespace octree - { - - template - class BufferedBranchNode : public OctreeNode - { - - public: - /** \brief Empty constructor. */ - BufferedBranchNode () : OctreeNode() - { - reset (); - } - - /** \brief Copy constructor. */ - BufferedBranchNode (const BufferedBranchNode& source) : OctreeNode() - { - *this = source; - } - - /** \brief Copy operator. */ - inline BufferedBranchNode& - operator = (const BufferedBranchNode &source_arg) - { - memset (child_node_array_, 0, sizeof(child_node_array_)); - - for (unsigned char b = 0; b < 2; ++b) - for (unsigned char 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); - - } - - /** \brief Empty constructor. */ - ~BufferedBranchNode () - { - } - - /** \brief Method to perform a deep copy of the octree */ - BufferedBranchNode* - deepCopy () const override - { - return new BufferedBranchNode (*this); - } - - /** \brief Get child pointer in current branch node - * \param buffer_arg: buffer selector - * \param index_arg: index of child in node - * \return pointer to child node - * */ - inline OctreeNode* - getChildPtr (unsigned char buffer_arg, unsigned char index_arg) const - { - assert( (buffer_arg<2) && (index_arg<8)); - return child_node_array_[buffer_arg][index_arg]; - } - - /** \brief Set child pointer in current branch node - * \param buffer_arg: buffer selector - * \param index_arg: index of child in node - * \param newNode_arg: pointer to new child node - * */ - inline void setChildPtr (unsigned char buffer_arg, unsigned char index_arg, - OctreeNode* newNode_arg) - { - assert( (buffer_arg<2) && (index_arg<8)); - child_node_array_[buffer_arg][index_arg] = newNode_arg; - } - - /** \brief Check if branch is pointing to a particular child node - * \param buffer_arg: buffer selector - * \param index_arg: index of child in node - * \return "true" if pointer to child node exists; "false" otherwise - * */ - inline bool hasChild (unsigned char buffer_arg, unsigned char index_arg) const - { - assert( (buffer_arg<2) && (index_arg<8)); - return (child_node_array_[buffer_arg][index_arg] != nullptr); - } - - /** \brief Get the type of octree node. Returns LEAVE_NODE type */ - node_type_t getNodeType () const override - { - return BRANCH_NODE; - } - - /** \brief Reset branch node container for every branch buffer. */ - inline void reset () - { - memset (&child_node_array_[0][0], 0, sizeof(OctreeNode*) * 8 * 2); - } - - /** \brief Get const pointer to container */ - const ContainerT* - operator->() const - { - return &container_; - } - - /** \brief Get pointer to container */ - ContainerT* - operator-> () - { - return &container_; - } - - /** \brief Get const reference to container */ - const ContainerT& - operator* () const - { - return container_; - } - - /** \brief Get reference to container */ - ContainerT& - operator* () - { - return container_; - } - - /** \brief Get const reference to container */ - const ContainerT& - getContainer () const - { - return container_; - } - - /** \brief Get reference to container */ - ContainerT& - getContainer () - { - return container_; - } - - /** \brief Get const pointer to container */ - const ContainerT* - getContainerPtr() const - { - return &container_; - } - - /** \brief Get pointer to container */ - ContainerT* - getContainerPtr () - { - return &container_; - } - - protected: - ContainerT container_; - - OctreeNode* child_node_array_[2][8]; - }; - - /** \brief @b Octree double buffer class - * - * \note This octree implementation keeps two separate octree structures - * in memory. - * - * \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. - * \ingroup octree - * \author Julius Kammerl (julius@kammerl.de) - */ - template - class Octree2BufBase - { - - public: - - using OctreeT = Octree2BufBase; - - // iterators are friends - friend class OctreeIteratorBase ; - friend class OctreeDepthFirstIterator ; - friend class OctreeBreadthFirstIterator ; - friend class OctreeLeafNodeDepthFirstIterator ; - friend class OctreeLeafNodeBreadthFirstIterator ; - - using BranchNode = BufferedBranchNode; - using LeafNode = OctreeLeafNode; - - using BranchContainer = BranchContainerT; - using LeafContainer = LeafContainerT; - - // Octree default iterators - using Iterator = OctreeDepthFirstIterator; - using ConstIterator = const OctreeDepthFirstIterator; - Iterator begin(unsigned int max_depth_arg = 0) {return Iterator(this, max_depth_arg);}; - const Iterator end() {return Iterator();}; - - // Octree leaf node iterators - // The previous deprecated names - // LeafNodeIterator and ConstLeafNodeIterator are deprecated. - // Please use LeafNodeDepthFirstIterator and ConstLeafNodeDepthFirstIterator instead. - using LeafNodeIterator = OctreeLeafNodeDepthFirstIterator; - using ConstLeafNodeIterator = const OctreeLeafNodeDepthFirstIterator; - - [[deprecated("use leaf_depth_begin() instead")]] - LeafNodeIterator leaf_begin (unsigned int max_depth_arg = 0) - { - return LeafNodeIterator (this, max_depth_arg); - }; - - [[deprecated("use leaf_depth_end() instead")]] - const LeafNodeIterator leaf_end () - { - return LeafNodeIterator (); - }; - - // The currently valide names - using LeafNodeDepthFirstIterator = OctreeLeafNodeDepthFirstIterator; - using ConstLeafNodeDepthFirstIterator = const OctreeLeafNodeDepthFirstIterator; - LeafNodeDepthFirstIterator leaf_depth_begin (unsigned int max_depth_arg = 0) - { - return LeafNodeDepthFirstIterator (this, max_depth_arg); - }; - - const LeafNodeDepthFirstIterator leaf_depth_end () - { - return LeafNodeDepthFirstIterator(); - }; - - // Octree depth-first iterators - using DepthFirstIterator = OctreeDepthFirstIterator; - using ConstDepthFirstIterator = const OctreeDepthFirstIterator; - DepthFirstIterator depth_begin(unsigned int maxDepth_arg = 0) {return DepthFirstIterator(this, maxDepth_arg);}; - const DepthFirstIterator depth_end() {return DepthFirstIterator();}; - - // Octree breadth-first iterators - using BreadthFirstIterator = OctreeBreadthFirstIterator; - using ConstBreadthFirstIterator = const OctreeBreadthFirstIterator; - BreadthFirstIterator breadth_begin(unsigned int max_depth_arg = 0) {return BreadthFirstIterator(this, max_depth_arg);}; - const BreadthFirstIterator breadth_end() {return BreadthFirstIterator();}; - - // Octree leaf node iterators - using LeafNodeBreadthIterator = OctreeLeafNodeBreadthFirstIterator; - using ConstLeafNodeBreadthIterator = const OctreeLeafNodeBreadthFirstIterator; - - LeafNodeBreadthIterator leaf_breadth_begin (unsigned int max_depth_arg = 0u) - { - return LeafNodeBreadthIterator (this, max_depth_arg? max_depth_arg : this->octree_depth_); - }; - - const LeafNodeBreadthIterator leaf_breadth_end () - { - return LeafNodeBreadthIterator (this, 0, nullptr); - }; - - /** \brief Empty constructor. */ - Octree2BufBase (); - - /** \brief Empty deconstructor. */ - virtual - ~Octree2BufBase (); - - /** \brief Copy constructor. */ - Octree2BufBase (const Octree2BufBase& source) : - 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_) - { - } - - /** \brief Copy constructor. */ - inline Octree2BufBase& - operator = (const Octree2BufBase& source) - { - 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 max_voxel_index_arg: maximum amount of voxels per dimension - * */ - void - setMaxVoxelIndex (unsigned int max_voxel_index_arg); - - /** \brief Set the maximum depth of the octree. - * \param depth_arg: maximum depth of octree - * */ - void - setTreeDepth (unsigned int depth_arg); - - /** \brief Get the maximum depth of the octree. - * \return depth_arg: maximum depth of octree - * */ - inline unsigned int getTreeDepth () const - { - return this->octree_depth_; - } - - /** \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 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 idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg); - - /** \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 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 idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg); - - /** \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 idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg) const; - - /** \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 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 (leaf_count_); - } - - /** \brief Return the amount of existing branches in the octree. - * \return amount of branch nodes. - * */ - inline std::size_t getBranchCount () const - { - return (branch_count_); - } - - /** \brief Delete the octree structure and its leaf nodes. - * */ - void - deleteTree (); - - /** \brief Delete octree structure of previous buffer. */ - inline void deletePreviousBuffer () - { - treeCleanUpRecursive (root_node_); - } - - /** \brief Delete the octree structure in the current buffer. */ - inline void deleteCurrentBuffer () - { - buffer_selector_ = !buffer_selector_; - treeCleanUpRecursive (root_node_); - leaf_count_ = 0; - } - - /** \brief Switch buffers and reset current octree structure. */ - void - switchBuffers (); - - /** \brief Serialize octree into a binary output vector describing its branch node structure. - * \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& 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 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& 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 leaf_container_vector_arg: vector of pointers to all LeafContainerT objects in the octree - * */ - void - 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 leaf_container_vector_arg: vector of pointers to all LeafContainerT objects in the octree - * */ - void - 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 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& 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 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& binary_tree_in_arg, - std::vector& leaf_container_vector_arg, - bool do_XOR_decoding_arg = false); - - protected: - - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - // Protected octree methods based on octree keys - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - - /** \brief Retrieve root node */ - OctreeNode* - getRootNode () const - { - return (this->root_node_); - } - - /** \brief Find leaf node - * \param key_arg: octree key addressing a leaf node. - * \return pointer to leaf container. If leaf node is not found, this pointer returns 0. - * */ - inline LeafContainerT* - findLeaf (const OctreeKey& key_arg) const - { - LeafContainerT* result = nullptr; - findLeafRecursive (key_arg, depth_mask_, root_node_, result); - return result; - } - - /** \brief Create a leaf node. - * \note If the leaf node at the given octree node does not exist, it will be created and added to the tree. - * \param key_arg: octree key addressing a leaf node. - * \return pointer to an existing or created leaf container. - * */ - inline LeafContainerT* - createLeaf (const OctreeKey& key_arg) - { - LeafNode* leaf_node; - BranchNode* leaf_node_parent; - - createLeafRecursive (key_arg, depth_mask_ ,root_node_, leaf_node, leaf_node_parent, false); - - LeafContainerT* ret = leaf_node->getContainerPtr(); - - return ret; - } - - /** \brief Check if leaf doesn't exist in the octree - * \param key_arg: octree key addressing a leaf node. - * \return "true" if leaf node is found; "false" otherwise - * */ - inline bool existLeaf (const OctreeKey& key_arg) const - { - return (findLeaf(key_arg) != nullptr); - } - - /** \brief Remove leaf node from octree - * \param key_arg: octree key addressing a leaf node. - * */ - inline void removeLeaf (const OctreeKey& key_arg) - { - if (key_arg <= max_key_) - { - deleteLeafRecursive (key_arg, depth_mask_, root_node_); - - // we changed the octree structure -> dirty - tree_dirty_flag_ = true; - } - } - - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - // Branch node accessor inline functions - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - - /** \brief Check if branch is pointing to a particular child node - * \param branch_arg: reference to octree branch class - * \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 child_idx_arg) const - { - // test occupancyByte for child existence - return (branch_arg.getChildPtr(buffer_selector_, child_idx_arg) != nullptr); - } - - /** \brief Retrieve a child node pointer for child node at child_idx. - * \param branch_arg: reference to octree branch class - * \param child_idx_arg: index to child node - * \return pointer to octree child node class - */ - inline OctreeNode* - getBranchChildPtr (const BranchNode& branch_arg, - unsigned char child_idx_arg) const - { - 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 child_idx_arg: index to child node - * \param new_child_arg: pointer to new child node - * */ - inline void - setBranchChildPtr (BranchNode& branch_arg, unsigned char child_idx_arg, OctreeNode* new_child_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 - * \param branch_arg: reference to octree branch class - * \return a single byte with 8 bits of child node information - * */ - inline char getBranchBitPattern (const BranchNode& branch_arg) const - { - char node_bits; - - // create bit pattern - node_bits = 0; - for (unsigned char i = 0; i < 8; i++) - { - const OctreeNode* child = branch_arg.getChildPtr(buffer_selector_, i); - node_bits |= static_cast ( (!!child) << i); - } - - return (node_bits); - } - - /** \brief Generate bit pattern reflecting the existence of child node pointers in specific buffer - * \param branch_arg: reference to octree branch class - * \param bufferSelector_arg: buffer selector - * \return a single byte with 8 bits of child node information - * */ - inline char getBranchBitPattern (const BranchNode& branch_arg, - unsigned char bufferSelector_arg) const - { - char node_bits; - - // create bit pattern - node_bits = 0; - for (unsigned char i = 0; i < 8; i++) - { - const OctreeNode* child = branch_arg.getChildPtr(bufferSelector_arg, i); - node_bits |= static_cast ( (!!child) << i); - } - - return (node_bits); - } - - /** \brief Generate XOR bit pattern reflecting differences between the two octree buffers - * \param branch_arg: reference to octree branch class - * \return a single byte with 8 bits of child node XOR difference information - * */ - inline char getBranchXORBitPattern ( - const BranchNode& branch_arg) const - { - char node_bits[2]; - - // create bit pattern for both buffers - node_bits[0] = node_bits[1] = 0; - - for (unsigned char i = 0; i < 8; i++) - { - const OctreeNode* childA = branch_arg.getChildPtr(0, i); - const OctreeNode* childB = branch_arg.getChildPtr(1, i); - - node_bits[0] |= static_cast ( (!!childA) << i); - node_bits[1] |= static_cast ( (!!childB) << i); - } - - return node_bits[0] ^ node_bits[1]; - } - - /** \brief Test if branch changed between previous and current buffer - * \param branch_arg: reference to octree branch class - * \return "true", if child node information differs between current and previous octree buffer - * */ - inline bool hasBranchChanges (const BranchNode& branch_arg) const - { - return (getBranchXORBitPattern (branch_arg) > 0); - } - - /** \brief Delete child node and all its subchilds from octree in specific buffer - * \param branch_arg: reference to octree branch class - * \param buffer_selector_arg: buffer selector - * \param child_idx_arg: index to child node - * */ - inline void deleteBranchChild (BranchNode& branch_arg, - unsigned char buffer_selector_arg, - unsigned char child_idx_arg) - { - if (branch_arg.hasChild(buffer_selector_arg, child_idx_arg)) - { - OctreeNode* branchChild = branch_arg.getChildPtr(buffer_selector_arg, child_idx_arg); - - switch (branchChild->getNodeType ()) - { - case BRANCH_NODE: - { - // free child branch recursively - deleteBranch (*static_cast (branchChild)); - - // delete unused branch - delete (branchChild); - break; - } - - case LEAF_NODE: - { - // push unused leaf to branch pool - delete (branchChild); - break; - } - default: - break; - } - - // set branch child pointer to 0 - branch_arg.setChildPtr(buffer_selector_arg, child_idx_arg, nullptr); - } - } - - /** \brief Delete child node and all its subchilds from octree in current buffer - * \param branch_arg: reference to octree branch class - * \param child_idx_arg: index to child node - * */ - inline void deleteBranchChild (BranchNode& branch_arg, unsigned char child_idx_arg) - { - deleteBranchChild(branch_arg, buffer_selector_, child_idx_arg); - } - - /** \brief Delete branch and all its subchilds from octree (both buffers) - * \param branch_arg: reference to octree branch class - * */ - inline void deleteBranch (BranchNode& branch_arg) - { - // delete all branch node children - for (char i = 0; i < 8; i++) - { - - if (branch_arg.getChildPtr(0, i) == branch_arg.getChildPtr(1, i)) - { - // reference was copied - there is only one child instance to be deleted - deleteBranchChild (branch_arg, 0, i); - - // remove pointers from both buffers - branch_arg.setChildPtr(0, i, nullptr); - branch_arg.setChildPtr(1, i, nullptr); - } - else - { - deleteBranchChild (branch_arg, 0, i); - deleteBranchChild (branch_arg, 1, i); - } - } - } - - /** \brief Fetch and add a new branch child to a branch class in current buffer - * \param branch_arg: reference to octree branch class - * \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 child_idx_arg) - { - BranchNode* new_branch_child = new BranchNode(); - - branch_arg.setChildPtr (buffer_selector_, child_idx_arg, - static_cast (new_branch_child)); - - 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 child_idx_arg: index to child node - * \return pointer of new leaf child to this reference - * */ - inline LeafNode* - createLeafChild (BranchNode& branch_arg, unsigned char child_idx_arg) - { - LeafNode* new_leaf_child = new LeafNode(); - - branch_arg.setChildPtr(buffer_selector_, child_idx_arg, new_leaf_child); - - return new_leaf_child; - } - - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - // Recursive octree methods - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - - /** \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 depth_mask_arg: depth mask used for octree key analysis and for branch depth indicator - * \param branch_arg: current branch node - * \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 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 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 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 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 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 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: 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* 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 branch_arg: current branch node - * \param depth_mask_arg: depth mask used for octree key analysis and branch depth indicator - * \param key_arg: reference to an octree key - * \param binary_tree_in_it_arg iterator of binary input data - * \param binary_tree_in_it_end_arg - * \param leaf_container_vector_it_arg: iterator pointing to leaf container pointers to be added to a leaf node - * \param leaf_container_vector_it_end_arg: iterator pointing to leaf container 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 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); - - - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - // Serialization callbacks - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - - /** \brief Callback executed for every leaf node data during serialization - **/ - virtual void serializeTreeCallback (LeafContainerT &, const OctreeKey &) - { - - } - - /** \brief Callback executed for every leaf node data during deserialization - **/ - virtual void deserializeTreeCallback (LeafContainerT&, const OctreeKey&) - { - - } - - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - // Helpers - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - - /** \brief Recursively explore the octree and remove unused branch and leaf nodes - * \param branch_arg: current branch node - **/ - void - treeCleanUpRecursive (BranchNode* branch_arg); - - /** \brief Helper function to calculate the binary logarithm - * \param n_arg: some value - * \return binary logarithm (log2) of argument n_arg - */ - [[deprecated("use std::log2 instead")]] - inline double Log2 (double n_arg) - { - return std::log2 (n_arg); - } - - /** \brief Test if octree is able to dynamically change its depth. This is required for adaptive bounding box adjustment. - * \return "false" - not resizeable due to XOR serialization - **/ - inline bool octreeCanResize () - { - return (false); - } - - /** \brief Prints binary representation of a byte - used for debugging - * \param data_arg - byte to be printed to stdout - **/ - inline void printBinary (char data_arg) - { - unsigned char mask = 1; // Bit mask - - // Extract the bits - for (int i = 0; i < 8; i++) - { - // Mask each bit in the byte and print it - std::cout << ((data_arg & (mask << i)) ? "1" : "0"); - } - std::cout << std::endl; - } - - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - // Globals - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - - /** \brief Amount of leaf nodes **/ - std::size_t leaf_count_; - - /** \brief Amount of branch nodes **/ - std::size_t branch_count_; - - /** \brief Pointer to root branch node of octree **/ - BranchNode* root_node_; - - /** \brief Depth mask based on octree depth **/ - unsigned int depth_mask_; - - /** \brief key range */ - OctreeKey max_key_; - - /** \brief Currently active octree buffer **/ - unsigned char buffer_selector_; - - // flags indicating if unused branches and leafs might exist in previous buffer - bool tree_dirty_flag_; - - /** \brief Octree depth */ - unsigned int octree_depth_; - - /** \brief Enable dynamic_depth - * \note Note that this parameter is ignored in octree2buf! */ - bool dynamic_depth_enabled_; - - }; - } -} + /** \brief Enable dynamic_depth + * \note Note that this parameter is ignored in octree2buf! */ + bool dynamic_depth_enabled_; +}; +} // namespace octree +} // namespace pcl #ifdef PCL_NO_PRECOMPILE #include diff --git a/octree/include/pcl/octree/octree_base.h b/octree/include/pcl/octree/octree_base.h index 69ca9edf..026d65d4 100644 --- a/octree/include/pcl/octree/octree_base.h +++ b/octree/include/pcl/octree/octree_base.h @@ -40,638 +40,669 @@ #include -#include #include -#include #include +#include +#include +#include + +namespace pcl { +namespace octree { +/** \brief Octree class + * \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 The tree depth equates to the bit length of the voxel indices. + * \ingroup octree + * \author Julius Kammerl (julius@kammerl.de) + */ +template +class OctreeBase { +public: + using OctreeT = OctreeBase; + + using BranchNode = OctreeBranchNode; + using LeafNode = OctreeLeafNode; + + using BranchContainer = BranchContainerT; + using LeafContainer = LeafContainerT; + +protected: + /////////////////////////////////////////////////////////////////////// + // Members + /////////////////////////////////////////////////////////////////////// + + /** \brief Amount of leaf nodes **/ + std::size_t leaf_count_; + + /** \brief Amount of branch nodes **/ + std::size_t branch_count_; + + /** \brief Pointer to root branch node of octree **/ + BranchNode* root_node_; + + /** \brief Depth mask based on octree depth **/ + unsigned int depth_mask_; + + /** \brief Octree depth */ + unsigned int octree_depth_; + + /** \brief Enable dynamic_depth **/ + bool dynamic_depth_enabled_; + + /** \brief key range */ + OctreeKey max_key_; + +public: + // iterators are friends + friend class OctreeIteratorBase; + friend class OctreeDepthFirstIterator; + friend class OctreeBreadthFirstIterator; + friend class OctreeFixedDepthIterator; + friend class OctreeLeafNodeDepthFirstIterator; + friend class OctreeLeafNodeBreadthFirstIterator; + + // Octree default iterators + using Iterator = OctreeDepthFirstIterator; + using ConstIterator = const OctreeDepthFirstIterator; + + Iterator + begin(unsigned int max_depth_arg = 0u) + { + return Iterator(this, max_depth_arg ? max_depth_arg : this->octree_depth_); + }; + + const Iterator + end() + { + return Iterator(this, 0, nullptr); + }; + + // Octree leaf node iterators + // The previous deprecated names + // LeafNodeIterator and ConstLeafNodeIterator are deprecated. + // Please use LeafNodeDepthFirstIterator and ConstLeafNodeDepthFirstIterator instead. + using LeafNodeIterator = OctreeLeafNodeDepthFirstIterator; + using ConstLeafNodeIterator = const OctreeLeafNodeDepthFirstIterator; + + PCL_DEPRECATED("use leaf_depth_begin() instead") + LeafNodeIterator + leaf_begin(unsigned int max_depth_arg = 0u) + { + return LeafNodeIterator(this, max_depth_arg ? max_depth_arg : this->octree_depth_); + }; + + PCL_DEPRECATED("use leaf_depth_end() instead") const LeafNodeIterator leaf_end() + { + return LeafNodeIterator(this, 0, nullptr); + }; + + // The currently valide names + using LeafNodeDepthFirstIterator = OctreeLeafNodeDepthFirstIterator; + using ConstLeafNodeDepthFirstIterator = + const OctreeLeafNodeDepthFirstIterator; + + LeafNodeDepthFirstIterator + leaf_depth_begin(unsigned int max_depth_arg = 0u) + { + return LeafNodeDepthFirstIterator( + this, max_depth_arg ? max_depth_arg : this->octree_depth_); + }; + + const LeafNodeDepthFirstIterator + leaf_depth_end() + { + return LeafNodeDepthFirstIterator(this, 0, nullptr); + }; + + // Octree depth-first iterators + using DepthFirstIterator = OctreeDepthFirstIterator; + using ConstDepthFirstIterator = const OctreeDepthFirstIterator; + + DepthFirstIterator + depth_begin(unsigned int max_depth_arg = 0u) + { + return DepthFirstIterator(this, + max_depth_arg ? max_depth_arg : this->octree_depth_); + }; + + const DepthFirstIterator + depth_end() + { + return DepthFirstIterator(this, 0, nullptr); + }; + + // Octree breadth-first iterators + using BreadthFirstIterator = OctreeBreadthFirstIterator; + using ConstBreadthFirstIterator = const OctreeBreadthFirstIterator; + + BreadthFirstIterator + breadth_begin(unsigned int max_depth_arg = 0u) + { + return BreadthFirstIterator(this, + max_depth_arg ? max_depth_arg : this->octree_depth_); + }; + + const BreadthFirstIterator + breadth_end() + { + return BreadthFirstIterator(this, 0, nullptr); + }; + + // Octree breadth iterators at a given depth + using FixedDepthIterator = OctreeFixedDepthIterator; + using ConstFixedDepthIterator = const OctreeFixedDepthIterator; + + FixedDepthIterator + fixed_depth_begin(unsigned int fixed_depth_arg = 0u) + { + return FixedDepthIterator(this, fixed_depth_arg); + }; + + const FixedDepthIterator + fixed_depth_end() + { + return FixedDepthIterator(this, 0, nullptr); + }; + + // Octree leaf node iterators + using LeafNodeBreadthFirstIterator = OctreeLeafNodeBreadthFirstIterator; + using ConstLeafNodeBreadthFirstIterator = + const OctreeLeafNodeBreadthFirstIterator; + + LeafNodeBreadthFirstIterator + leaf_breadth_begin(unsigned int max_depth_arg = 0u) + { + return LeafNodeBreadthFirstIterator( + this, max_depth_arg ? max_depth_arg : this->octree_depth_); + }; + + const LeafNodeBreadthFirstIterator + leaf_breadth_end() + { + return LeafNodeBreadthFirstIterator(this, 0, nullptr); + }; + + /** \brief Empty constructor. */ + OctreeBase(); + + /** \brief Empty deconstructor. */ + virtual ~OctreeBase(); + + /** \brief Copy constructor. */ + OctreeBase(const OctreeBase& source) + : 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_) + , max_key_(source.max_key_) + {} + + /** \brief Copy operator. */ + OctreeBase& + operator=(const OctreeBase& source) + { + 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] max_voxel_index_arg maximum amount of voxels per dimension + */ + void + setMaxVoxelIndex(unsigned int max_voxel_index_arg); + + /** \brief Set the maximum depth of the octree. + * \param max_depth_arg: maximum depth of octree + */ + void + setTreeDepth(unsigned int max_depth_arg); + + /** \brief Get the maximum depth of the octree. + * \return depth_arg: maximum depth of octree + */ + unsigned int + getTreeDepth() const + { + return this->octree_depth_; + } + + /** \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 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 idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg); + + /** \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 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 idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg); + + /** \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 idx_x_arg, + unsigned int idx_y_arg, + unsigned int idx_z_arg) const; + + /** \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 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. + */ + std::size_t + getLeafCount() const + { + return leaf_count_; + } + + /** \brief Return the amount of existing branch nodes in the octree. + * \return amount of branch nodes. + */ + std::size_t + getBranchCount() const + { + return branch_count_; + } + + /** \brief Delete the octree structure and its leaf nodes. + */ + void + deleteTree(); + + /** \brief Serialize octree into a binary output vector describing its branch node + * structure. + * \param binary_tree_out_arg: reference to output vector for writing binary tree + * structure. + */ + void + 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 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& 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 leaf_container_vector_arg: pointers to LeafContainerT vector that receives a + * copy of all LeafContainerT objects in the octree. + */ + void + 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 binary_tree_input_arg: reference to input vector for reading binary tree + * structure. + */ + void + 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 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& binary_tree_input_arg, + std::vector& leaf_container_vector_arg); + +protected: + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + // Protected octree methods based on octree keys + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + /** \brief Create a leaf node + * \param key_arg: octree key addressing a leaf node. + * \return pointer to leaf node + */ + LeafContainerT* + createLeaf(const OctreeKey& key_arg) + { + + LeafNode* leaf_node; + BranchNode* leaf_node_parent; + + createLeafRecursive(key_arg, depth_mask_, root_node_, leaf_node, leaf_node_parent); + + LeafContainerT* ret = leaf_node->getContainerPtr(); + + return ret; + } + + /** \brief Find leaf node + * \param key_arg: octree key addressing a leaf node. + * \return pointer to leaf node. If leaf node is not found, this pointer returns 0. + */ + LeafContainerT* + findLeaf(const OctreeKey& key_arg) const + { + LeafContainerT* result = nullptr; + findLeafRecursive(key_arg, depth_mask_, root_node_, result); + return result; + } + + /** \brief Check for existence of a leaf node in the octree + * \param key_arg: octree key addressing a leaf node. + * \return "true" if leaf node is found; "false" otherwise + */ + bool + existLeaf(const OctreeKey& key_arg) const + { + return (findLeaf(key_arg) != nullptr); + } + + /** \brief Remove leaf node from octree + * \param key_arg: octree key addressing a leaf node. + */ + void + removeLeaf(const OctreeKey& key_arg) + { + if (key_arg <= max_key_) + deleteLeafRecursive(key_arg, depth_mask_, root_node_); + } + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + // Branch node access functions + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + /** \brief Retrieve root node */ + OctreeNode* + getRootNode() const + { + return this->root_node_; + } + + /** \brief Check if branch is pointing to a particular child node + * \param branch_arg: reference to octree branch class + * \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 child_idx_arg) const + { + // test occupancyByte for child existence + return (branch_arg.getChildPtr(child_idx_arg) != nullptr); + } + + /** \brief Retrieve a child node pointer for child node at child_idx. + * \param branch_arg: reference to octree branch class + * \param child_idx_arg: index to child node + * \return pointer to octree child node class + */ + OctreeNode* + getBranchChildPtr(const BranchNode& branch_arg, unsigned char child_idx_arg) const + { + return branch_arg.getChildPtr(child_idx_arg); + } + + /** \brief Assign new child node to branch + * \param branch_arg: reference to octree branch class + * \param child_idx_arg: index to child node + * \param new_child_arg: pointer to new child node + */ + void + setBranchChildPtr(BranchNode& branch_arg, + unsigned char child_idx_arg, + OctreeNode* new_child_arg) + { + branch_arg[child_idx_arg] = new_child_arg; + } + + /** \brief Generate bit pattern reflecting the existence of child node pointers + * \param branch_arg: reference to octree branch class + * \return a single byte with 8 bits of child node information + */ + char + getBranchBitPattern(const BranchNode& branch_arg) const + { + char node_bits; + + // create bit pattern + node_bits = 0; + for (unsigned char i = 0; i < 8; i++) { + const OctreeNode* child = branch_arg.getChildPtr(i); + node_bits |= static_cast((!!child) << i); + } + + return (node_bits); + } + + /** \brief Delete child node and all its subchilds from octree + * \param branch_arg: reference to octree branch class + * \param child_idx_arg: index to child node + */ + void + deleteBranchChild(BranchNode& branch_arg, unsigned char child_idx_arg) + { + if (branch_arg.hasChild(child_idx_arg)) { + OctreeNode* branch_child = branch_arg[child_idx_arg]; + + switch (branch_child->getNodeType()) { + case BRANCH_NODE: { + // free child branch recursively + deleteBranch(*static_cast(branch_child)); + // delete branch node + delete branch_child; + } break; + + case LEAF_NODE: { + // delete leaf node + delete branch_child; + break; + } + default: + break; + } + + // set branch child pointer to 0 + branch_arg[child_idx_arg] = nullptr; + } + } + + /** \brief Delete branch and all its subchilds from octree + * \param branch_arg: reference to octree branch class + */ + void + deleteBranch(BranchNode& branch_arg) + { + // delete all branch node children + for (char i = 0; i < 8; i++) + deleteBranchChild(branch_arg, i); + } + + /** \brief Create and add a new branch child to a branch class + * \param branch_arg: reference to octree branch class + * \param child_idx_arg: index to child node + * \return pointer of new branch child to this reference + */ + BranchNode* + createBranchChild(BranchNode& branch_arg, unsigned char child_idx_arg) + { + BranchNode* new_branch_child = new BranchNode(); + branch_arg[child_idx_arg] = static_cast(new_branch_child); + + 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 child_idx_arg: index to child node + * \return pointer of new leaf child to this reference + */ + LeafNode* + createLeafChild(BranchNode& branch_arg, unsigned char child_idx_arg) + { + LeafNode* new_leaf_child = new LeafNode(); + branch_arg[child_idx_arg] = static_cast(new_leaf_child); + + return new_leaf_child; + } -namespace pcl -{ - namespace octree - { - /** \brief Octree class - * \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. - * \ingroup octree - * \author Julius Kammerl (julius@kammerl.de) - */ - template - class OctreeBase - { - public: - - using OctreeT = OctreeBase; - - using BranchNode = OctreeBranchNode; - using LeafNode = OctreeLeafNode; - - using BranchContainer = BranchContainerT; - using LeafContainer = LeafContainerT; - - protected: - - /////////////////////////////////////////////////////////////////////// - // Members - /////////////////////////////////////////////////////////////////////// - - /** \brief Amount of leaf nodes **/ - std::size_t leaf_count_; - - /** \brief Amount of branch nodes **/ - std::size_t branch_count_; - - /** \brief Pointer to root branch node of octree **/ - BranchNode* root_node_; - - /** \brief Depth mask based on octree depth **/ - unsigned int depth_mask_; - - /** \brief Octree depth */ - unsigned int octree_depth_; - - /** \brief Enable dynamic_depth **/ - bool dynamic_depth_enabled_; - - /** \brief key range */ - OctreeKey max_key_; - - public: - - // iterators are friends - friend class OctreeIteratorBase ; - friend class OctreeDepthFirstIterator ; - friend class OctreeBreadthFirstIterator ; - friend class OctreeFixedDepthIterator ; - friend class OctreeLeafNodeDepthFirstIterator ; - friend class OctreeLeafNodeBreadthFirstIterator ; - - // Octree default iterators - using Iterator = OctreeDepthFirstIterator; - using ConstIterator = const OctreeDepthFirstIterator; - - Iterator begin (unsigned int max_depth_arg = 0u) - { - return Iterator (this, max_depth_arg? max_depth_arg : this->octree_depth_); - }; - - const Iterator end () - { - return Iterator (this, 0, nullptr); - }; - - // Octree leaf node iterators - // The previous deprecated names - // LeafNodeIterator and ConstLeafNodeIterator are deprecated. - // Please use LeafNodeDepthFirstIterator and ConstLeafNodeDepthFirstIterator instead. - using LeafNodeIterator = OctreeLeafNodeDepthFirstIterator; - using ConstLeafNodeIterator = const OctreeLeafNodeDepthFirstIterator; - - [[deprecated("use leaf_depth_begin() instead")]] - LeafNodeIterator leaf_begin (unsigned int max_depth_arg = 0u) - { - return LeafNodeIterator (this, max_depth_arg? max_depth_arg : this->octree_depth_); - }; - - [[deprecated("use leaf_depth_end() instead")]] - const LeafNodeIterator leaf_end () - { - return LeafNodeIterator (this, 0, nullptr); - }; - - // The currently valide names - using LeafNodeDepthFirstIterator = OctreeLeafNodeDepthFirstIterator; - using ConstLeafNodeDepthFirstIterator = const OctreeLeafNodeDepthFirstIterator; - - LeafNodeDepthFirstIterator leaf_depth_begin (unsigned int max_depth_arg = 0u) - { - return LeafNodeDepthFirstIterator (this, max_depth_arg? max_depth_arg : this->octree_depth_); - }; - - const LeafNodeDepthFirstIterator leaf_depth_end () - { - return LeafNodeDepthFirstIterator (this, 0, nullptr); - }; - - // Octree depth-first iterators - using DepthFirstIterator = OctreeDepthFirstIterator; - using ConstDepthFirstIterator = const OctreeDepthFirstIterator; - - DepthFirstIterator depth_begin (unsigned int max_depth_arg = 0u) - { - return DepthFirstIterator (this, max_depth_arg? max_depth_arg : this->octree_depth_); - }; - - const DepthFirstIterator depth_end () - { - return DepthFirstIterator (this, 0, nullptr); - }; - - // Octree breadth-first iterators - using BreadthFirstIterator = OctreeBreadthFirstIterator; - using ConstBreadthFirstIterator = const OctreeBreadthFirstIterator; - - BreadthFirstIterator breadth_begin (unsigned int max_depth_arg = 0u) - { - return BreadthFirstIterator (this, max_depth_arg? max_depth_arg : this->octree_depth_); - }; - - const BreadthFirstIterator breadth_end () - { - return BreadthFirstIterator (this, 0, nullptr); - }; - - // Octree breadth iterators at a given depth - using FixedDepthIterator = OctreeFixedDepthIterator; - using ConstFixedDepthIterator = const OctreeFixedDepthIterator; - - FixedDepthIterator fixed_depth_begin (unsigned int fixed_depth_arg = 0u) - { - return FixedDepthIterator (this, fixed_depth_arg); - }; - - const FixedDepthIterator fixed_depth_end () - { - return FixedDepthIterator (this, 0, nullptr); - }; - - // Octree leaf node iterators - using LeafNodeBreadthFirstIterator = OctreeLeafNodeBreadthFirstIterator; - using ConstLeafNodeBreadthFirstIterator = const OctreeLeafNodeBreadthFirstIterator; - - LeafNodeBreadthFirstIterator leaf_breadth_begin (unsigned int max_depth_arg = 0u) - { - return LeafNodeBreadthFirstIterator (this, max_depth_arg? max_depth_arg : this->octree_depth_); - }; - - const LeafNodeBreadthFirstIterator leaf_breadth_end () - { - return LeafNodeBreadthFirstIterator (this, 0, nullptr); - }; - - /** \brief Empty constructor. */ - OctreeBase (); - - /** \brief Empty deconstructor. */ - virtual - ~OctreeBase (); - - /** \brief Copy constructor. */ - OctreeBase (const OctreeBase& source) : - 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_), - max_key_ (source.max_key_) - { - } - - /** \brief Copy operator. */ - OctreeBase& - operator = (const OctreeBase &source) - { - 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] max_voxel_index_arg maximum amount of voxels per dimension - */ - void - setMaxVoxelIndex (unsigned int max_voxel_index_arg); - - /** \brief Set the maximum depth of the octree. - * \param max_depth_arg: maximum depth of octree - * */ - void - setTreeDepth (unsigned int max_depth_arg); - - /** \brief Get the maximum depth of the octree. - * \return depth_arg: maximum depth of octree - * */ - unsigned int - getTreeDepth () const - { - return this->octree_depth_; - } - - /** \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 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 idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg); - - /** \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 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 idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg); - - /** \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 idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg) const ; - - /** \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 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. - * */ - std::size_t - getLeafCount () const - { - return leaf_count_; - } - - /** \brief Return the amount of existing branch nodes in the octree. - * \return amount of branch nodes. - * */ - std::size_t - getBranchCount () const - { - return branch_count_; - } - - /** \brief Delete the octree structure and its leaf nodes. - * */ - void - deleteTree ( ); - - /** \brief Serialize octree into a binary output vector describing its branch node structure. - * \param binary_tree_out_arg: reference to output vector for writing binary tree structure. - * */ - void - 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 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& 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 leaf_container_vector_arg: pointers to LeafContainerT vector that receives a copy of all LeafContainerT objects in the octree. - * */ - void - 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 binary_tree_input_arg: reference to input vector for reading binary tree structure. - * */ - void - 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 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& binary_tree_input_arg, std::vector& leaf_container_vector_arg); - - protected: - - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - // Protected octree methods based on octree keys - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - - /** \brief Create a leaf node - * \param key_arg: octree key addressing a leaf node. - * \return pointer to leaf node - * */ - LeafContainerT* - createLeaf (const OctreeKey& key_arg) - { - - LeafNode* leaf_node; - BranchNode* leaf_node_parent; - - createLeafRecursive (key_arg, depth_mask_ ,root_node_, leaf_node, leaf_node_parent); - - LeafContainerT* ret = leaf_node->getContainerPtr(); - - return ret; - } - - /** \brief Find leaf node - * \param key_arg: octree key addressing a leaf node. - * \return pointer to leaf node. If leaf node is not found, this pointer returns 0. - * */ - LeafContainerT* - findLeaf (const OctreeKey& key_arg) const - { - LeafContainerT* result = nullptr; - findLeafRecursive (key_arg, depth_mask_, root_node_, result); - return result; - } - - /** \brief Check for existence of a leaf node in the octree - * \param key_arg: octree key addressing a leaf node. - * \return "true" if leaf node is found; "false" otherwise - * */ - bool - existLeaf (const OctreeKey& key_arg) const - { - return (findLeaf(key_arg) != nullptr); - } - - /** \brief Remove leaf node from octree - * \param key_arg: octree key addressing a leaf node. - * */ - void - removeLeaf (const OctreeKey& key_arg) - { - if (key_arg <= max_key_) - deleteLeafRecursive (key_arg, depth_mask_, root_node_); - } - - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - // Branch node access functions - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - - /** \brief Retrieve root node */ - OctreeNode* - getRootNode () const - { - return this->root_node_; - } - - /** \brief Check if branch is pointing to a particular child node - * \param branch_arg: reference to octree branch class - * \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 child_idx_arg) const - { - // test occupancyByte for child existence - return (branch_arg.getChildPtr(child_idx_arg) != nullptr); - } - - /** \brief Retrieve a child node pointer for child node at child_idx. - * \param branch_arg: reference to octree branch class - * \param child_idx_arg: index to child node - * \return pointer to octree child node class - */ - OctreeNode* - getBranchChildPtr (const BranchNode& branch_arg, - unsigned char child_idx_arg) const - { - return branch_arg.getChildPtr(child_idx_arg); - } - - /** \brief Assign new child node to branch - * \param branch_arg: reference to octree branch class - * \param child_idx_arg: index to child node - * \param new_child_arg: pointer to new child node - * */ - void setBranchChildPtr (BranchNode& branch_arg, - unsigned char child_idx_arg, - OctreeNode* new_child_arg) - { - branch_arg[child_idx_arg] = new_child_arg; - } - - /** \brief Generate bit pattern reflecting the existence of child node pointers - * \param branch_arg: reference to octree branch class - * \return a single byte with 8 bits of child node information - * */ - char - getBranchBitPattern (const BranchNode& branch_arg) const - { - char node_bits; - - // create bit pattern - node_bits = 0; - for (unsigned char i = 0; i < 8; i++) { - const OctreeNode* child = branch_arg.getChildPtr(i); - node_bits |= static_cast ((!!child) << i); - } - - return (node_bits); - } - - /** \brief Delete child node and all its subchilds from octree - * \param branch_arg: reference to octree branch class - * \param child_idx_arg: index to child node - * */ - void - deleteBranchChild (BranchNode& branch_arg, unsigned char child_idx_arg) - { - if (branch_arg.hasChild(child_idx_arg)) - { - OctreeNode* branch_child = branch_arg[child_idx_arg]; - - switch (branch_child->getNodeType ()) - { - case BRANCH_NODE: - { - // free child branch recursively - deleteBranch (*static_cast (branch_child)); - // delete branch node - delete branch_child; - } - break; - - case LEAF_NODE: - { - // delete leaf node - delete branch_child; - break; - } - default: - break; - } - - // set branch child pointer to 0 - branch_arg[child_idx_arg] = nullptr; - } - } - - /** \brief Delete branch and all its subchilds from octree - * \param branch_arg: reference to octree branch class - * */ - void - deleteBranch (BranchNode& branch_arg) - { - // delete all branch node children - for (char i = 0; i < 8; i++) - deleteBranchChild (branch_arg, i); - } - - /** \brief Create and add a new branch child to a branch class - * \param branch_arg: reference to octree branch class - * \param child_idx_arg: index to child node - * \return pointer of new branch child to this reference - * */ - BranchNode* createBranchChild (BranchNode& branch_arg, - unsigned char child_idx_arg) - { - BranchNode* new_branch_child = new BranchNode(); - branch_arg[child_idx_arg] = static_cast (new_branch_child); - - 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 child_idx_arg: index to child node - * \return pointer of new leaf child to this reference - * */ - LeafNode* - createLeafChild (BranchNode& branch_arg, unsigned char child_idx_arg) - { - LeafNode* new_leaf_child = new LeafNode(); - branch_arg[child_idx_arg] = static_cast (new_leaf_child); - - return new_leaf_child; - } - - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - // Recursive octree methods - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - - /** \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 depth_mask_arg: depth mask used for octree key analysis and for branch depth indicator - * \param branch_arg: current branch 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 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 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 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 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 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 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* binary_tree_out_arg, - typename std::vector* leaf_container_vector_arg) const; - - /** \brief Recursive method for deserializing octree structure - * \param branch_arg: current branch node - * \param depth_mask_arg: depth mask used for octree key analysis and branch depth indicator - * \param key_arg: reference to an octree key - * \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); - - - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - // Serialization callbacks - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - - /** \brief Callback executed for every leaf node during serialization - **/ - virtual void - serializeTreeCallback (LeafContainerT&, const OctreeKey &) const - { - - } - - /** \brief Callback executed for every leaf node during deserialization - **/ - virtual void - deserializeTreeCallback (LeafContainerT&, const OctreeKey&) - { - - } - - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - // Helpers - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - - /** \brief Helper function to calculate the binary logarithm - * \param n_arg: some value - * \return binary logarithm (log2) of argument n_arg - */ - [[deprecated("use std::log2 instead")]] - double - Log2 (double n_arg) - { - return std::log2( n_arg ); - } - - /** \brief Test if octree is able to dynamically change its depth. This is required for adaptive bounding box adjustment. - * \return "true" - **/ - bool - octreeCanResize () - { - return (true); - } - }; + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + // Recursive octree methods + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + /** \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 depth_mask_arg: depth mask used for octree key analysis and for branch depth + * indicator + * \param branch_arg: current branch 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 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 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 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 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 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 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* binary_tree_out_arg, + typename std::vector* leaf_container_vector_arg) const; + + /** \brief Recursive method for deserializing octree structure + * \param branch_arg: current branch node + * \param depth_mask_arg: depth mask used for octree key analysis and branch depth + * indicator + * \param key_arg: reference to an octree key + * \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); + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + // Serialization callbacks + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + /** \brief Callback executed for every leaf node during serialization + **/ + virtual void + serializeTreeCallback(LeafContainerT&, const OctreeKey&) const + {} + + /** \brief Callback executed for every leaf node during deserialization + **/ + virtual void + deserializeTreeCallback(LeafContainerT&, const OctreeKey&) + {} + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + // Helpers + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + /** \brief Helper function to calculate the binary logarithm + * \param n_arg: some value + * \return binary logarithm (log2) of argument n_arg + */ + PCL_DEPRECATED("use std::log2 instead") double Log2(double n_arg) + { + return std::log2(n_arg); + } + + /** \brief Test if octree is able to dynamically change its depth. This is required + *for adaptive bounding box adjustment. + * \return "true" + **/ + bool + octreeCanResize() + { + return (true); } -} +}; +} // namespace octree +} // namespace pcl #ifdef PCL_NO_PRECOMPILE #include diff --git a/octree/include/pcl/octree/octree_container.h b/octree/include/pcl/octree/octree_container.h index 0663f179..83238004 100644 --- a/octree/include/pcl/octree/octree_container.h +++ b/octree/include/pcl/octree/octree_container.h @@ -38,296 +38,299 @@ #pragma once -#include #include +#include #include -namespace pcl -{ - namespace octree +namespace pcl { +namespace octree { +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** \brief @b Octree container class that can serve as a base to construct own leaf node + * container classes. + * \author Julius Kammerl (julius@kammerl.de) + */ +class OctreeContainerBase { +public: + virtual ~OctreeContainerBase() = default; + + /** \brief Equal comparison operator + */ + virtual bool + operator==(const OctreeContainerBase&) const + { + return false; + } + + /** \brief Inequal comparison operator + * \param[in] other OctreeContainerBase to compare with + */ + bool + operator!=(const OctreeContainerBase& other) const + { + return (!operator==(other)); + } + + /** \brief Pure abstract method to get size of container (number of indices) + * \return number of points/indices stored in leaf node container. + */ + virtual std::size_t + getSize() const + { + return 0u; + } + + /** \brief Pure abstract reset leaf node implementation. */ + virtual void + reset() = 0; + + /** \brief Empty addPointIndex implementation. This leaf node does not store any point + * indices. + */ + void + addPointIndex(const int&) + {} + + /** \brief Empty getPointIndex implementation as this leaf node does not store any + * point indices. + */ + void + getPointIndex(int&) const + {} + + /** \brief Empty getPointIndices implementation as this leaf node does not store any + * data. \ + */ + void + getPointIndices(std::vector&) const + {} +}; + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** \brief @b Octree container class that does not store any information. + * \note Can be used for occupancy trees that are used for checking only the existence + * of leaf nodes in the tree + * \author Julius Kammerl (julius@kammerl.de) + */ + +class OctreeContainerEmpty : public OctreeContainerBase { +public: + /** \brief Octree deep copy method */ + virtual OctreeContainerEmpty* + deepCopy() const + { + return (new OctreeContainerEmpty(*this)); + } + + /** \brief Abstract get size of container (number of DataT objects) + * \return number of DataT elements in leaf node container. + */ + std::size_t + getSize() const override + { + return 0; + } + + /** \brief Abstract reset leaf node implementation. */ + void + reset() override + {} + + /** \brief Empty addPointIndex implementation. This leaf node does not store any point + * indices. + */ + void + addPointIndex(int) + {} + + /** \brief Empty getPointIndex implementation as this leaf node does not store any + * point indices. + */ + int + getPointIndex() const + { + assert("getPointIndex: undefined point index"); + return -1; + } + + /** \brief Empty getPointIndices implementation as this leaf node does not store any + * data. + */ + void + getPointIndices(std::vector&) const + {} +}; + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** \brief @b Octree container class that does store a single point index. + * \note Enables the octree to store a single DataT element within its leaf nodes. + * \author Julius Kammerl (julius@kammerl.de) + */ +class OctreeContainerPointIndex : public OctreeContainerBase { +public: + /** \brief Empty constructor. */ + OctreeContainerPointIndex() { reset(); } + + /** \brief Octree deep copy method */ + virtual OctreeContainerPointIndex* + deepCopy() const + { + return (new OctreeContainerPointIndex(*this)); + } + + /** \brief Equal comparison operator + * \param[in] other OctreeContainerBase to compare with + */ + bool + operator==(const OctreeContainerBase& other) const override { - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - /** \brief @b Octree container class that can serve as a base to construct own leaf node container classes. - * \author Julius Kammerl (julius@kammerl.de) - */ - class OctreeContainerBase - { - public: - virtual ~OctreeContainerBase () = default; - - /** \brief Equal comparison operator - */ - virtual bool - operator== (const OctreeContainerBase&) const - { - return false; - } - - /** \brief Inequal comparison operator - * \param[in] other OctreeContainerBase to compare with - */ - bool - operator!= (const OctreeContainerBase& other) const - { - return (!operator== (other)); - } - - /** \brief Pure abstract method to get size of container (number of indices) - * \return number of points/indices stored in leaf node container. - */ - virtual std::size_t - getSize () const - { - return 0u; - } - - /** \brief Pure abstract reset leaf node implementation. */ - virtual void - reset () = 0; - - /** \brief Empty addPointIndex implementation. This leaf node does not store any point indices. - */ - void - addPointIndex (const int&) - { - } - - /** \brief Empty getPointIndex implementation as this leaf node does not store any point indices. - */ - void - getPointIndex (int&) const - { - } - - /** \brief Empty getPointIndices implementation as this leaf node does not store any data. \ - */ - void - getPointIndices (std::vector&) const - { - } - - }; - - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - /** \brief @b Octree container class that does not store any information. - * \note Can be used for occupancy trees that are used for checking only the existence of leaf nodes in the tree - * \author Julius Kammerl (julius@kammerl.de) - */ - - class OctreeContainerEmpty : public OctreeContainerBase - { - public: - /** \brief Octree deep copy method */ - virtual OctreeContainerEmpty * - deepCopy () const - { - return (new OctreeContainerEmpty (*this)); - } - - /** \brief Abstract get size of container (number of DataT objects) - * \return number of DataT elements in leaf node container. - */ - std::size_t - getSize () const override - { - return 0; - } - - /** \brief Abstract reset leaf node implementation. */ - void - reset () override - { - - } - - /** \brief Empty addPointIndex implementation. This leaf node does not store any point indices. - */ - void - addPointIndex (int) - { - } - - /** \brief Empty getPointIndex implementation as this leaf node does not store any point indices. - */ - int - getPointIndex () const - { - assert("getPointIndex: undefined point index"); - return -1; - } - - /** \brief Empty getPointIndices implementation as this leaf node does not store any data. \ - */ - void - getPointIndices (std::vector&) const - { - } - - }; - - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - /** \brief @b Octree container class that does store a single point index. - * \note Enables the octree to store a single DataT element within its leaf nodes. - * \author Julius Kammerl (julius@kammerl.de) - */ - class OctreeContainerPointIndex : public OctreeContainerBase - { - public: - /** \brief Empty constructor. */ - OctreeContainerPointIndex () - { - reset (); - } - - /** \brief Octree deep copy method */ - virtual OctreeContainerPointIndex* - deepCopy () const - { - return (new OctreeContainerPointIndex (*this)); - } - - /** \brief Equal comparison operator - * \param[in] other OctreeContainerBase to compare with - */ - bool - operator== (const OctreeContainerBase& other) const override - { - const OctreeContainerPointIndex* otherConDataT = dynamic_cast (&other); - - return (this->data_ == otherConDataT->data_); - } - - /** \brief Add point index to container memory. This container stores a only a single point index. - * \param[in] data_arg index to be stored within leaf node. - */ - void - addPointIndex (int data_arg) - { - data_ = data_arg; - } - - /** \brief Retrieve point index from container. This container stores a only a single point index - * \return index stored within container. - */ - int - getPointIndex () const - { - return data_; - } - - /** \brief Retrieve point indices from container. This container stores only a single point index - * \param[out] data_vector_arg vector of point indices to be stored within data vector - */ - void - getPointIndices (std::vector& data_vector_arg) const - { - if (data_>=0) - data_vector_arg.push_back (data_); - } - - /** \brief Get size of container (number of DataT objects) - * \return number of DataT elements in leaf node container. - */ - std::size_t - getSize () const override - { - return data_<0 ? 0 : 1; - } - - /** \brief Reset leaf node memory to zero. */ - void - reset () override - { - data_ = -1; - } - protected: - /** \brief Point index stored in octree. */ - int data_; - }; - - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - /** \brief @b Octree container class that does store a vector of point indices. - * \note Enables the octree to store multiple DataT elements within its leaf nodes. - * \author Julius Kammerl (julius@kammerl.de) - */ - class OctreeContainerPointIndices : public OctreeContainerBase - { - public: - /** \brief Octree deep copy method */ - virtual OctreeContainerPointIndices * - deepCopy () const - { - return (new OctreeContainerPointIndices (*this)); - } - - /** \brief Equal comparison operator - * \param[in] other OctreeContainerDataTVector to compare with - */ - bool - operator== (const OctreeContainerBase& other) const override - { - const OctreeContainerPointIndices* otherConDataTVec = dynamic_cast (&other); - - return (this->leafDataTVector_ == otherConDataTVec->leafDataTVector_); - } - - /** \brief Add point index to container memory. This container stores a vector of point indices. - * \param[in] data_arg index to be stored within leaf node. - */ - void - addPointIndex (int data_arg) - { - leafDataTVector_.push_back (data_arg); - } - - /** \brief Retrieve point index from container. This container stores a vector of point indices. - * \return index stored within container. - */ - int - getPointIndex ( ) const - { - return leafDataTVector_.back (); - } - - /** \brief Retrieve point indices from container. This container stores a vector of point indices. - * \param[out] data_vector_arg vector of point indices to be stored within data vector - */ - void - getPointIndices (std::vector& data_vector_arg) const - { - 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. - * \return reference to vector of point indices to be stored within data vector - */ - std::vector& - getPointIndicesVector () - { - return leafDataTVector_; - } - - /** \brief Get size of container (number of indices) - * \return number of point indices in container. - */ - std::size_t - getSize () const override - { - return leafDataTVector_.size (); - } - - /** \brief Reset leaf node. Clear DataT vector.*/ - void - reset () override - { - leafDataTVector_.clear (); - } - - protected: - /** \brief Leaf node DataT vector. */ - std::vector leafDataTVector_; - }; + const OctreeContainerPointIndex* otherConDataT = + dynamic_cast(&other); + return (this->data_ == otherConDataT->data_); + } + + /** \brief Add point index to container memory. This container stores a only a single + * point index. + * \param[in] data_arg index to be stored within leaf node. + */ + void + addPointIndex(int data_arg) + { + data_ = data_arg; + } + + /** \brief Retrieve point index from container. This container stores a only a single + * point index + * \return index stored within container. + */ + int + getPointIndex() const + { + return data_; + } + + /** \brief Retrieve point indices from container. This container stores only a single + * point index + * \param[out] data_vector_arg vector of point indices to be stored within + * data vector + */ + void + getPointIndices(std::vector& data_vector_arg) const + { + if (data_ >= 0) + data_vector_arg.push_back(data_); } -} + + /** \brief Get size of container (number of DataT objects) + * \return number of DataT elements in leaf node container. + */ + std::size_t + getSize() const override + { + return data_ < 0 ? 0 : 1; + } + + /** \brief Reset leaf node memory to zero. */ + void + reset() override + { + data_ = -1; + } + +protected: + /** \brief Point index stored in octree. */ + int data_; +}; + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** \brief @b Octree container class that does store a vector of point indices. + * \note Enables the octree to store multiple DataT elements within its leaf nodes. + * \author Julius Kammerl (julius@kammerl.de) + */ +class OctreeContainerPointIndices : public OctreeContainerBase { +public: + /** \brief Octree deep copy method */ + virtual OctreeContainerPointIndices* + deepCopy() const + { + return (new OctreeContainerPointIndices(*this)); + } + + /** \brief Equal comparison operator + * \param[in] other OctreeContainerDataTVector to compare with + */ + bool + operator==(const OctreeContainerBase& other) const override + { + const OctreeContainerPointIndices* otherConDataTVec = + dynamic_cast(&other); + + return (this->leafDataTVector_ == otherConDataTVec->leafDataTVector_); + } + + /** \brief Add point index to container memory. This container stores a vector of + * point indices. + * \param[in] data_arg index to be stored within leaf node. + */ + void + addPointIndex(int data_arg) + { + leafDataTVector_.push_back(data_arg); + } + + /** \brief Retrieve point index from container. This container stores a vector of + * point indices. + * \return index stored within container. + */ + int + getPointIndex() const + { + return leafDataTVector_.back(); + } + + /** \brief Retrieve point indices from container. This container stores a vector of + * point indices. + * \param[out] data_vector_arg vector of point indices to be stored + * within data vector + */ + void + getPointIndices(std::vector& data_vector_arg) const + { + 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. + * \return reference to vector of point indices to be stored within data vector + */ + std::vector& + getPointIndicesVector() + { + return leafDataTVector_; + } + + /** \brief Get size of container (number of indices) + * \return number of point indices in container. + */ + std::size_t + getSize() const override + { + return leafDataTVector_.size(); + } + + /** \brief Reset leaf node. Clear DataT vector.*/ + void + reset() override + { + leafDataTVector_.clear(); + } + +protected: + /** \brief Leaf node DataT vector. */ + std::vector leafDataTVector_; +}; + +} // namespace octree +} // namespace pcl diff --git a/octree/include/pcl/octree/octree_impl.h b/octree/include/pcl/octree/octree_impl.h index 0a6f3075..c8fa320b 100644 --- a/octree/include/pcl/octree/octree_impl.h +++ b/octree/include/pcl/octree/octree_impl.h @@ -40,8 +40,8 @@ #include -#include #include -#include +#include #include +#include #include diff --git a/octree/include/pcl/octree/octree_iterator.h b/octree/include/pcl/octree/octree_iterator.h index 9afffc8a..2d20cf2b 100644 --- a/octree/include/pcl/octree/octree_iterator.h +++ b/octree/include/pcl/octree/octree_iterator.h @@ -40,784 +40,769 @@ #pragma once #include -#include #include +#include -#include #include +#include #include // Ignore warnings in the above headers #ifdef __GNUC__ -#pragma GCC system_header +#pragma GCC system_header #endif -namespace pcl -{ - namespace octree +namespace pcl { +namespace octree { + +// Octree iterator state pushed on stack/list +struct IteratorState { + OctreeNode* node_; + OctreeKey key_; + unsigned int depth_; +}; + +/** \brief @b Abstract octree iterator class + * \note Octree iterator base class + * \ingroup octree + * \author Julius Kammerl (julius@kammerl.de) + */ +template +class OctreeIteratorBase : public std::iterator { +public: + using LeafNode = typename OctreeT::LeafNode; + using BranchNode = typename OctreeT::BranchNode; + + using LeafContainer = typename OctreeT::LeafContainer; + using BranchContainer = typename OctreeT::BranchContainer; + + /** \brief Empty constructor. + */ + explicit 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 max_depth_arg = 0) + : octree_(octree_arg), 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 + * \param[in] current_state A pointer to the current iterator state + * + * \warning For advanced users only. + */ + explicit OctreeIteratorBase(OctreeT* octree_arg, + unsigned int max_depth_arg, + IteratorState* current_state) + : octree_(octree_arg), current_state_(current_state), max_octree_depth_(max_depth_arg) + {} + + /** \brief Empty deconstructor. */ + virtual ~OctreeIteratorBase() {} + + /** \brief Equal comparison operator + * \param[in] other OctreeIteratorBase to compare with + */ + bool + operator==(const OctreeIteratorBase& other) const + { + if (this == &other) // same object + return true; + if (octree_ != other.octree_) // refer to different octrees + return false; + if (!current_state_ && !other.current_state_) // both are end iterators + return true; + if (max_octree_depth_ == other.max_octree_depth_ && current_state_ && + other.current_state_ && // null dereference protection + current_state_->key_ == other.current_state_->key_) + return true; + return false; + } + + /** \brief Inequal comparison operator + * \param[in] other OctreeIteratorBase to compare with + */ + bool + operator!=(const OctreeIteratorBase& other) const + { + return !operator==(other); + } + + /** \brief Reset iterator */ + inline void + reset() + { + current_state_ = 0; + if (octree_ && (!max_octree_depth_)) { + max_octree_depth_ = octree_->getTreeDepth(); + } + } + + /** \brief Get octree key for the current iterator octree node + * \return octree key of current node + */ + inline const OctreeKey& + getCurrentOctreeKey() const + { + assert(octree_ != 0); + assert(current_state_ != 0); + + return (current_state_->key_); + } + + /** \brief Get the current depth level of octree + * \return depth level + */ + inline unsigned int + getCurrentOctreeDepth() const + { + assert(octree_ != 0); + assert(current_state_ != 0); + + return (current_state_->depth_); + } + + /** \brief Get the current octree node + * \return pointer to current octree node + */ + inline OctreeNode* + getCurrentOctreeNode() const + { + assert(octree_ != 0); + assert(current_state_ != 0); + + return (current_state_->node_); + } + + /** \brief check if current node is a branch node + * \return true if current node is a branch node, false otherwise + */ + inline bool + isBranchNode() const + { + assert(octree_ != 0); + assert(current_state_ != 0); + + return (current_state_->node_->getNodeType() == BRANCH_NODE); + } + + /** \brief check if current node is a branch node + * \return true if current node is a branch node, false otherwise + */ + inline bool + isLeafNode() const + { + assert(octree_ != 0); + assert(current_state_ != 0); + + return (current_state_->node_->getNodeType() == LEAF_NODE); + } + + /** \brief *operator. + * \return pointer to the current octree node + */ + inline OctreeNode* operator*() const + { // return designated object + if (octree_ && current_state_) { + return (current_state_->node_); + } + else { + return 0; + } + } + + /** \brief Get bit pattern of children configuration of current node + * \return bit pattern (byte) describing the existence of 8 children of the current + * node + */ + inline char + getNodeConfiguration() const + { + char ret = 0; + + assert(octree_ != 0); + assert(current_state_ != 0); + + if (isBranchNode()) { + + // current node is a branch node + const BranchNode* current_branch = + static_cast(current_state_->node_); + + // get child configuration bit pattern + ret = octree_->getBranchBitPattern(*current_branch); + } + + return (ret); + } + + /** \brief Method for retrieving a single leaf container from the octree leaf node + * \return Reference to container class of leaf node. + */ + const LeafContainer& + getLeafContainer() const + { + assert(octree_ != 0); + assert(current_state_ != 0); + assert(this->isLeafNode()); + + LeafNode* leaf_node = static_cast(current_state_->node_); + + return leaf_node->getContainer(); + } + + /** \brief Method for retrieving a single leaf container from the octree leaf node + * \return Reference to container class of leaf node. + */ + LeafContainer& + getLeafContainer() + { + assert(octree_ != 0); + assert(current_state_ != 0); + assert(this->isLeafNode()); + + LeafNode* leaf_node = static_cast(current_state_->node_); + + return leaf_node->getContainer(); + } + + /** \brief Method for retrieving the container from an octree branch node + * \return BranchContainer. + */ + const BranchContainer& + getBranchContainer() const + { + assert(octree_ != 0); + assert(current_state_ != 0); + assert(this->isBranchNode()); + + BranchNode* branch_node = static_cast(current_state_->node_); + + return branch_node->getContainer(); + } + + /** \brief Method for retrieving the container from an octree branch node + * \return BranchContainer. + */ + BranchContainer& + getBranchContainer() + { + assert(octree_ != 0); + assert(current_state_ != 0); + assert(this->isBranchNode()); + + BranchNode* branch_node = static_cast(current_state_->node_); + + return branch_node->getContainer(); + } + + /** \brief get a integer identifier for current node (note: identifier depends on tree + * depth). \return node id. + */ + virtual unsigned long + getNodeID() const + { + unsigned long id = 0; + + assert(octree_ != 0); + assert(current_state_ != 0); + + if (current_state_) { + const OctreeKey& key = getCurrentOctreeKey(); + // calculate integer id with respect to octree key + unsigned int depth = octree_->getTreeDepth(); + id = static_cast(key.x) << (depth * 2) | + static_cast(key.y) << (depth * 1) | + static_cast(key.z) << (depth * 0); + } + + return id; + } + +protected: + /** \brief Reference to octree class. */ + OctreeT* octree_; + + /** \brief Pointer to current iterator state. */ + IteratorState* current_state_; + + /** \brief Maximum octree depth */ + unsigned int max_octree_depth_; +}; + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** \brief @b Octree iterator class + * \note This class implements a forward iterator for traversing octrees in a + * depth-first manner. + * \ingroup octree + * \author Julius Kammerl (julius@kammerl.de) + */ +template +class OctreeDepthFirstIterator : public OctreeIteratorBase { + +public: + using LeafNode = typename OctreeIteratorBase::LeafNode; + using BranchNode = typename OctreeIteratorBase::BranchNode; + + /** \brief Empty constructor. + * \param[in] max_depth_arg Depth limitation during traversal + */ + explicit 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 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 + * \param[in] current_state A pointer to the current iterator state + * + * \warning For advanced users only. + */ + explicit OctreeDepthFirstIterator( + OctreeT* octree_arg, + unsigned int max_depth_arg, + IteratorState* current_state, + const std::vector& stack = std::vector()) + : OctreeIteratorBase(octree_arg, max_depth_arg, current_state), stack_(stack) + {} + + /** \brief Copy Constructor. + * \param[in] other Another OctreeDepthFirstIterator to copy from + */ + OctreeDepthFirstIterator(const OctreeDepthFirstIterator& other) + : OctreeIteratorBase(other), stack_(other.stack_) + { + this->current_state_ = stack_.size() ? &stack_.back() : NULL; + } + + /** \brief Copy assignment + * \param[in] src the iterator to copy into this + */ + inline OctreeDepthFirstIterator& + operator=(const OctreeDepthFirstIterator& src) + { + + OctreeIteratorBase::operator=(src); + + stack_ = src.stack_; + + if (stack_.size()) { + this->current_state_ = &stack_.back(); + } + else { + this->current_state_ = 0; + } + + return (*this); + } + + /** \brief Reset the iterator to the root node of the octree + */ + virtual void + reset(); + + /** \brief Preincrement operator. + * \note recursively step to next octree node + */ + OctreeDepthFirstIterator& + operator++(); + + /** \brief postincrement operator. + * \note recursively step to next octree node + */ + inline OctreeDepthFirstIterator + operator++(int) + { + OctreeDepthFirstIterator _Tmp = *this; + ++*this; + return (_Tmp); + } + + /** \brief Skip all child voxels of current node and return to parent node. + */ + void + skipChildVoxels(); + +protected: + /** Stack structure. */ + std::vector stack_; +}; + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** \brief @b Octree iterator class + * \note This class implements a forward iterator for traversing octrees in a + * breadth-first manner. + * \ingroup octree + * \author Julius Kammerl (julius@kammerl.de) + */ +template +class OctreeBreadthFirstIterator : public OctreeIteratorBase { +public: + // public typedefs + using BranchNode = typename OctreeIteratorBase::BranchNode; + using LeafNode = typename OctreeIteratorBase::LeafNode; + + /** \brief Empty constructor. + * \param[in] max_depth_arg Depth limitation during traversal + */ + explicit 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 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 + * \param[in] current_state A pointer to the current iterator state + * + * \warning For advanced users only. + */ + explicit OctreeBreadthFirstIterator( + OctreeT* octree_arg, + unsigned int max_depth_arg, + IteratorState* current_state, + const std::deque& fifo = std::deque()) + : OctreeIteratorBase(octree_arg, max_depth_arg, current_state), FIFO_(fifo) + {} + + /** \brief Copy Constructor. + * \param[in] other Another OctreeBreadthFirstIterator to copy from + */ + OctreeBreadthFirstIterator(const OctreeBreadthFirstIterator& other) + : OctreeIteratorBase(other), FIFO_(other.FIFO_) + { + this->current_state_ = FIFO_.size() ? &FIFO_.front() : NULL; + } + + /** \brief Copy operator. + * \param[in] src the iterator to copy into this + */ + inline OctreeBreadthFirstIterator& + operator=(const OctreeBreadthFirstIterator& src) { - // Octree iterator state pushed on stack/list - struct IteratorState{ - OctreeNode* node_; - OctreeKey key_; - unsigned int depth_; - }; - - - /** \brief @b Abstract octree iterator class - * \note Octree iterator base class - * \ingroup octree - * \author Julius Kammerl (julius@kammerl.de) - */ - template - class OctreeIteratorBase : public std::iterator - { - public: - - using LeafNode = typename OctreeT::LeafNode; - using BranchNode = typename OctreeT::BranchNode; - - using LeafContainer = typename OctreeT::LeafContainer; - using BranchContainer = typename OctreeT::BranchContainer; - - /** \brief Empty constructor. - */ - explicit - 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 max_depth_arg = 0) : - octree_ (octree_arg), 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 - * \param[in] current_state A pointer to the current iterator state - * - * \warning For advanced users only. - */ - explicit - OctreeIteratorBase (OctreeT* octree_arg, - unsigned int max_depth_arg, - IteratorState* current_state) - : octree_(octree_arg) - , current_state_ (current_state) - , max_octree_depth_ (max_depth_arg) - {} - - /** \brief Empty deconstructor. */ - virtual - ~OctreeIteratorBase () - { - } - - /** \brief Equal comparison operator - * \param[in] other OctreeIteratorBase to compare with - */ - bool operator==(const OctreeIteratorBase& other) const - { - if (this == &other) // same object - return true; - if (octree_ != other.octree_) // refer to different octrees - return false; - if (!current_state_ && !other.current_state_) // both are end iterators - return true; - if (max_octree_depth_ == other.max_octree_depth_ && - current_state_ && other.current_state_ && // null dereference protection - current_state_->key_ == other.current_state_->key_) - return true; - return false; - } - - /** \brief Inequal comparison operator - * \param[in] other OctreeIteratorBase to compare with - */ - bool operator!=(const OctreeIteratorBase& other) const - { - return !operator== (other); - } - - /** \brief Reset iterator */ - inline void reset () - { - current_state_ = 0; - if (octree_ && (!max_octree_depth_)) - { - max_octree_depth_ = octree_->getTreeDepth(); - } - } - - /** \brief Get octree key for the current iterator octree node - * \return octree key of current node - */ - inline const OctreeKey& - getCurrentOctreeKey () const - { - assert(octree_!=0); - assert(current_state_!=0); - - return (current_state_->key_); - } - - /** \brief Get the current depth level of octree - * \return depth level - */ - inline unsigned int - getCurrentOctreeDepth () const - { - assert(octree_!=0); - assert(current_state_!=0); - - return (current_state_->depth_); - } - - /** \brief Get the current octree node - * \return pointer to current octree node - */ - inline OctreeNode* - getCurrentOctreeNode () const - { - assert(octree_!=0); - assert(current_state_!=0); - - return (current_state_->node_); - } - - - /** \brief check if current node is a branch node - * \return true if current node is a branch node, false otherwise - */ - inline bool - isBranchNode () const - { - assert(octree_!=0); - assert(current_state_!=0); - - return (current_state_->node_->getNodeType () == BRANCH_NODE); - } - - /** \brief check if current node is a branch node - * \return true if current node is a branch node, false otherwise - */ - inline bool - isLeafNode () const - { - assert(octree_!=0); - assert(current_state_!=0); - - return (current_state_->node_->getNodeType () == LEAF_NODE); - } - - /** \brief *operator. - * \return pointer to the current octree node - */ - inline OctreeNode* - operator* () const - { // return designated object - if (octree_ && current_state_) - { - return (current_state_->node_); - } else - { - return 0; - } - } - - /** \brief Get bit pattern of children configuration of current node - * \return bit pattern (byte) describing the existence of 8 children of the current node - */ - inline char - getNodeConfiguration () const - { - char ret = 0; - - assert(octree_!=0); - assert(current_state_!=0); - - if (isBranchNode ()) - { - - // current node is a branch node - const BranchNode* current_branch = static_cast (current_state_->node_); - - // get child configuration bit pattern - ret = octree_->getBranchBitPattern (*current_branch); - - } - - return (ret); - } - - /** \brief Method for retrieving a single leaf container from the octree leaf node - * \return Reference to container class of leaf node. - */ - const LeafContainer& - getLeafContainer () const - { - assert(octree_!=0); - assert(current_state_!=0); - assert(this->isLeafNode()); - - LeafNode* leaf_node = static_cast(current_state_->node_); - - return leaf_node->getContainer(); - } - - /** \brief Method for retrieving a single leaf container from the octree leaf node - * \return Reference to container class of leaf node. - */ - LeafContainer& - getLeafContainer () - { - assert(octree_!=0); - assert(current_state_!=0); - assert(this->isLeafNode()); - - LeafNode* leaf_node = static_cast(current_state_->node_); - - return leaf_node->getContainer(); - } - - /** \brief Method for retrieving the container from an octree branch node - * \return BranchContainer. - */ - const BranchContainer& - getBranchContainer () const - { - assert(octree_!=0); - assert(current_state_!=0); - assert(this->isBranchNode()); - - BranchNode* branch_node = static_cast(current_state_->node_); - - return branch_node->getContainer(); - } - - /** \brief Method for retrieving the container from an octree branch node - * \return BranchContainer. - */ - BranchContainer& - getBranchContainer () - { - assert(octree_!=0); - assert(current_state_!=0); - assert(this->isBranchNode()); - - BranchNode* branch_node = static_cast(current_state_->node_); - - return branch_node->getContainer(); - } - - /** \brief get a integer identifier for current node (note: identifier depends on tree depth). - * \return node id. - */ - virtual unsigned long - getNodeID () const - { - unsigned long id = 0; - - assert(octree_!=0); - assert(current_state_!=0); - - if (current_state_) - { - const OctreeKey& key = getCurrentOctreeKey(); - // calculate integer id with respect to octree key - unsigned int depth = octree_->getTreeDepth (); - id = static_cast (key.x) << (depth * 2) - | static_cast (key.y) << (depth * 1) - | static_cast (key.z) << (depth * 0); - } - - return id; - } - - protected: - /** \brief Reference to octree class. */ - OctreeT* octree_; - - /** \brief Pointer to current iterator state. */ - IteratorState* current_state_; - - /** \brief Maximum octree depth */ - unsigned int max_octree_depth_; - }; - - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - /** \brief @b Octree iterator class - * \note This class implements a forward iterator for traversing octrees in a depth-first manner. - * \ingroup octree - * \author Julius Kammerl (julius@kammerl.de) - */ - template - class OctreeDepthFirstIterator : public OctreeIteratorBase - { - - public: - - using LeafNode = typename OctreeIteratorBase::LeafNode; - using BranchNode = typename OctreeIteratorBase::BranchNode; - - /** \brief Empty constructor. - * \param[in] max_depth_arg Depth limitation during traversal - */ - explicit - 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 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 - * \param[in] current_state A pointer to the current iterator state - * - * \warning For advanced users only. - */ - explicit - OctreeDepthFirstIterator (OctreeT* octree_arg, - unsigned int max_depth_arg, - IteratorState* current_state, - const std::vector& stack = std::vector ()) - : OctreeIteratorBase (octree_arg, max_depth_arg, current_state) - , stack_ (stack) - {} - - /** \brief Copy Constructor. - * \param[in] other Another OctreeDepthFirstIterator to copy from - */ - OctreeDepthFirstIterator (const OctreeDepthFirstIterator& other) - : OctreeIteratorBase (other) - , stack_ (other.stack_) - { - this->current_state_ = stack_.size ()? &stack_.back () : NULL; - } - - /** \brief Copy assignment - * \param[in] src the iterator to copy into this - */ - inline OctreeDepthFirstIterator& - operator = (const OctreeDepthFirstIterator& src) - { - - OctreeIteratorBase::operator=(src); - - stack_ = src.stack_; - - if (stack_.size()) - { - this->current_state_ = &stack_.back (); - } else - { - this->current_state_ = 0; - } - - return (*this); - } - - /** \brief Reset the iterator to the root node of the octree - */ - virtual void - reset (); - - /** \brief Preincrement operator. - * \note recursively step to next octree node - */ - OctreeDepthFirstIterator& - operator++ (); - - /** \brief postincrement operator. - * \note recursively step to next octree node - */ - inline OctreeDepthFirstIterator - operator++ (int) - { - OctreeDepthFirstIterator _Tmp = *this; - ++*this; - return (_Tmp); - } - - /** \brief Skip all child voxels of current node and return to parent node. - */ - void - skipChildVoxels (); - - protected: - /** Stack structure. */ - std::vector stack_; - }; - - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - /** \brief @b Octree iterator class - * \note This class implements a forward iterator for traversing octrees in a breadth-first manner. - * \ingroup octree - * \author Julius Kammerl (julius@kammerl.de) - */ - template - class OctreeBreadthFirstIterator : public OctreeIteratorBase - { - public: - // public typedefs - using BranchNode = typename OctreeIteratorBase::BranchNode; - using LeafNode = typename OctreeIteratorBase::LeafNode; - - /** \brief Empty constructor. - * \param[in] max_depth_arg Depth limitation during traversal - */ - explicit - 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 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 - * \param[in] current_state A pointer to the current iterator state - * - * \warning For advanced users only. - */ - explicit - OctreeBreadthFirstIterator (OctreeT* octree_arg, - unsigned int max_depth_arg, - IteratorState* current_state, - const std::deque& fifo = std::deque ()) - : OctreeIteratorBase (octree_arg, max_depth_arg, current_state) - , FIFO_ (fifo) - {} - - /** \brief Copy Constructor. - * \param[in] other Another OctreeBreadthFirstIterator to copy from - */ - OctreeBreadthFirstIterator (const OctreeBreadthFirstIterator& other) - : OctreeIteratorBase (other) - , FIFO_ (other.FIFO_) - { - this->current_state_ = FIFO_.size ()? &FIFO_.front () : NULL; - } - - /** \brief Copy operator. - * \param[in] src the iterator to copy into this - */ - inline OctreeBreadthFirstIterator& - operator = (const OctreeBreadthFirstIterator& src) - { - - OctreeIteratorBase::operator=(src); - - FIFO_ = src.FIFO_; - - if (FIFO_.size()) - { - this->current_state_ = &FIFO_.front(); - } else - { - this->current_state_ = 0; - } - - return (*this); - } - - /** \brief Reset the iterator to the root node of the octree - */ - void - reset (); - - /** \brief Preincrement operator. - * \note step to next octree node - */ - OctreeBreadthFirstIterator& - operator++ (); - - /** \brief postincrement operator. - * \note step to next octree node - */ - inline OctreeBreadthFirstIterator - operator++ (int) - { - OctreeBreadthFirstIterator _Tmp = *this; - ++*this; - return (_Tmp); - } - - protected: - /** FIFO list */ - std::deque FIFO_; - }; - - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - /** \brief @b Octree iterator class - * \note Iterator over all existing nodes at a given depth. It walks across an octree - * in a breadth-first manner. - * \ingroup octree - * \author Fabien Rozar (fabien.rozar@gmail.com) - */ - template - class OctreeFixedDepthIterator : public OctreeBreadthFirstIterator - { - public: - - // public typedefs - using typename OctreeBreadthFirstIterator::BranchNode; - using typename OctreeBreadthFirstIterator::LeafNode; - - /** \brief Empty constructor. - */ - OctreeFixedDepthIterator (); - - /** \brief Constructor. - * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its root node. - * \param[in] fixed_depth_arg Depth level during traversal - */ - explicit - OctreeFixedDepthIterator (OctreeT* octree_arg, unsigned int fixed_depth_arg = 0); - - /** \brief Constructor. - * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its root node. - * \param[in] fixed_depth_arg Depth level during traversal - * \param[in] current_state A pointer to the current iterator state - * \param[in] fifo Internal container of octree node to go through - * - * \warning For advanced users only. - */ - OctreeFixedDepthIterator (OctreeT* octree_arg, - unsigned int fixed_depth_arg, - IteratorState* current_state, - const std::deque& fifo = std::deque ()) - : OctreeBreadthFirstIterator (octree_arg, fixed_depth_arg, current_state, fifo) - , fixed_depth_ (fixed_depth_arg) - {} - - /** \brief Copy Constructor. - * \param[in] other Another OctreeFixedDepthIterator to copy from - */ - OctreeFixedDepthIterator (const OctreeFixedDepthIterator& other) - : OctreeBreadthFirstIterator (other) - { - this->fixed_depth_ = other.fixed_depth_; - } - - /** \brief Copy assignment. - * \param[in] src the iterator to copy into this - * \return pointer to the current octree node - */ - inline OctreeFixedDepthIterator& - operator = (const OctreeFixedDepthIterator& src) - { - OctreeBreadthFirstIterator::operator= (src); - this->fixed_depth_ = src.fixed_depth_; - - return (*this); - } - - /** \brief Reset the iterator to the first node at the depth given as parameter - * \param[in] fixed_depth_arg Depth level during traversal - */ - void - reset (unsigned int fixed_depth_arg); - - /** \brief Reset the iterator to the first node at the current depth - */ - void - reset () - { - this->reset (fixed_depth_); - } - - protected: - using OctreeBreadthFirstIterator::FIFO_; - - /** \brief Given level of the node to be iterated */ - unsigned int fixed_depth_; - }; - - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - /** \brief Octree leaf node iterator class - * \note This class implements a forward iterator for traversing the leaf nodes of an octree data structure. - * \ingroup octree - * \author Julius Kammerl (julius@kammerl.de) - */ - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - template - class OctreeLeafNodeDepthFirstIterator : public OctreeDepthFirstIterator - { - using BranchNode = typename OctreeDepthFirstIterator::BranchNode; - using LeafNode = typename OctreeDepthFirstIterator::LeafNode; - - public: - /** \brief Empty constructor. - * \param[in] max_depth_arg Depth limitation during traversal - */ - explicit - OctreeLeafNodeDepthFirstIterator (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 - OctreeLeafNodeDepthFirstIterator (OctreeT* octree_arg, unsigned int max_depth_arg = 0) : - OctreeDepthFirstIterator (octree_arg, 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 - * \param[in] current_state A pointer to the current iterator state - * - * \warning For advanced users only. - */ - explicit - OctreeLeafNodeDepthFirstIterator (OctreeT* octree_arg, - unsigned int max_depth_arg, - IteratorState* current_state, - const std::vector& stack = std::vector ()) - : OctreeDepthFirstIterator (octree_arg, - max_depth_arg, - current_state, - stack) - {} - - /** \brief Reset the iterator to the root node of the octree - */ - inline void - reset () - { - OctreeDepthFirstIterator::reset (); - this->operator++ (); - } - - /** \brief Preincrement operator. - * \note recursively step to next octree leaf node - */ - inline OctreeLeafNodeDepthFirstIterator& - operator++ () - { - do - { - OctreeDepthFirstIterator::operator++ (); - } while ((this->current_state_) && (this->current_state_->node_->getNodeType () != LEAF_NODE)); - - return (*this); - } - - /** \brief postincrement operator. - * \note step to next octree node - */ - inline OctreeLeafNodeDepthFirstIterator - operator++ (int) - { - OctreeLeafNodeDepthFirstIterator _Tmp = *this; - ++*this; - return (_Tmp); - } - - /** \brief *operator. - * \return pointer to the current octree leaf node - */ - OctreeNode* - operator* () const - { - // return designated object - OctreeNode* ret = 0; - - if (this->current_state_ && (this->current_state_->node_->getNodeType () == LEAF_NODE)) - ret = this->current_state_->node_; - return (ret); - } - }; - - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - /** \brief Octree leaf node iterator class - * \note This class implements a forward iterator for traversing the leaf nodes of an octree data structure - * in the breadth first way. - * \ingroup octree - * \author Fabien Rozar (fabien.rozar@gmail.com) - */ - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - template - class OctreeLeafNodeBreadthFirstIterator : public OctreeBreadthFirstIterator - { - using BranchNode = typename OctreeBreadthFirstIterator::BranchNode; - using LeafNode = typename OctreeBreadthFirstIterator::LeafNode; - - public: - /** \brief Empty constructor. - * \param[in] max_depth_arg Depth limitation during traversal - */ - explicit - OctreeLeafNodeBreadthFirstIterator (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 - OctreeLeafNodeBreadthFirstIterator (OctreeT* octree_arg, unsigned int max_depth_arg = 0); - - /** \brief Copy 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 - * \param[in] current_state A pointer to the current iterator state - * \param[in] fifo Internal container of octree node to go through - * - * \warning For advanced users only. - */ - explicit - OctreeLeafNodeBreadthFirstIterator (OctreeT* octree_arg, - unsigned int max_depth_arg, - IteratorState* current_state, - const std::deque& fifo = std::deque ()); - - /** \brief Reset the iterator to the first leaf in the breadth first way. - */ - inline void - reset (); - - /** \brief Preincrement operator. - * \note recursively step to next octree leaf node - */ - inline OctreeLeafNodeBreadthFirstIterator& - operator++ (); - - - /** \brief Postincrement operator. - * \note step to next octree node - */ - inline OctreeLeafNodeBreadthFirstIterator - operator++ (int); - }; - - } -} + OctreeIteratorBase::operator=(src); + + FIFO_ = src.FIFO_; + + if (FIFO_.size()) { + this->current_state_ = &FIFO_.front(); + } + else { + this->current_state_ = 0; + } + + return (*this); + } + + /** \brief Reset the iterator to the root node of the octree + */ + void + reset(); + + /** \brief Preincrement operator. + * \note step to next octree node + */ + OctreeBreadthFirstIterator& + operator++(); + + /** \brief postincrement operator. + * \note step to next octree node + */ + inline OctreeBreadthFirstIterator + operator++(int) + { + OctreeBreadthFirstIterator _Tmp = *this; + ++*this; + return (_Tmp); + } + +protected: + /** FIFO list */ + std::deque FIFO_; +}; + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** \brief @b Octree iterator class + * \note Iterator over all existing nodes at a given depth. It walks across an octree + * in a breadth-first manner. + * \ingroup octree + * \author Fabien Rozar (fabien.rozar@gmail.com) + */ +template +class OctreeFixedDepthIterator : public OctreeBreadthFirstIterator { +public: + // public typedefs + using typename OctreeBreadthFirstIterator::BranchNode; + using typename OctreeBreadthFirstIterator::LeafNode; + + /** \brief Empty constructor. + */ + OctreeFixedDepthIterator(); + + /** \brief Constructor. + * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its + * root node. + * \param[in] fixed_depth_arg Depth level during traversal + */ + explicit OctreeFixedDepthIterator(OctreeT* octree_arg, + unsigned int fixed_depth_arg = 0); + + /** \brief Constructor. + * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its + * root node. + * \param[in] fixed_depth_arg Depth level during traversal + * \param[in] current_state A pointer to the current iterator state + * \param[in] fifo Internal container of octree node to go through + * + * \warning For advanced users only. + */ + OctreeFixedDepthIterator( + OctreeT* octree_arg, + unsigned int fixed_depth_arg, + IteratorState* current_state, + const std::deque& fifo = std::deque()) + : OctreeBreadthFirstIterator( + octree_arg, fixed_depth_arg, current_state, fifo) + , fixed_depth_(fixed_depth_arg) + {} + + /** \brief Copy Constructor. + * \param[in] other Another OctreeFixedDepthIterator to copy from + */ + OctreeFixedDepthIterator(const OctreeFixedDepthIterator& other) + : OctreeBreadthFirstIterator(other) + { + this->fixed_depth_ = other.fixed_depth_; + } + + /** \brief Copy assignment. + * \param[in] src the iterator to copy into this + * \return pointer to the current octree node + */ + inline OctreeFixedDepthIterator& + operator=(const OctreeFixedDepthIterator& src) + { + OctreeBreadthFirstIterator::operator=(src); + this->fixed_depth_ = src.fixed_depth_; + + return (*this); + } + + /** \brief Reset the iterator to the first node at the depth given as parameter + * \param[in] fixed_depth_arg Depth level during traversal + */ + void + reset(unsigned int fixed_depth_arg); + + /** \brief Reset the iterator to the first node at the current depth + */ + void + reset() + { + this->reset(fixed_depth_); + } + +protected: + using OctreeBreadthFirstIterator::FIFO_; + + /** \brief Given level of the node to be iterated */ + unsigned int fixed_depth_; +}; + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** \brief Octree leaf node iterator class + * \note This class implements a forward iterator for traversing the leaf nodes of an + * octree data structure. + * \ingroup octree + * \author Julius Kammerl (julius@kammerl.de) + */ +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +class OctreeLeafNodeDepthFirstIterator : public OctreeDepthFirstIterator { + using BranchNode = typename OctreeDepthFirstIterator::BranchNode; + using LeafNode = typename OctreeDepthFirstIterator::LeafNode; + +public: + /** \brief Empty constructor. + * \param[in] max_depth_arg Depth limitation during traversal + */ + explicit OctreeLeafNodeDepthFirstIterator(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 OctreeLeafNodeDepthFirstIterator(OctreeT* octree_arg, + unsigned int max_depth_arg = 0) + : OctreeDepthFirstIterator(octree_arg, 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 + * \param[in] current_state A pointer to the current iterator state + * + * \warning For advanced users only. + */ + explicit OctreeLeafNodeDepthFirstIterator( + OctreeT* octree_arg, + unsigned int max_depth_arg, + IteratorState* current_state, + const std::vector& stack = std::vector()) + : OctreeDepthFirstIterator(octree_arg, max_depth_arg, current_state, stack) + {} + + /** \brief Reset the iterator to the root node of the octree + */ + inline void + reset() + { + OctreeDepthFirstIterator::reset(); + this->operator++(); + } + + /** \brief Preincrement operator. + * \note recursively step to next octree leaf node + */ + inline OctreeLeafNodeDepthFirstIterator& + operator++() + { + do { + OctreeDepthFirstIterator::operator++(); + } while ((this->current_state_) && + (this->current_state_->node_->getNodeType() != LEAF_NODE)); + + return (*this); + } + + /** \brief postincrement operator. + * \note step to next octree node + */ + inline OctreeLeafNodeDepthFirstIterator + operator++(int) + { + OctreeLeafNodeDepthFirstIterator _Tmp = *this; + ++*this; + return (_Tmp); + } + + /** \brief *operator. + * \return pointer to the current octree leaf node + */ + OctreeNode* operator*() const + { + // return designated object + OctreeNode* ret = 0; + + if (this->current_state_ && + (this->current_state_->node_->getNodeType() == LEAF_NODE)) + ret = this->current_state_->node_; + return (ret); + } +}; + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** \brief Octree leaf node iterator class + * \note This class implements a forward iterator for traversing the leaf nodes of an + * octree data structure in the breadth first way. + * \ingroup octree + * \author Fabien Rozar + * (fabien.rozar@gmail.com) + */ +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +class OctreeLeafNodeBreadthFirstIterator : public OctreeBreadthFirstIterator { + using BranchNode = typename OctreeBreadthFirstIterator::BranchNode; + using LeafNode = typename OctreeBreadthFirstIterator::LeafNode; + +public: + /** \brief Empty constructor. + * \param[in] max_depth_arg Depth limitation during traversal + */ + explicit OctreeLeafNodeBreadthFirstIterator(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 OctreeLeafNodeBreadthFirstIterator(OctreeT* octree_arg, + unsigned int max_depth_arg = 0); + + /** \brief Copy 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 + * \param[in] current_state A pointer to the current iterator state + * \param[in] fifo Internal container of octree node to go through + * + * \warning For advanced users only. + */ + explicit OctreeLeafNodeBreadthFirstIterator( + OctreeT* octree_arg, + unsigned int max_depth_arg, + IteratorState* current_state, + const std::deque& fifo = std::deque()); + + /** \brief Reset the iterator to the first leaf in the breadth first way. + */ + inline void + reset(); + + /** \brief Preincrement operator. + * \note recursively step to next octree leaf node + */ + inline OctreeLeafNodeBreadthFirstIterator& + operator++(); + + /** \brief Postincrement operator. + * \note step to next octree node + */ + inline OctreeLeafNodeBreadthFirstIterator + operator++(int); +}; + +} // namespace octree +} // namespace pcl /* * Note: Since octree iterators depend on octrees, don't precompile them. diff --git a/octree/include/pcl/octree/octree_key.h b/octree/include/pcl/octree/octree_key.h index f37e37b7..6ba5196a 100644 --- a/octree/include/pcl/octree/octree_key.h +++ b/octree/include/pcl/octree/octree_key.h @@ -37,126 +37,120 @@ #pragma once -namespace pcl -{ - namespace octree +#include + +namespace pcl { +namespace octree { +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** \brief @b Octree key class + * \note Octree keys contain integer indices for each coordinate axis in order to + * address an octree leaf node. + * \author Julius Kammerl (julius@kammerl.de) + */ +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +class OctreeKey { +public: + /** \brief Empty constructor. */ + OctreeKey() : x(0), y(0), z(0) {} + + /** \brief Constructor for key initialization. */ + OctreeKey(unsigned int keyX, unsigned int keyY, unsigned int keyZ) + : x(keyX), y(keyY), z(keyZ) + {} + + /** \brief Copy constructor. */ + OctreeKey(const OctreeKey& source) { memcpy(key_, source.key_, sizeof(key_)); } + + OctreeKey& + operator=(const OctreeKey&) = default; + + /** \brief Operator== for comparing octree keys with each other. + * \return "true" if leaf node indices are identical; "false" otherwise. + * */ + bool + operator==(const OctreeKey& b) const { - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - /** \brief @b Octree key class - * \note Octree keys contain integer indices for each coordinate axis in order to address an octree leaf node. - * \author Julius Kammerl (julius@kammerl.de) - */ - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - class OctreeKey - { - public: - - /** \brief Empty constructor. */ - OctreeKey () : - x (0), y (0), z (0) - { - } - - /** \brief Constructor for key initialization. */ - OctreeKey (unsigned int keyX, unsigned int keyY, unsigned int keyZ) : - x (keyX), y (keyY), z (keyZ) - { - } - - /** \brief Copy constructor. */ - OctreeKey (const OctreeKey& source) - { - memcpy(key_, source.key_, sizeof(key_)); - } - - OctreeKey& operator=(const OctreeKey&) = default; - - /** \brief Operator== for comparing octree keys with each other. - * \return "true" if leaf node indices are identical; "false" otherwise. - * */ - bool - operator == (const OctreeKey& b) const - { - return ((b.x == this->x) && (b.y == this->y) && (b.z == this->z)); - } - - /** \brief Inequal comparison operator - * \param[in] other OctreeIteratorBase to compare with - * \return "true" if the current and other iterators are different ; "false" otherwise. - */ - bool operator!= (const OctreeKey& other) const - { - return !operator== (other); - } - - /** \brief Operator<= for comparing octree keys with each other. - * \return "true" if key indices are not greater than the key indices of b ; "false" otherwise. - * */ - bool - operator <= (const OctreeKey& b) const - { - return ((b.x >= this->x) && (b.y >= this->y) && (b.z >= this->z)); - } - - /** \brief Operator>= for comparing octree keys with each other. - * \return "true" if key indices are not smaller than the key indices of b ; "false" otherwise. - * */ - bool - operator >= (const OctreeKey& b) const - { - return ((b.x <= this->x) && (b.y <= this->y) && (b.z <= this->z)); - } - - /** \brief push a child node to the octree key - * \param[in] childIndex index of child node to be added (0-7) - * */ - inline void - pushBranch (unsigned char childIndex) - { - this->x = (this->x << 1) | (!!(childIndex & (1 << 2))); - this->y = (this->y << 1) | (!!(childIndex & (1 << 1))); - this->z = (this->z << 1) | (!!(childIndex & (1 << 0))); - } - - /** \brief pop child node from octree key - * */ - inline void - popBranch () - { - this->x >>= 1; - this->y >>= 1; - this->z >>= 1; - } - - /** \brief get child node index using depthMask - * \param[in] depthMask bit mask with single bit set at query depth - * \return child node index - * */ - inline unsigned char - getChildIdxWithDepthMask (unsigned int depthMask) const - { - return static_cast (((!!(this->x & depthMask)) << 2) - | ((!!(this->y & depthMask)) << 1) - | (!!(this->z & depthMask))); - } - - /* \brief maximum depth that can be addressed */ - static const unsigned char maxDepth = static_cast(sizeof(std::uint32_t)*8); - - // Indices addressing a voxel at (X, Y, Z) - - union - { - struct - { - std::uint32_t x; - std::uint32_t y; - std::uint32_t z; - }; - std::uint32_t key_[3]; - }; + return ((b.x == this->x) && (b.y == this->y) && (b.z == this->z)); + } + /** \brief Inequal comparison operator + * \param[in] other OctreeIteratorBase to compare with + * \return "true" if the current and other iterators are different ; "false" + * otherwise. + */ + bool + operator!=(const OctreeKey& other) const + { + return !operator==(other); + } - }; + /** \brief Operator<= for comparing octree keys with each other. + * \return "true" if key indices are not greater than the key indices of b ; "false" + * otherwise. + * */ + bool + operator<=(const OctreeKey& b) const + { + return ((b.x >= this->x) && (b.y >= this->y) && (b.z >= this->z)); + } + + /** \brief Operator>= for comparing octree keys with each other. + * \return "true" if key indices are not smaller than the key indices of b ; "false" + * otherwise. + * */ + bool + operator>=(const OctreeKey& b) const + { + return ((b.x <= this->x) && (b.y <= this->y) && (b.z <= this->z)); + } + + /** \brief push a child node to the octree key + * \param[in] childIndex index of child node to be added (0-7) + * */ + inline void + pushBranch(unsigned char childIndex) + { + this->x = (this->x << 1) | (!!(childIndex & (1 << 2))); + this->y = (this->y << 1) | (!!(childIndex & (1 << 1))); + this->z = (this->z << 1) | (!!(childIndex & (1 << 0))); + } + + /** \brief pop child node from octree key + * */ + inline void + popBranch() + { + this->x >>= 1; + this->y >>= 1; + this->z >>= 1; } -} + + /** \brief get child node index using depthMask + * \param[in] depthMask bit mask with single bit set at query depth + * \return child node index + * */ + inline unsigned char + getChildIdxWithDepthMask(unsigned int depthMask) const + { + return static_cast(((!!(this->x & depthMask)) << 2) | + ((!!(this->y & depthMask)) << 1) | + (!!(this->z & depthMask))); + } + + /* \brief maximum depth that can be addressed */ + static const unsigned char maxDepth = + static_cast(sizeof(std::uint32_t) * 8); + + // Indices addressing a voxel at (X, Y, Z) + + union { + struct { + std::uint32_t x; + std::uint32_t y; + std::uint32_t z; + }; + std::uint32_t key_[3]; + }; +}; +} // namespace octree +} // namespace pcl diff --git a/octree/include/pcl/octree/octree_node_pool.h b/octree/include/pcl/octree/octree_node_pool.h index 7d54ea16..f261eb83 100644 --- a/octree/include/pcl/octree/octree_node_pool.h +++ b/octree/include/pcl/octree/octree_node_pool.h @@ -41,86 +41,72 @@ #include -namespace pcl -{ - namespace octree - { - - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - /** \brief @b Octree node pool - * \note Used to reduce memory allocation and class instantiation events when generating octrees at high rate - * \author Julius Kammerl (julius@kammerl.de) - */ - template - class OctreeNodePool - { - public: - /** \brief Empty constructor. */ - OctreeNodePool () : - nodePool_ () - { - } +namespace pcl { +namespace octree { - /** \brief Empty deconstructor. */ - virtual - ~OctreeNodePool () - { - deletePool (); - } +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** \brief @b Octree node pool + * \note Used to reduce memory allocation and class instantiation events when generating + * octrees at high rate + * \author Julius Kammerl (julius@kammerl.de) + */ +template +class OctreeNodePool { +public: + /** \brief Empty constructor. */ + OctreeNodePool() : nodePool_() {} - /** \brief Push node to pool - * \param node_arg: add this node to the pool - * */ - inline - void - pushNode (NodeT* node_arg) - { - nodePool_.push_back (node_arg); - } + /** \brief Empty deconstructor. */ + virtual ~OctreeNodePool() { deletePool(); } - /** \brief Pop node from pool - Allocates new nodes if pool is empty - * \return Pointer to octree node - * */ - inline NodeT* - popNode () - { + /** \brief Push node to pool + * \param node_arg: add this node to the pool + * */ + inline void + pushNode(NodeT* node_arg) + { + nodePool_.push_back(node_arg); + } - NodeT* newLeafNode; + /** \brief Pop node from pool - Allocates new nodes if pool is empty + * \return Pointer to octree node + * */ + inline NodeT* + popNode() + { - if (!nodePool_.size ()) - { - // leaf pool is empty - // we need to create a new octree leaf class - newLeafNode = new NodeT (); - } - else - { - // reuse leaf node from branch pool - newLeafNode = nodePool_.back (); - nodePool_.pop_back (); - newLeafNode->reset (); - } + NodeT* newLeafNode; - return newLeafNode; - } + if (!nodePool_.size()) { + // leaf pool is empty + // we need to create a new octree leaf class + newLeafNode = new NodeT(); + } + else { + // reuse leaf node from branch pool + newLeafNode = nodePool_.back(); + nodePool_.pop_back(); + newLeafNode->reset(); + } + return newLeafNode; + } - /** \brief Delete all nodes in pool - * */ - void - deletePool () - { - // delete all branch instances from branch pool - while (!nodePool_.empty ()) - { - delete (nodePool_.back ()); - nodePool_.pop_back (); - } - } + /** \brief Delete all nodes in pool + * */ + void + deletePool() + { + // delete all branch instances from branch pool + while (!nodePool_.empty()) { + delete (nodePool_.back()); + nodePool_.pop_back(); + } + } - protected: - std::vector nodePool_; - }; +protected: + std::vector nodePool_; +}; - } -} +} // namespace octree +} // namespace pcl diff --git a/octree/include/pcl/octree/octree_nodes.h b/octree/include/pcl/octree/octree_nodes.h index fca6f7da..0277fc98 100644 --- a/octree/include/pcl/octree/octree_nodes.h +++ b/octree/include/pcl/octree/octree_nodes.h @@ -48,346 +48,288 @@ #include "octree_container.h" -namespace pcl -{ - namespace octree - { +namespace pcl { +namespace octree { - // enum of node types within the octree - enum node_type_t - { - BRANCH_NODE, LEAF_NODE - }; - - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - /** \brief @b Abstract octree node class - * \note Every octree node should implement the getNodeType () method - * \author Julius Kammerl (julius@kammerl.de) - */ - class PCL_EXPORTS OctreeNode - { - public: - - OctreeNode () - { - } +// enum of node types within the octree +enum node_type_t { BRANCH_NODE, LEAF_NODE }; - virtual - ~OctreeNode () - { - } - /** \brief Pure virtual method for receiving the type of octree node (branch or leaf) */ - virtual node_type_t - getNodeType () const = 0; +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** \brief @b Abstract octree node class + * \note Every octree node should implement the getNodeType () method + * \author Julius Kammerl (julius@kammerl.de) + */ +class PCL_EXPORTS OctreeNode { +public: + OctreeNode() {} + + virtual ~OctreeNode() {} + /** \brief Pure virtual method for receiving the type of octree node (branch or leaf) + */ + virtual node_type_t + getNodeType() const = 0; + + /** \brief Pure virtual method to perform a deep copy of the octree */ + virtual OctreeNode* + deepCopy() const = 0; +}; + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** \brief @b Abstract octree leaf class + * \note Octree leafs may collect data of type DataT + * \author Julius Kammerl (julius@kammerl.de) + */ - /** \brief Pure virtual method to perform a deep copy of the octree */ - virtual OctreeNode* - deepCopy () const = 0; +template +class OctreeLeafNode : public OctreeNode { +public: + /** \brief Empty constructor. */ + OctreeLeafNode() : OctreeNode() {} - }; + /** \brief Copy constructor. */ + OctreeLeafNode(const OctreeLeafNode& source) : OctreeNode() + { + container_ = source.container_; + } - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - /** \brief @b Abstract octree leaf class - * \note Octree leafs may collect data of type DataT - * \author Julius Kammerl (julius@kammerl.de) - */ + /** \brief Empty deconstructor. */ - template - class OctreeLeafNode : public OctreeNode - { - public: + ~OctreeLeafNode() {} - /** \brief Empty constructor. */ - OctreeLeafNode () : - OctreeNode () - { - } + /** \brief Method to perform a deep copy of the octree */ + OctreeLeafNode* + deepCopy() const override + { + return new OctreeLeafNode(*this); + } - /** \brief Copy constructor. */ - OctreeLeafNode (const OctreeLeafNode& source) : - OctreeNode () - { - container_ = source.container_; - } + /** \brief Get the type of octree node. Returns LEAVE_NODE type */ + node_type_t + getNodeType() const override + { + return LEAF_NODE; + } - /** \brief Empty deconstructor. */ - - ~OctreeLeafNode () - { - } + /** \brief Get const pointer to container */ + const ContainerT* operator->() const { return &container_; } - /** \brief Method to perform a deep copy of the octree */ - OctreeLeafNode* - deepCopy () const override - { - return new OctreeLeafNode (*this); - } + /** \brief Get pointer to container */ + ContainerT* operator->() { return &container_; } - /** \brief Get the type of octree node. Returns LEAVE_NODE type */ - node_type_t - getNodeType () const override - { - return LEAF_NODE; - } + /** \brief Get const reference to container */ + const ContainerT& operator*() const { return container_; } - /** \brief Get const pointer to container */ - const ContainerT* - operator->() const - { - return &container_; - } + /** \brief Get reference to container */ + ContainerT& operator*() { return container_; } - /** \brief Get pointer to container */ - ContainerT* - operator-> () - { - return &container_; - } + /** \brief Get const reference to container */ + const ContainerT& + getContainer() const + { + return container_; + } - /** \brief Get const reference to container */ - const ContainerT& - operator* () const - { - return container_; - } + /** \brief Get reference to container */ + ContainerT& + getContainer() + { + return container_; + } - /** \brief Get reference to container */ - ContainerT& - operator* () - { - return container_; - } + /** \brief Get const pointer to container */ + const ContainerT* + getContainerPtr() const + { + return &container_; + } - /** \brief Get const reference to container */ - const ContainerT& - getContainer () const - { - return container_; - } + /** \brief Get pointer to container */ + ContainerT* + getContainerPtr() + { + return &container_; + } - /** \brief Get reference to container */ - ContainerT& - getContainer () - { - return container_; - } +protected: + ContainerT container_; - /** \brief Get const pointer to container */ - const ContainerT* - getContainerPtr() const - { - return &container_; - } +public: + // Type ContainerT may have fixed-size Eigen objects inside + PCL_MAKE_ALIGNED_OPERATOR_NEW +}; - /** \brief Get pointer to container */ - ContainerT* - getContainerPtr () - { - return &container_; - } +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** \brief @b Abstract octree branch class + * \note Octree branch classes may collect data of type DataT + * \author Julius Kammerl (julius@kammerl.de) + */ +template +class OctreeBranchNode : public OctreeNode { +public: + /** \brief Empty constructor. */ + OctreeBranchNode() : OctreeNode() + { + // reset pointer to child node vectors + memset(child_node_array_, 0, sizeof(child_node_array_)); + } - protected: - ContainerT container_; - - public: - //Type ContainerT may have fixed-size Eigen objects inside - PCL_MAKE_ALIGNED_OPERATOR_NEW - }; - - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - /** \brief @b Abstract octree branch class - * \note Octree branch classes may collect data of type DataT - * \author Julius Kammerl (julius@kammerl.de) - */ - template - class OctreeBranchNode : public OctreeNode - { - public: + /** \brief Empty constructor. */ + OctreeBranchNode(const OctreeBranchNode& source) : OctreeNode() + { + memset(child_node_array_, 0, sizeof(child_node_array_)); - /** \brief Empty constructor. */ - OctreeBranchNode () : - OctreeNode() - { - // reset pointer to child node vectors - memset (child_node_array_, 0, sizeof(child_node_array_)); - } + for (unsigned char i = 0; i < 8; ++i) + if (source.child_node_array_[i]) + child_node_array_[i] = source.child_node_array_[i]->deepCopy(); + } - /** \brief Empty constructor. */ - OctreeBranchNode (const OctreeBranchNode& source) : - OctreeNode() - { - memset (child_node_array_, 0, sizeof(child_node_array_)); + /** \brief Copy operator. */ + inline OctreeBranchNode& + operator=(const OctreeBranchNode& source) + { + memset(child_node_array_, 0, sizeof(child_node_array_)); - for (unsigned char i = 0; i < 8; ++i) - if (source.child_node_array_[i]) - child_node_array_[i] = source.child_node_array_[i]->deepCopy (); - } + for (unsigned char i = 0; i < 8; ++i) + if (source.child_node_array_[i]) + child_node_array_[i] = source.child_node_array_[i]->deepCopy(); + return (*this); + } - /** \brief Copy operator. */ - inline OctreeBranchNode& - operator = (const OctreeBranchNode &source) - { - memset (child_node_array_, 0, sizeof(child_node_array_)); + /** \brief Octree deep copy method */ + OctreeBranchNode* + deepCopy() const override + { + return (new OctreeBranchNode(*this)); + } - for (unsigned char i = 0; i < 8; ++i) - if (source.child_node_array_[i]) - child_node_array_[i] = source.child_node_array_[i]->deepCopy (); - return (*this); - } + /** \brief Empty deconstructor. */ - /** \brief Octree deep copy method */ - OctreeBranchNode* - deepCopy () const override - { - return (new OctreeBranchNode (*this)); - } + ~OctreeBranchNode() {} - /** \brief Empty deconstructor. */ - - ~OctreeBranchNode () - { - } + /** \brief Access operator. + * \param child_idx_arg: index to child node + * \return OctreeNode pointer + * */ + inline OctreeNode*& operator[](unsigned char child_idx_arg) + { + assert(child_idx_arg < 8); + return child_node_array_[child_idx_arg]; + } - /** \brief Access operator. - * \param child_idx_arg: index to child node - * \return OctreeNode pointer - * */ - inline OctreeNode*& - operator[] (unsigned char child_idx_arg) - { - assert(child_idx_arg < 8); - return child_node_array_[child_idx_arg]; - } + /** \brief Get pointer to child + * \param child_idx_arg: index to child node + * \return OctreeNode pointer + * */ + inline OctreeNode* + getChildPtr(unsigned char child_idx_arg) const + { + assert(child_idx_arg < 8); + return child_node_array_[child_idx_arg]; + } - /** \brief Get pointer to child - * \param child_idx_arg: index to child node - * \return OctreeNode pointer - * */ - inline OctreeNode* - getChildPtr (unsigned char child_idx_arg) const - { - assert(child_idx_arg < 8); - return child_node_array_[child_idx_arg]; - } + /** \brief Get pointer to child + * \return OctreeNode pointer + * */ + inline void + setChildPtr(OctreeNode* child, unsigned char index) + { + assert(index < 8); + child_node_array_[index] = child; + } - /** \brief Get pointer to child - * \return OctreeNode pointer - * */ - inline void setChildPtr (OctreeNode* child, unsigned char index) - { - assert(index < 8); - child_node_array_[index] = child; - } + /** \brief Check if branch is pointing to a particular 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 child_idx_arg) const + { + return (child_node_array_[child_idx_arg] != nullptr); + } + /** \brief Check if branch can be pruned + * \note if all children are leaf nodes AND contain identical containers, branch can + * be pruned + * \return "true" if branch can be pruned; "false" otherwise + **/ + /* inline bool isPrunable () const + { + const OctreeNode* firstChild = child_node_array_[0]; + if (!firstChild || firstChild->getNodeType()==BRANCH_NODE) + return false; - /** \brief Check if branch is pointing to a particular 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 child_idx_arg) const + bool prunable = true; + for (unsigned char i = 1; i < 8 && prunable; ++i) { - return (child_node_array_[child_idx_arg] != nullptr); + const OctreeNode* child = child_node_array_[i]; + if ( (!child) || + (child->getNodeType()==BRANCH_NODE) || + ((*static_cast(child)) == (*static_cast(child)) ) ) prunable = false; } + return prunable; + }*/ - /** \brief Check if branch can be pruned - * \note if all children are leaf nodes AND contain identical containers, branch can be pruned - * \return "true" if branch can be pruned; "false" otherwise - * */ - /* inline bool isPrunable () const - { - 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 = child_node_array_[i]; - if ( (!child) || - (child->getNodeType()==BRANCH_NODE) || - ((*static_cast(child)) == (*static_cast(child)) ) ) - prunable = false; - } - - return prunable; - }*/ - - /** \brief Get the type of octree node. Returns LEAVE_NODE type */ - node_type_t - getNodeType () const override - { - return BRANCH_NODE; - } - - // reset node - void reset() - { - memset(child_node_array_, 0, sizeof(child_node_array_)); - container_.reset(); - } + /** \brief Get the type of octree node. Returns LEAVE_NODE type */ + node_type_t + getNodeType() const override + { + return BRANCH_NODE; + } + // reset node + void + reset() + { + memset(child_node_array_, 0, sizeof(child_node_array_)); + container_.reset(); + } - /** \brief Get const pointer to container */ - const ContainerT* - operator->() const - { - return &container_; - } + /** \brief Get const pointer to container */ + const ContainerT* operator->() const { return &container_; } - /** \brief Get pointer to container */ - ContainerT* - operator-> () - { - return &container_; - } + /** \brief Get pointer to container */ + ContainerT* operator->() { return &container_; } - /** \brief Get const reference to container */ - const ContainerT& - operator* () const - { - return container_; - } + /** \brief Get const reference to container */ + const ContainerT& operator*() const { return container_; } - /** \brief Get reference to container */ - ContainerT& - operator* () - { - return container_; - } + /** \brief Get reference to container */ + ContainerT& operator*() { return container_; } - /** \brief Get const reference to container */ - const ContainerT& - getContainer () const - { - return container_; - } + /** \brief Get const reference to container */ + const ContainerT& + getContainer() const + { + return container_; + } - /** \brief Get reference to container */ - ContainerT& - getContainer () - { - return container_; - } + /** \brief Get reference to container */ + ContainerT& + getContainer() + { + return container_; + } - /** \brief Get const pointer to container */ - const ContainerT* - getContainerPtr() const - { - return &container_; - } + /** \brief Get const pointer to container */ + const ContainerT* + getContainerPtr() const + { + return &container_; + } - /** \brief Get pointer to container */ - ContainerT* - getContainerPtr () - { - return &container_; - } + /** \brief Get pointer to container */ + ContainerT* + getContainerPtr() + { + return &container_; + } - protected: - OctreeNode* child_node_array_[8]; +protected: + OctreeNode* child_node_array_[8]; - ContainerT container_; - }; - } -} + ContainerT container_; +}; +} // namespace octree +} // namespace pcl diff --git a/octree/include/pcl/octree/octree_pointcloud.h b/octree/include/pcl/octree/octree_pointcloud.h index 583ec82d..929c3c4c 100644 --- a/octree/include/pcl/octree/octree_pointcloud.h +++ b/octree/include/pcl/octree/octree_pointcloud.h @@ -45,502 +45,563 @@ #include -namespace pcl -{ - namespace octree +namespace pcl { +namespace octree { + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** \brief @b Octree pointcloud class + * \note Octree implementation for pointclouds. Only indices are stored by the octree + * leaf nodes (zero-copy). + * \note The octree pointcloud class needs to be initialized with its voxel resolution. + * Its bounding box is automatically adjusted + * \note according to the pointcloud dimension or it can be predefined. + * \note Note: The tree depth equates to the resolution and the bounding box dimensions + * of the octree. + * \tparam PointT: type of point used in pointcloud + * \tparam LeafContainerT: leaf node container + * \tparam BranchContainerT: branch node container + * \tparam OctreeT: octree implementation + * \ingroup octree + * \author Julius Kammerl * (julius@kammerl.de) + */ +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template > + +class OctreePointCloud : public OctreeT { +public: + using Base = OctreeT; + + using LeafNode = typename OctreeT::LeafNode; + using BranchNode = typename OctreeT::BranchNode; + + /** \brief Octree pointcloud constructor. + * \param[in] resolution_arg octree resolution at lowest octree level + */ + OctreePointCloud(const double resolution_arg); + + // public typedefs + using IndicesPtr = shared_ptr>; + using IndicesConstPtr = shared_ptr>; + + using PointCloud = pcl::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + + // public typedefs for single/double buffering + using SingleBuffer = OctreePointCloud>; + // using DoubleBuffer = OctreePointCloud >; + + // Boost shared pointers + using Ptr = + shared_ptr>; + using ConstPtr = shared_ptr< + const OctreePointCloud>; + + // Eigen aligned allocator + using AlignedPointTVector = std::vector>; + using AlignedPointXYZVector = + std::vector>; + + /** \brief Provide a pointer to the input data set. + * \param[in] cloud_arg the const boost shared pointer to a PointCloud message + * \param[in] indices_arg the point indices subset that is to be used from \a cloud - + * if 0 the whole point cloud is used + */ + inline void + setInputCloud(const PointCloudConstPtr& cloud_arg, + const IndicesConstPtr& indices_arg = IndicesConstPtr()) + { + input_ = cloud_arg; + indices_ = indices_arg; + } + + /** \brief Get a pointer to the vector of indices used. + * \return pointer to vector of indices used. + */ + inline IndicesConstPtr const + getIndices() const + { + return (indices_); + } + + /** \brief Get a pointer to the input point cloud dataset. + * \return pointer to pointcloud input class. + */ + inline PointCloudConstPtr + getInputCloud() const + { + return (input_); + } + + /** \brief Set the search epsilon precision (error bound) for nearest neighbors + * searches. + * \param[in] eps precision (error bound) for nearest neighbors searches + */ + inline void + setEpsilon(double eps) + { + epsilon_ = eps; + } + + /** \brief Get the search epsilon precision (error bound) for nearest neighbors + * searches. */ + inline double + getEpsilon() const + { + return (epsilon_); + } + + /** \brief Set/change the octree voxel resolution + * \param[in] resolution_arg side length of voxels at lowest tree level + */ + inline void + setResolution(double resolution_arg) + { + // octree needs to be empty to change its resolution + assert(this->leaf_count_ == 0); + + resolution_ = resolution_arg; + + getKeyBitSize(); + } + + /** \brief Get octree voxel resolution + * \return voxel resolution at lowest tree level + */ + inline double + getResolution() const + { + return (resolution_); + } + + /** \brief Get the maximum depth of the octree. + * \return depth_arg: maximum depth of octree + * */ + inline unsigned int + getTreeDepth() const { + return this->octree_depth_; + } + + /** \brief Add points from input point cloud to octree. */ + void + addPointsFromInputCloud(); + + /** \brief Add point at given index from input point cloud to octree. Index will be + * also added to indices vector. + * \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 point_idx_arg, IndicesPtr indices_arg); + + /** \brief Add point simultaneously to octree and input point cloud. + * \param[in] point_arg point to be added + * \param[in] cloud_arg pointer to input point cloud dataset (given by \a + * setInputCloud) + */ + void + addPointToCloud(const PointT& point_arg, PointCloudPtr cloud_arg); + + /** \brief Add point simultaneously to octree and input point cloud. A corresponding + * index will be added to the indices vector. + * \param[in] point_arg point to be added + * \param[in] cloud_arg pointer to input point cloud dataset (given by \a + * setInputCloud) + * \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); + + /** \brief Check if voxel at given point exist. + * \param[in] point_arg point to be checked + * \return "true" if voxel exist; "false" otherwise + */ + bool + isVoxelOccupiedAtPoint(const PointT& point_arg) const; + + /** \brief Delete the octree structure and its leaf nodes. + * */ + void + deleteTree() + { + // reset bounding box + 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] 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 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] point_idx_arg point to be checked + * \return "true" if voxel exist; "false" otherwise + */ + bool + isVoxelOccupiedAtPoint(const int& point_idx_arg) const; + + /** \brief Get a PointT vector of centers of all occupied voxels. + * \param[out] voxel_center_list_arg results are written to this vector of PointT + * elements + * \return number of occupied voxels + */ + int + 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 + * along the line with small steps. Voxels are ordered, from closest to + * furthest w.r.t. the origin. + * \param[in] origin origin of the line segment + * \param[in] end end of the line segment + * \param[out] voxel_center_list results are written to this vector of PointT elements + * \param[in] precision determines the size of the steps: step_size = + * octree_resolution x precision + * \return number of intersected voxels + */ + int + getApproxIntersectedVoxelCentersBySegment(const Eigen::Vector3f& origin, + const Eigen::Vector3f& end, + AlignedPointTVector& voxel_center_list, + float precision = 0.2); + + /** \brief Delete leaf node / voxel at given point + * \param[in] point_arg point addressing the voxel to be deleted. + */ + void + deleteVoxelAtPoint(const PointT& point_arg); + + /** \brief Delete leaf node / voxel at given point from input cloud + * \param[in] point_idx_arg index of point addressing the voxel to be deleted. + */ + void + deleteVoxelAtPoint(const int& point_idx_arg); + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + // Bounding box methods + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + /** \brief Investigate dimensions of pointcloud data set and define corresponding + * bounding box for octree. */ + void + defineBoundingBox(); + + /** \brief Define bounding box for octree + * \note Bounding box cannot be changed once the octree contains elements. + * \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 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] 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 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) + * \note Bounding box cannot be changed once the octree contains elements. + * \param[in] cubeLen_arg side length of bounding box cube. + */ + void + defineBoundingBox(const double cubeLen_arg); + + /** \brief Get bounding box for octree + * \note Bounding box cannot be changed once the octree contains elements. + * \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& 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] tree_depth_arg depth/level in octree + * \return squared diameter + */ + double + 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 + { + return getVoxelSquaredDiameter(this->octree_depth_); + } + + /** \brief Calculates the squared voxel cube side length at given tree depth + * \param[in] tree_depth_arg depth/level in octree + * \return squared voxel cube side length + */ + double + 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->octree_depth_); + } + + /** \brief Generate bounds of the current voxel of an octree iterator + * \param[in] iterator: octree iterator + * \param[out] min_pt lower bound of voxel + * \param[out] max_pt upper bound of voxel + */ + inline void + getVoxelBounds(const OctreeIteratorBase& iterator, + Eigen::Vector3f& min_pt, + Eigen::Vector3f& max_pt) const + { + this->genVoxelBoundsFromOctreeKey(iterator.getCurrentOctreeKey(), + iterator.getCurrentOctreeDepth(), + min_pt, + max_pt); + } + + /** \brief Enable dynamic octree structure + * \note Leaf nodes are kept as close to the root as possible and are only expanded + * if the number of DataT objects within a leaf node exceeds a fixed limit. + * \param maxObjsPerLeaf: maximum number of DataT objects per leaf + * */ + inline void + enableDynamicDepth(std::size_t maxObjsPerLeaf) + { + assert(this->leaf_count_ == 0); + max_objs_per_leaf_ = maxObjsPerLeaf; + + this->dynamic_depth_enabled_ = max_objs_per_leaf_ > 0; + } - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - /** \brief @b Octree pointcloud class - * \note Octree implementation for pointclouds. Only indices are stored by the octree leaf nodes (zero-copy). - * \note The octree pointcloud class needs to be initialized with its voxel resolution. Its bounding box is automatically adjusted - * \note according to the pointcloud dimension or it can be predefined. - * \note Note: The tree depth equates to the resolution and the bounding box dimensions of the octree. - * \note - * \note typename: PointT: type of point used in pointcloud - * \note typename: LeafContainerT: leaf node container ( - * \note typename: BranchContainerT: branch node container - * \note typename: OctreeT: octree implementation () - * \ingroup octree - * \author Julius Kammerl (julius@kammerl.de) - */ - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - template > - - class OctreePointCloud : public OctreeT - { - public: - using Base = OctreeT; - - using LeafNode = typename OctreeT::LeafNode; - using BranchNode = typename OctreeT::BranchNode; - - /** \brief Octree pointcloud constructor. - * \param[in] resolution_arg octree resolution at lowest octree level - */ - OctreePointCloud (const double resolution_arg); - - // public typedefs - using IndicesPtr = shared_ptr >; - using IndicesConstPtr = shared_ptr >; - - using PointCloud = pcl::PointCloud; - using PointCloudPtr = typename PointCloud::Ptr; - using PointCloudConstPtr = typename PointCloud::ConstPtr; - - // public typedefs for single/double buffering - using SingleBuffer = OctreePointCloud >; - // using DoubleBuffer = OctreePointCloud >; - - // Boost shared pointers - using Ptr = shared_ptr >; - using ConstPtr = shared_ptr >; - - // Eigen aligned allocator - using AlignedPointTVector = std::vector >; - using AlignedPointXYZVector = std::vector >; - - /** \brief Provide a pointer to the input data set. - * \param[in] cloud_arg the const boost shared pointer to a PointCloud message - * \param[in] indices_arg the point indices subset that is to be used from \a cloud - if 0 the whole point cloud is used - */ - inline void setInputCloud (const PointCloudConstPtr &cloud_arg, - const IndicesConstPtr &indices_arg = IndicesConstPtr ()) - { - input_ = cloud_arg; - indices_ = indices_arg; - } - - /** \brief Get a pointer to the vector of indices used. - * \return pointer to vector of indices used. - */ - inline IndicesConstPtr const getIndices () const - { - return (indices_); - } - - /** \brief Get a pointer to the input point cloud dataset. - * \return pointer to pointcloud input class. - */ - inline PointCloudConstPtr getInputCloud () const - { - return (input_); - } - - /** \brief Set the search epsilon precision (error bound) for nearest neighbors searches. - * \param[in] eps precision (error bound) for nearest neighbors searches - */ - inline void setEpsilon (double eps) - { - epsilon_ = eps; - } - - /** \brief Get the search epsilon precision (error bound) for nearest neighbors searches. */ - inline double getEpsilon () const - { - return (epsilon_); - } - - /** \brief Set/change the octree voxel resolution - * \param[in] resolution_arg side length of voxels at lowest tree level - */ - inline void setResolution (double resolution_arg) - { - // octree needs to be empty to change its resolution - assert( this->leaf_count_ == 0); - - resolution_ = resolution_arg; - - getKeyBitSize (); - } - - /** \brief Get octree voxel resolution - * \return voxel resolution at lowest tree level - */ - inline double getResolution () const - { - return (resolution_); - } - - /** \brief Get the maximum depth of the octree. - * \return depth_arg: maximum depth of octree - * */ - inline unsigned int getTreeDepth () const - { - return this->octree_depth_; - } - - /** \brief Add points from input point cloud to octree. */ - void - addPointsFromInputCloud (); - - /** \brief Add point at given index from input point cloud to octree. Index will be also added to indices vector. - * \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 point_idx_arg, IndicesPtr indices_arg); - - /** \brief Add point simultaneously to octree and input point cloud. - * \param[in] point_arg point to be added - * \param[in] cloud_arg pointer to input point cloud dataset (given by \a setInputCloud) - */ - void - addPointToCloud (const PointT& point_arg, PointCloudPtr cloud_arg); - - /** \brief Add point simultaneously to octree and input point cloud. A corresponding index will be added to the indices vector. - * \param[in] point_arg point to be added - * \param[in] cloud_arg pointer to input point cloud dataset (given by \a setInputCloud) - * \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); - - /** \brief Check if voxel at given point exist. - * \param[in] point_arg point to be checked - * \return "true" if voxel exist; "false" otherwise - */ - bool - isVoxelOccupiedAtPoint (const PointT& point_arg) const; - - /** \brief Delete the octree structure and its leaf nodes. - * */ - void deleteTree () - { - // reset bounding box - 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] 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 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] point_idx_arg point to be checked - * \return "true" if voxel exist; "false" otherwise - */ - bool - isVoxelOccupiedAtPoint (const int& point_idx_arg) const; - - /** \brief Get a PointT vector of centers of all occupied voxels. - * \param[out] voxel_center_list_arg results are written to this vector of PointT elements - * \return number of occupied voxels - */ - int - 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 - * along the line with small steps. Voxels are ordered, from closest to - * furthest w.r.t. the origin. - * \param[in] origin origin of the line segment - * \param[in] end end of the line segment - * \param[out] voxel_center_list results are written to this vector of PointT elements - * \param[in] precision determines the size of the steps: step_size = octree_resolution x precision - * \return number of intersected voxels - */ - int - getApproxIntersectedVoxelCentersBySegment ( - const Eigen::Vector3f& origin, const Eigen::Vector3f& end, - AlignedPointTVector &voxel_center_list, float precision = 0.2); - - /** \brief Delete leaf node / voxel at given point - * \param[in] point_arg point addressing the voxel to be deleted. - */ - void - deleteVoxelAtPoint (const PointT& point_arg); - - /** \brief Delete leaf node / voxel at given point from input cloud - * \param[in] point_idx_arg index of point addressing the voxel to be deleted. - */ - void - deleteVoxelAtPoint (const int& point_idx_arg); - - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - // Bounding box methods - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - - /** \brief Investigate dimensions of pointcloud data set and define corresponding bounding box for octree. */ - void - defineBoundingBox (); - - /** \brief Define bounding box for octree - * \note Bounding box cannot be changed once the octree contains elements. - * \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 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] 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 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) - * \note Bounding box cannot be changed once the octree contains elements. - * \param[in] cubeLen_arg side length of bounding box cube. - */ - void - defineBoundingBox (const double cubeLen_arg); - - /** \brief Get bounding box for octree - * \note Bounding box cannot be changed once the octree contains elements. - * \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& 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] tree_depth_arg depth/level in octree - * \return squared diameter - */ - double - 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 - { - return getVoxelSquaredDiameter (this->octree_depth_); - } - - /** \brief Calculates the squared voxel cube side length at given tree depth - * \param[in] tree_depth_arg depth/level in octree - * \return squared voxel cube side length - */ - double - 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->octree_depth_); - } - - /** \brief Generate bounds of the current voxel of an octree iterator - * \param[in] iterator: octree iterator - * \param[out] min_pt lower bound of voxel - * \param[out] max_pt upper bound of voxel - */ - inline void - getVoxelBounds (const OctreeIteratorBase& iterator, Eigen::Vector3f &min_pt, Eigen::Vector3f &max_pt) const - { - this->genVoxelBoundsFromOctreeKey (iterator.getCurrentOctreeKey (), - iterator.getCurrentOctreeDepth (), min_pt, max_pt); - } - - /** \brief Enable dynamic octree structure - * \note Leaf nodes are kept as close to the root as possible and are only expanded if the number of DataT objects within a leaf node exceeds a fixed limit. - * \param maxObjsPerLeaf: maximum number of DataT objects per leaf - * */ - inline void - enableDynamicDepth ( std::size_t maxObjsPerLeaf ) - { - assert(this->leaf_count_==0); - max_objs_per_leaf_ = maxObjsPerLeaf; - - this->dynamic_depth_enabled_ = max_objs_per_leaf_ > 0; - } - - - protected: - - /** \brief Add point at index from input pointcloud dataset to octree - * \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 point_idx_arg); - - /** \brief Add point at index from input pointcloud dataset to octree - * \param[in] leaf_node to be expanded - * \param[in] parent_branch parent of leaf node to be expanded - * \param[in] child_idx child index of leaf node (in parent branch) - * \param[in] depth_mask of leaf node to be expanded - */ - void - expandLeafNode (LeafNode* leaf_node, BranchNode* parent_branch, unsigned char child_idx, unsigned int depth_mask); - - /** \brief Get point at index from input pointcloud dataset - * \param[in] index_arg index representing the point in the dataset given by \a setInputCloud - * \return PointT from input pointcloud dataset - */ - const PointT& - getPointByIndex (const unsigned int index_arg) const; - - /** \brief Find octree leaf node at a given point - * \param[in] point_arg query point - * \return pointer to leaf node. If leaf node does not exist, pointer is 0. - */ - LeafContainerT* - findLeafAtPoint (const PointT& point_arg) const - { - OctreeKey key; - - // generate key for point - this->genOctreeKeyforPoint (point_arg, key); - - return (this->findLeaf (key)); - } - - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - // Protected octree methods based on octree keys - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - - /** \brief Define octree key setting and octree depth based on defined bounding box. */ - void - getKeyBitSize (); - - /** \brief Grow the bounding box/octree until point fits - * \param[in] point_idx_arg point that should be within bounding box; - */ - void - adoptBoundingBoxToPoint (const PointT& point_idx_arg); - - /** \brief Checks if given point is within the bounding box of the octree - * \param[in] point_idx_arg point to be checked for bounding box violations - * \return "true" - no bound violation - */ - inline bool isPointWithinBoundingBox (const PointT& point_idx_arg) const - { - 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 - * \param[in] point_arg the point addressing a voxel - * \param[out] key_arg write octree key to this reference - */ - void - genOctreeKeyforPoint (const PointT & point_arg, - OctreeKey &key_arg) const; - - /** \brief Generate octree key for voxel at a given point - * \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 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. - * \param[in] data_arg index value representing a point in the dataset given by \a setInputCloud - * \param[out] key_arg write octree key to this reference - * \return "true" - octree keys are assignable - */ - virtual bool - genOctreeKeyForDataT (const int& data_arg, OctreeKey & key_arg) const; - - /** \brief Generate a point at center of leaf node voxel - * \param[in] key_arg octree key addressing a leaf node. - * \param[out] point_arg write leaf node voxel center to this point reference - */ - void - genLeafNodeCenterFromOctreeKey (const OctreeKey & key_arg, - PointT& point_arg) const; - - /** \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] 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 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] 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 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] 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 &voxel_center_list_arg) const; - - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - // Globals - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - /** \brief Pointer to input point cloud dataset. */ - PointCloudConstPtr input_; - - /** \brief A pointer to the vector of point indices to use. */ - IndicesConstPtr indices_; - - /** \brief Epsilon precision (error bound) for nearest neighbors searches. */ - double epsilon_; - - /** \brief Octree resolution. */ - double resolution_; - - // Octree bounding box coordinates - double min_x_; - double max_x_; - - double min_y_; - double max_y_; - - double min_z_; - double max_z_; - - /** \brief Flag indicating if octree has defined bounding box. */ - 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 max_objs_per_leaf_; - }; +protected: + /** \brief Add point at index from input pointcloud dataset to octree + * \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 point_idx_arg); + + /** \brief Add point at index from input pointcloud dataset to octree + * \param[in] leaf_node to be expanded + * \param[in] parent_branch parent of leaf node to be expanded + * \param[in] child_idx child index of leaf node (in parent branch) + * \param[in] depth_mask of leaf node to be expanded + */ + void + expandLeafNode(LeafNode* leaf_node, + BranchNode* parent_branch, + unsigned char child_idx, + unsigned int depth_mask); + + /** \brief Get point at index from input pointcloud dataset + * \param[in] index_arg index representing the point in the dataset given by \a + * setInputCloud + * \return PointT from input pointcloud dataset + */ + const PointT& + getPointByIndex(const unsigned int index_arg) const; + + /** \brief Find octree leaf node at a given point + * \param[in] point_arg query point + * \return pointer to leaf node. If leaf node does not exist, pointer is 0. + */ + LeafContainerT* + findLeafAtPoint(const PointT& point_arg) const + { + OctreeKey key; + + // generate key for point + this->genOctreeKeyforPoint(point_arg, key); + + return (this->findLeaf(key)); + } + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + // Protected octree methods based on octree keys + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + /** \brief Define octree key setting and octree depth based on defined bounding box. + */ + void + getKeyBitSize(); + + /** \brief Grow the bounding box/octree until point fits + * \param[in] point_idx_arg point that should be within bounding box; + */ + void + adoptBoundingBoxToPoint(const PointT& point_idx_arg); + + /** \brief Checks if given point is within the bounding box of the octree + * \param[in] point_idx_arg point to be checked for bounding box violations + * \return "true" - no bound violation + */ + inline bool + isPointWithinBoundingBox(const PointT& point_idx_arg) const + { + 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 + * \param[in] point_arg the point addressing a voxel + * \param[out] key_arg write octree key to this reference + */ + void + genOctreeKeyforPoint(const PointT& point_arg, OctreeKey& key_arg) const; + + /** \brief Generate octree key for voxel at a given point + * \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 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. + * \param[in] data_arg index value representing a point in the dataset given by \a + * setInputCloud + * \param[out] key_arg write octree key to this reference \return "true" - octree keys + * are assignable + */ + virtual bool + genOctreeKeyForDataT(const int& data_arg, OctreeKey& key_arg) const; + + /** \brief Generate a point at center of leaf node voxel + * \param[in] key_arg octree key addressing a leaf node. + * \param[out] point_arg write leaf node voxel center to this point reference + */ + void + genLeafNodeCenterFromOctreeKey(const OctreeKey& key_arg, PointT& point_arg) const; + + /** \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] 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 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] 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 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] 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& voxel_center_list_arg) const; + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + // Globals + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /** \brief Pointer to input point cloud dataset. */ + PointCloudConstPtr input_; + + /** \brief A pointer to the vector of point indices to use. */ + IndicesConstPtr indices_; + + /** \brief Epsilon precision (error bound) for nearest neighbors searches. */ + double epsilon_; + + /** \brief Octree resolution. */ + double resolution_; + + // Octree bounding box coordinates + double min_x_; + double max_x_; + + double min_y_; + double max_y_; + + double min_z_; + double max_z_; + + /** \brief Flag indicating if octree has defined bounding box. */ + 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 max_objs_per_leaf_; +}; + +} // namespace octree +} // namespace pcl #ifdef PCL_NO_PRECOMPILE #include diff --git a/octree/include/pcl/octree/octree_pointcloud_adjacency.h b/octree/include/pcl/octree/octree_pointcloud_adjacency.h index 5a3f0ac1..64db9804 100644 --- a/octree/include/pcl/octree/octree_pointcloud_adjacency.h +++ b/octree/include/pcl/octree/octree_pointcloud_adjacency.h @@ -43,181 +43,208 @@ #include #include -#include #include +#include -namespace pcl -{ +namespace pcl { - namespace octree +namespace octree { +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** \brief @b Octree pointcloud voxel class which maintains adjacency information for + * its voxels. + * + * This pointcloud octree class generates an octree from a point cloud (zero-copy). The + * octree pointcloud is initialized with its voxel resolution. Its bounding box is + * automatically adjusted or can be predefined. + * + * The OctreePointCloudAdjacencyContainer class can be used to store data in leaf nodes. + * + * An optional transform function can be provided which changes how the voxel grid is + * computed - this can be used to, for example, make voxel bins larger as they increase + * in distance from the origin (camera). \note See SupervoxelClustering for an example + * of how to provide a transform function. + * + * If used in academic work, please cite: + * + * - J. Papon, A. Abramov, M. Schoeler, F. Woergoetter + * Voxel Cloud Connectivity Segmentation - Supervoxels from PointClouds + * In Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition + * (CVPR) 2013 + * + * \ingroup octree + * \author Jeremie Papon (jpapon@gmail.com) */ +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template , + typename BranchContainerT = OctreeContainerEmpty> +class OctreePointCloudAdjacency +: public OctreePointCloud { + +public: + using OctreeBaseT = OctreeBase; + + using OctreeAdjacencyT = + OctreePointCloudAdjacency; + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + + using OctreePointCloudT = + OctreePointCloud; + using LeafNode = typename OctreePointCloudT::LeafNode; + using BranchNode = typename OctreePointCloudT::BranchNode; + + using PointCloud = pcl::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + + // BGL graph + using VoxelAdjacencyList = boost:: + adjacency_list; + using VoxelID = typename VoxelAdjacencyList::vertex_descriptor; + using EdgeID = typename VoxelAdjacencyList::edge_descriptor; + + // Leaf vector - pointers to all leaves + using LeafVectorT = std::vector; + + // Fast leaf iterators that don't require traversing tree + using iterator = typename LeafVectorT::iterator; + using const_iterator = typename LeafVectorT::const_iterator; + + inline iterator + begin() { - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - /** \brief @b Octree pointcloud voxel class which maintains adjacency information for its voxels. - * - * This pointcloud octree class generates an octree from a point cloud (zero-copy). The octree pointcloud is - * initialized with its voxel resolution. Its bounding box is automatically adjusted or can be predefined. - * - * The OctreePointCloudAdjacencyContainer class can be used to store data in leaf nodes. - * - * An optional transform function can be provided which changes how the voxel grid is computed - this can be used to, - * for example, make voxel bins larger as they increase in distance from the origin (camera). - * \note See SupervoxelClustering for an example of how to provide a transform function. - * - * If used in academic work, please cite: - * - * - J. Papon, A. Abramov, M. Schoeler, F. Woergoetter - * Voxel Cloud Connectivity Segmentation - Supervoxels from PointClouds - * In Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition (CVPR) 2013 - * - * \ingroup octree - * \author Jeremie Papon (jpapon@gmail.com) */ - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - template , - typename BranchContainerT = OctreeContainerEmpty> - class OctreePointCloudAdjacency : public OctreePointCloud - { - - public: - - using OctreeBaseT = OctreeBase; - - using OctreeAdjacencyT = OctreePointCloudAdjacency; - using Ptr = shared_ptr; - using ConstPtr = shared_ptr; - - using OctreePointCloudT = OctreePointCloud; - using LeafNode = typename OctreePointCloudT::LeafNode; - using BranchNode = typename OctreePointCloudT::BranchNode; - - using PointCloud = pcl::PointCloud; - using PointCloudPtr = typename PointCloud::Ptr; - using PointCloudConstPtr = typename PointCloud::ConstPtr; - - // BGL graph - using VoxelAdjacencyList = boost::adjacency_list; - using VoxelID = typename VoxelAdjacencyList::vertex_descriptor; - using EdgeID = typename VoxelAdjacencyList::edge_descriptor; - - // Leaf vector - pointers to all leaves - using LeafVectorT = std::vector; - - // Fast leaf iterators that don't require traversing tree - using iterator = typename LeafVectorT::iterator; - using const_iterator = typename LeafVectorT::const_iterator; - - inline iterator begin () { return (leaf_vector_.begin ()); } - inline iterator end () { return (leaf_vector_.end ()); } - inline LeafContainerT* at (std::size_t idx) { return leaf_vector_.at (idx); } - - // Size of neighbors - inline std::size_t size () const { return leaf_vector_.size (); } - - /** \brief Constructor. - * - * \param[in] resolution_arg Octree resolution at lowest octree level (voxel size) */ - OctreePointCloudAdjacency (const double resolution_arg); - - /** \brief Adds points from cloud to the octree. - * - * \note This overrides addPointsFromInputCloud() from the OctreePointCloud class. */ - void - addPointsFromInputCloud (); - - /** \brief Gets the leaf container for a given point. - * - * \param[in] point_arg Point to search for - * - * \returns Pointer to the leaf container - null if no leaf container found. */ - LeafContainerT* - getLeafContainerAtPoint (const PointT& point_arg) const; - - /** \brief Computes an adjacency graph of voxel relations. - * - * \warning This slows down rapidly as cloud size increases due to the number of edges. - * - * \param[out] voxel_adjacency_graph Boost Graph Library Adjacency graph of the voxel touching relationships. - * Vertices are PointT, edges represent touching, and edge lengths are the distance between the points. */ - void - computeVoxelAdjacencyGraph (VoxelAdjacencyList &voxel_adjacency_graph); - - /** \brief Sets a point transform (and inverse) used to transform the space of the input cloud. - * - * This is useful for changing how adjacency is calculated - such as relaxing the adjacency criterion for - * points further from the camera. - * - * \param[in] transform_func A boost:function pointer to the transform to be used. The transform must have one - * parameter (a point) which it modifies in place. */ - void - setTransformFunction (std::function transform_func) - { - transform_func_ = transform_func; - } - - /** \brief Tests whether input point is occluded from specified camera point by other voxels. - * - * \param[in] point_arg Point to test for - * \param[in] camera_pos Position of camera, defaults to origin - * - * \returns True if path to camera is blocked by a voxel, false otherwise. */ - bool - testForOcclusion (const PointT& point_arg, const PointXYZ &camera_pos = PointXYZ (0, 0, 0)); - - protected: - - /** \brief Add point at index from input pointcloud dataset to octree. - * - * \param[in] point_idx_arg The index representing the point in the dataset given by setInputCloud() to be added - * - * \note This virtual implementation allows the use of a transform function to compute keys. */ - void - addPointIdx (const int point_idx_arg) override; - - /** \brief Fills in the neighbors fields for new voxels. - * - * \param[in] key_arg Key of the voxel to check neighbors for - * \param[in] leaf_container Pointer to container of the leaf to check neighbors for */ - void - computeNeighbors (OctreeKey &key_arg, LeafContainerT* leaf_container); - - /** \brief Generates octree key for specified point (uses transform if provided). - * - * \param[in] point_arg Point to generate key for - * \param[out] key_arg Resulting octree key */ - void - genOctreeKeyforPoint (const PointT& point_arg, OctreeKey& key_arg) const; - - private: - - /** \brief Add point at given index from input point cloud to octree. - * - * Index will be also added to indices vector. This functionality is not enabled for adjacency octree. */ - using OctreePointCloudT::addPointFromCloud; - - /** \brief Add point simultaneously to octree and input point cloud. - * - * This functionality is not enabled for adjacency octree. */ - using OctreePointCloudT::addPointToCloud; - - using OctreePointCloudT::input_; - using OctreePointCloudT::resolution_; - using OctreePointCloudT::min_x_; - using OctreePointCloudT::min_y_; - using OctreePointCloudT::min_z_; - using OctreePointCloudT::max_x_; - using OctreePointCloudT::max_y_; - using OctreePointCloudT::max_z_; - - /// Local leaf pointer vector used to make iterating through leaves fast. - LeafVectorT leaf_vector_; - - std::function transform_func_; - - }; + return (leaf_vector_.begin()); + } + inline iterator + end() + { + return (leaf_vector_.end()); + } + inline LeafContainerT* + at(std::size_t idx) + { + return leaf_vector_.at(idx); + } + // Size of neighbors + inline std::size_t + size() const + { + return leaf_vector_.size(); } -} + /** \brief Constructor. + * + * \param[in] resolution_arg Octree resolution at lowest octree level (voxel size) */ + OctreePointCloudAdjacency(const double resolution_arg); + + /** \brief Adds points from cloud to the octree. + * + * \note This overrides addPointsFromInputCloud() from the OctreePointCloud class. */ + void + addPointsFromInputCloud(); + + /** \brief Gets the leaf container for a given point. + * + * \param[in] point_arg Point to search for + * + * \returns Pointer to the leaf container - null if no leaf container found. */ + LeafContainerT* + getLeafContainerAtPoint(const PointT& point_arg) const; + + /** \brief Computes an adjacency graph of voxel relations. + * + * \warning This slows down rapidly as cloud size increases due to the number of + * edges. + * + * \param[out] voxel_adjacency_graph Boost Graph Library Adjacency graph of the voxel + * touching relationships. Vertices are PointT, edges represent touching, and edge + * lengths are the distance between the points. */ + void + computeVoxelAdjacencyGraph(VoxelAdjacencyList& voxel_adjacency_graph); + + /** \brief Sets a point transform (and inverse) used to transform the space of the + * input cloud. + * + * This is useful for changing how adjacency is calculated - such as relaxing the + * adjacency criterion for points further from the camera. + * + * \param[in] transform_func A boost:function pointer to the transform to be used. The + * transform must have one parameter (a point) which it modifies in place. */ + void + setTransformFunction(std::function transform_func) + { + transform_func_ = transform_func; + } -// Note: Do not precompile this octree type because it is typically used with custom leaf containers. + /** \brief Tests whether input point is occluded from specified camera point by other + * voxels. + * + * \param[in] point_arg Point to test for + * \param[in] camera_pos Position of camera, defaults to origin + * + * \returns True if path to camera is blocked by a voxel, false otherwise. */ + bool + testForOcclusion(const PointT& point_arg, + const PointXYZ& camera_pos = PointXYZ(0, 0, 0)); + +protected: + /** \brief Add point at index from input pointcloud dataset to octree. + * + * \param[in] point_idx_arg The index representing the point in the dataset given by + * setInputCloud() to be added + * + * \note This virtual implementation allows the use of a transform function to compute + * keys. */ + void + addPointIdx(const int point_idx_arg) override; + + /** \brief Fills in the neighbors fields for new voxels. + * + * \param[in] key_arg Key of the voxel to check neighbors for + * \param[in] leaf_container Pointer to container of the leaf to check neighbors for + */ + void + computeNeighbors(OctreeKey& key_arg, LeafContainerT* leaf_container); + + /** \brief Generates octree key for specified point (uses transform if provided). + * + * \param[in] point_arg Point to generate key for + * \param[out] key_arg Resulting octree key */ + void + genOctreeKeyforPoint(const PointT& point_arg, OctreeKey& key_arg) const; + +private: + /** \brief Add point at given index from input point cloud to octree. + * + * Index will be also added to indices vector. This functionality is not enabled for + * adjacency octree. */ + using OctreePointCloudT::addPointFromCloud; + + /** \brief Add point simultaneously to octree and input point cloud. + * + * This functionality is not enabled for adjacency octree. */ + using OctreePointCloudT::addPointToCloud; + + using OctreePointCloudT::input_; + using OctreePointCloudT::max_x_; + using OctreePointCloudT::max_y_; + using OctreePointCloudT::max_z_; + using OctreePointCloudT::min_x_; + using OctreePointCloudT::min_y_; + using OctreePointCloudT::min_z_; + using OctreePointCloudT::resolution_; + + /// Local leaf pointer vector used to make iterating through leaves fast. + LeafVectorT leaf_vector_; + + std::function transform_func_; +}; + +} // namespace octree + +} // namespace pcl + +// Note: Do not precompile this octree type because it is typically used with custom +// leaf containers. #include diff --git a/octree/include/pcl/octree/octree_pointcloud_adjacency_container.h b/octree/include/pcl/octree/octree_pointcloud_adjacency_container.h index 59fcddf2..b6e5ad62 100644 --- a/octree/include/pcl/octree/octree_pointcloud_adjacency_container.h +++ b/octree/include/pcl/octree/octree_pointcloud_adjacency_container.h @@ -39,159 +39,186 @@ #pragma once -namespace pcl -{ - - namespace octree - { - /** \brief @b Octree adjacency leaf container class- stores a list of pointers to neighbors, number of points added, and a DataT value - * \note This class implements a leaf node that stores pointers to neighboring leaves - * \note This class also has a virtual computeData function, which is called by octreePointCloudAdjacency::addPointsFromInputCloud. - * \note You should make explicit instantiations of it for your pointtype/datatype combo (if needed) see supervoxel_clustering.hpp for an example of this - */ - template - class OctreePointCloudAdjacencyContainer : public OctreeContainerBase - { - template - friend class OctreePointCloudAdjacency; - public: - using NeighborListT = std::list *>; - using const_iterator = typename NeighborListT::const_iterator; - //const iterators to neighbors - inline const_iterator cbegin () const { return (neighbors_.begin ()); } - inline const_iterator cend () const { return (neighbors_.end ()); } - //size of neighbors - inline std::size_t size () const { return neighbors_.size (); } - - /** \brief Class initialization. */ - OctreePointCloudAdjacencyContainer () : - OctreeContainerBase () - { - this->reset(); - } - - /** \brief Empty class deconstructor. */ - ~OctreePointCloudAdjacencyContainer () - { - } - - /** \brief Returns the number of neighbors this leaf has - * \returns number of neighbors - */ - std::size_t - getNumNeighbors () const - { - return neighbors_.size (); - } +namespace pcl { - /** \brief Gets the number of points contributing to this leaf */ - int - getPointCounter () const { return num_points_; } - - /** \brief Returns a reference to the data member to access it without copying */ - DataT& - getData () { return data_; } - - /** \brief Sets the data member - * \param[in] data_arg New value for data - */ - void - setData (const DataT& data_arg) { data_ = data_arg;} - - /** \brief virtual method to get size of container - * \return number of points added to leaf node container. - */ - std::size_t - getSize () const override - { - return num_points_; - } - protected: - //iterators to neighbors - using iterator = typename NeighborListT::iterator; - inline iterator begin () { return (neighbors_.begin ()); } - inline iterator end () { return (neighbors_.end ()); } - - /** \brief deep copy function */ - virtual OctreePointCloudAdjacencyContainer * - deepCopy () const - { - OctreePointCloudAdjacencyContainer *new_container = new OctreePointCloudAdjacencyContainer; - new_container->setNeighbors (this->neighbors_); - new_container->setPointCounter (this->num_points_); - return new_container; - } - - /** \brief Add new point to container- this just counts points - * \note To actually store data in the leaves, need to specialize this - * for your point and data type as in supervoxel_clustering.hpp - */ - // param[in] new_point the new point to add - void - addPoint (const PointInT& /*new_point*/) - { - using namespace pcl::common; - ++num_points_; - } - - /** \brief Function for working on data added. Base implementation does nothing - * */ - void - computeData () - { - } - - /** \brief Sets the number of points contributing to this leaf */ - void - setPointCounter (int points_arg) { num_points_ = points_arg; } - - /** \brief Clear the voxel centroid */ - void - reset () override - { - neighbors_.clear (); - num_points_ = 0; - data_ = DataT (); - } - - /** \brief Add new neighbor to voxel. - * \param[in] neighbor the new neighbor to add - */ - void - addNeighbor (OctreePointCloudAdjacencyContainer *neighbor) - { - neighbors_.push_back (neighbor); - } - - /** \brief Remove neighbor from neighbor set. - * \param[in] neighbor the neighbor to remove - */ - void - removeNeighbor (OctreePointCloudAdjacencyContainer *neighbor) - { - for (iterator neighb_it = neighbors_.begin(); neighb_it != neighbors_.end(); ++neighb_it) - { - if ( *neighb_it == neighbor) - { - neighbors_.erase (neighb_it); - return; - } - } - } - - /** \brief Sets the whole neighbor set - * \param[in] neighbor_arg the new set - */ - void - setNeighbors (const NeighborListT &neighbor_arg) - { - neighbors_ = neighbor_arg; +namespace octree { +/** \brief @b Octree adjacency leaf container class- stores a list of pointers to + * neighbors, number of points added, and a DataT value \note This class implements a + * leaf node that stores pointers to neighboring leaves \note This class also has a + * virtual computeData function, which is called by + * octreePointCloudAdjacency::addPointsFromInputCloud. \note You should make explicit + * instantiations of it for your pointtype/datatype combo (if needed) see + * supervoxel_clustering.hpp for an example of this + */ +template +class OctreePointCloudAdjacencyContainer : public OctreeContainerBase { + template + friend class OctreePointCloudAdjacency; + +public: + using NeighborListT = std::list*>; + using const_iterator = typename NeighborListT::const_iterator; + // const iterators to neighbors + inline const_iterator + cbegin() const + { + return (neighbors_.begin()); + } + inline const_iterator + cend() const + { + return (neighbors_.end()); + } + // size of neighbors + inline std::size_t + size() const + { + return neighbors_.size(); + } + + /** \brief Class initialization. */ + OctreePointCloudAdjacencyContainer() : OctreeContainerBase() { this->reset(); } + + /** \brief Empty class deconstructor. */ + ~OctreePointCloudAdjacencyContainer() {} + + /** \brief Returns the number of neighbors this leaf has + * \returns number of neighbors + */ + std::size_t + getNumNeighbors() const + { + return neighbors_.size(); + } + + /** \brief Gets the number of points contributing to this leaf */ + int + getPointCounter() const + { + return num_points_; + } + + /** \brief Returns a reference to the data member to access it without copying */ + DataT& + getData() + { + return data_; + } + + /** \brief Sets the data member + * \param[in] data_arg New value for data + */ + void + setData(const DataT& data_arg) + { + data_ = data_arg; + } + + /** \brief virtual method to get size of container + * \return number of points added to leaf node container. + */ + std::size_t + getSize() const override + { + return num_points_; + } + +protected: + // iterators to neighbors + using iterator = typename NeighborListT::iterator; + inline iterator + begin() + { + return (neighbors_.begin()); + } + inline iterator + end() + { + return (neighbors_.end()); + } + + /** \brief deep copy function */ + virtual OctreePointCloudAdjacencyContainer* + deepCopy() const + { + OctreePointCloudAdjacencyContainer* new_container = + new OctreePointCloudAdjacencyContainer; + new_container->setNeighbors(this->neighbors_); + new_container->setPointCounter(this->num_points_); + return new_container; + } + + /** \brief Add new point to container- this just counts points + * \note To actually store data in the leaves, need to specialize this + * for your point and data type as in supervoxel_clustering.hpp + */ + // param[in] new_point the new point to add + void + addPoint(const PointInT& /*new_point*/) + { + using namespace pcl::common; + ++num_points_; + } + + /** \brief Function for working on data added. Base implementation does nothing + * */ + void + computeData() + {} + + /** \brief Sets the number of points contributing to this leaf */ + void + setPointCounter(int points_arg) + { + num_points_ = points_arg; + } + + /** \brief Clear the voxel centroid */ + void + reset() override + { + neighbors_.clear(); + num_points_ = 0; + data_ = DataT(); + } + + /** \brief Add new neighbor to voxel. + * \param[in] neighbor the new neighbor to add + */ + void + addNeighbor(OctreePointCloudAdjacencyContainer* neighbor) + { + neighbors_.push_back(neighbor); + } + + /** \brief Remove neighbor from neighbor set. + * \param[in] neighbor the neighbor to remove + */ + void + removeNeighbor(OctreePointCloudAdjacencyContainer* neighbor) + { + for (iterator neighb_it = neighbors_.begin(); neighb_it != neighbors_.end(); + ++neighb_it) { + if (*neighb_it == neighbor) { + neighbors_.erase(neighb_it); + return; } - - private: - int num_points_; - NeighborListT neighbors_; - DataT data_; - }; - } -} + } + } + + /** \brief Sets the whole neighbor set + * \param[in] neighbor_arg the new set + */ + void + setNeighbors(const NeighborListT& neighbor_arg) + { + neighbors_ = neighbor_arg; + } + +private: + int num_points_; + NeighborListT neighbors_; + DataT data_; +}; +} // namespace octree +} // namespace pcl diff --git a/octree/include/pcl/octree/octree_pointcloud_changedetector.h b/octree/include/pcl/octree/octree_pointcloud_changedetector.h index 7f3b96d4..8d2a3b4a 100644 --- a/octree/include/pcl/octree/octree_pointcloud_changedetector.h +++ b/octree/include/pcl/octree/octree_pointcloud_changedetector.h @@ -40,69 +40,75 @@ #include -#include #include +#include -namespace pcl -{ - namespace octree - { - - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - /** \brief @b Octree pointcloud change detector class - * \note This pointcloud octree class generate an octrees from a point cloud (zero-copy). It allows to detect new leaf nodes and serialize their point indices - * \note The octree pointcloud is initialized with its voxel resolution. Its bounding box is automatically adjusted or can be predefined. - * \note - * \note typename: PointT: type of point used in pointcloud - * \ingroup octree - * \author Julius Kammerl (julius@kammerl.de) - */ - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - template +namespace pcl { +namespace octree { - class OctreePointCloudChangeDetector : public OctreePointCloud > +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** \brief @b Octree pointcloud change detector class + * \note This pointcloud octree class generate an octrees from a point cloud + * (zero-copy). It allows to detect new leaf nodes and serialize their point indices + * \note The octree pointcloud is initialized with its voxel resolution. Its bounding + * box is automatically adjusted or can be predefined. + * \tparam PointT type of point used in pointcloud + * \ingroup octree + * \author Julius Kammerl (julius@kammerl.de) + */ +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template - { +class OctreePointCloudChangeDetector +: public OctreePointCloud> - public: +{ - using Ptr = shared_ptr>; - using ConstPtr = shared_ptr>; +public: + using Ptr = shared_ptr< + OctreePointCloudChangeDetector>; + using ConstPtr = shared_ptr< + const OctreePointCloudChangeDetector>; - /** \brief Constructor. - * \param resolution_arg: octree resolution at lowest octree level - * */ - OctreePointCloudChangeDetector (const double resolution_arg) : - OctreePointCloud > (resolution_arg) - { - } + /** \brief Constructor. + * \param resolution_arg: octree resolution at lowest octree level + * */ + OctreePointCloudChangeDetector(const double resolution_arg) + : OctreePointCloud>(resolution_arg) + {} - /** \brief Get a indices from all leaf nodes that did not exist in previous buffer. - * \param indicesVector_arg: results are written to this vector of int indices - * \param minPointsPerLeaf_arg: minimum amount of points required within leaf node to become serialized. - * \return number of point indices - */ - std::size_t getPointIndicesFromNewVoxels (std::vector &indicesVector_arg, - const int minPointsPerLeaf_arg = 0) - { + /** \brief Get a indices from all leaf nodes that did not exist in previous buffer. + * \param indicesVector_arg: results are written to this vector of int indices + * \param minPointsPerLeaf_arg: minimum amount of points required within leaf node to + * become serialized. + * \return number of point indices + */ + std::size_t + getPointIndicesFromNewVoxels(std::vector& indicesVector_arg, + const int minPointsPerLeaf_arg = 0) + { - std::vector leaf_containers; - this->serializeNewLeafs (leaf_containers); + std::vector leaf_containers; + this->serializeNewLeafs(leaf_containers); - for (const auto &leaf_container : leaf_containers) - { - if (static_cast (leaf_container->getSize ()) >= minPointsPerLeaf_arg) - leaf_container->getPointIndices(indicesVector_arg); - } + for (const auto& leaf_container : leaf_containers) { + if (static_cast(leaf_container->getSize()) >= minPointsPerLeaf_arg) + leaf_container->getPointIndices(indicesVector_arg); + } - return (indicesVector_arg.size ()); - } - }; + return (indicesVector_arg.size()); } -} +}; +} // namespace octree +} // namespace pcl -#define PCL_INSTANTIATE_OctreePointCloudChangeDetector(T) template class PCL_EXPORTS pcl::octree::OctreePointCloudChangeDetector; +#define PCL_INSTANTIATE_OctreePointCloudChangeDetector(T) \ + template class PCL_EXPORTS pcl::octree::OctreePointCloudChangeDetector; diff --git a/octree/include/pcl/octree/octree_pointcloud_density.h b/octree/include/pcl/octree/octree_pointcloud_density.h index c0277d78..19e97d04 100644 --- a/octree/include/pcl/octree/octree_pointcloud_density.h +++ b/octree/include/pcl/octree/octree_pointcloud_density.h @@ -40,119 +40,114 @@ #include -namespace pcl -{ - namespace octree +namespace pcl { +namespace octree { +/** \brief @b Octree pointcloud density leaf node class + * \note This class implements a leaf node that counts the amount of points which fall + * into its voxel space. + * \author Julius Kammerl (julius@kammerl.de) + */ +class OctreePointCloudDensityContainer : public OctreeContainerBase { +public: + /** \brief Class initialization. */ + OctreePointCloudDensityContainer() : point_counter_(0) {} + + /** \brief Empty class deconstructor. */ + ~OctreePointCloudDensityContainer() {} + + /** \brief deep copy function */ + virtual OctreePointCloudDensityContainer* + deepCopy() const + { + return (new OctreePointCloudDensityContainer(*this)); + } + + /** \brief Equal comparison operator + * \param[in] other OctreePointCloudDensityContainer to compare with + */ + bool + operator==(const OctreeContainerBase& other) const override { - /** \brief @b Octree pointcloud density leaf node class - * \note This class implements a leaf node that counts the amount of points which fall into its voxel space. - * \author Julius Kammerl (julius@kammerl.de) - */ - class OctreePointCloudDensityContainer : public OctreeContainerBase - { - public: - /** \brief Class initialization. */ - OctreePointCloudDensityContainer () : point_counter_ (0) - { - } - - /** \brief Empty class deconstructor. */ - ~OctreePointCloudDensityContainer () - { - } - - /** \brief deep copy function */ - virtual OctreePointCloudDensityContainer * - deepCopy () const - { - return (new OctreePointCloudDensityContainer (*this)); - } - - /** \brief Equal comparison operator - * \param[in] other OctreePointCloudDensityContainer to compare with - */ - bool operator==(const OctreeContainerBase& other) const override - { - const OctreePointCloudDensityContainer* otherContainer = - dynamic_cast(&other); - - return (this->point_counter_==otherContainer->point_counter_); - } - - /** \brief Read input data. Only an internal counter is increased. - */ - void - addPointIndex (int) - { - point_counter_++; - } - - /** \brief Return point counter. - * \return Amount of points - */ - unsigned int - getPointCounter () - { - return (point_counter_); - } - - /** \brief Reset leaf node. */ - void - reset () override - { - point_counter_ = 0; - } - - private: - unsigned int point_counter_; - - }; - - /** \brief @b Octree pointcloud density class - * \note This class generate an octrees from a point cloud (zero-copy). Only the amount of points that fall into the leaf node voxel are stored. - * \note The octree pointcloud is initialized with its voxel resolution. Its bounding box is automatically adjusted or can be predefined. - * \note - * \note typename: PointT: type of point used in pointcloud - * \ingroup octree - * \author Julius Kammerl (julius@kammerl.de) - */ - template - class OctreePointCloudDensity : public OctreePointCloud - { - public: - - /** \brief OctreePointCloudDensity class constructor. - * \param resolution_arg: octree resolution at lowest octree level - * */ - OctreePointCloudDensity (const double resolution_arg) : - OctreePointCloud (resolution_arg) - { - } - - /** \brief Empty class deconstructor. */ - - ~OctreePointCloudDensity () - { - } - - /** \brief Get the amount of points within a leaf node voxel which is addressed by a point - * \param[in] point_arg: a point addressing a voxel - * \return amount of points that fall within leaf node voxel - */ - unsigned int - getVoxelDensityAtPoint (const PointT& point_arg) const - { - unsigned int point_count = 0; - - OctreePointCloudDensityContainer* leaf = this->findLeafAtPoint (point_arg); - - if (leaf) - point_count = leaf->getPointCounter (); - - return (point_count); - } - }; + const OctreePointCloudDensityContainer* otherContainer = + dynamic_cast(&other); + + return (this->point_counter_ == otherContainer->point_counter_); + } + + /** \brief Read input data. Only an internal counter is increased. + */ + void + addPointIndex(int) + { + point_counter_++; + } + + /** \brief Return point counter. + * \return Amount of points + */ + unsigned int + getPointCounter() + { + return (point_counter_); + } + + /** \brief Reset leaf node. */ + void + reset() override + { + point_counter_ = 0; + } + +private: + unsigned int point_counter_; +}; + +/** \brief @b Octree pointcloud density class + * \note This class generate an octrees from a point cloud (zero-copy). Only the amount + * of points that fall into the leaf node voxel are stored. + * \note The octree pointcloud is initialized with its voxel resolution. Its bounding + * box is automatically adjusted or can be predefined. + * \tparam PointT type of point used in pointcloud + * \ingroup octree + * \author Julius Kammerl (julius@kammerl.de) + */ +template +class OctreePointCloudDensity +: public OctreePointCloud { +public: + /** \brief OctreePointCloudDensity class constructor. + * \param resolution_arg: octree resolution at lowest octree level + * */ + OctreePointCloudDensity(const double resolution_arg) + : OctreePointCloud(resolution_arg) + {} + + /** \brief Empty class deconstructor. */ + + ~OctreePointCloudDensity() {} + + /** \brief Get the amount of points within a leaf node voxel which is addressed by a + * point + * \param[in] point_arg: a point addressing a voxel \return amount of points + * that fall within leaf node voxel + */ + unsigned int + getVoxelDensityAtPoint(const PointT& point_arg) const + { + unsigned int point_count = 0; + + OctreePointCloudDensityContainer* leaf = this->findLeafAtPoint(point_arg); + + if (leaf) + point_count = leaf->getPointCounter(); + + return (point_count); } -} +}; +} // namespace octree +} // namespace pcl -#define PCL_INSTANTIATE_OctreePointCloudDensity(T) template class PCL_EXPORTS pcl::octree::OctreePointCloudDensity; +#define PCL_INSTANTIATE_OctreePointCloudDensity(T) \ + template class PCL_EXPORTS pcl::octree::OctreePointCloudDensity; diff --git a/octree/include/pcl/octree/octree_pointcloud_occupancy.h b/octree/include/pcl/octree/octree_pointcloud_occupancy.h index c7c84d7d..5c7d1101 100644 --- a/octree/include/pcl/octree/octree_pointcloud_occupancy.h +++ b/octree/include/pcl/octree/octree_pointcloud_occupancy.h @@ -40,87 +40,98 @@ #include -namespace pcl +namespace pcl { +namespace octree { + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** \brief @b Octree pointcloud occupancy class + * \tparam PointT type of point used in pointcloud + * This pointcloud octree class generate an octrees from a point cloud (zero-copy). No + * information is stored at the lead nodes. It can be used of occupancy checks. + * \note The octree pointcloud is initialized with its voxel resolution. Its bounding + * box is automatically adjusted or can be predefined. + * \ingroup octree + * \author Julius Kammerl (julius@kammerl.de) + */ +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +class OctreePointCloudOccupancy +: public OctreePointCloud> + { - namespace octree + +public: + // public typedefs for single/double buffering + using SingleBuffer = + OctreePointCloudOccupancy; + using DoubleBuffer = + OctreePointCloudOccupancy; + + // public point cloud typedefs + using PointCloud = + typename OctreePointCloud::PointCloud; + using PointCloudPtr = typename OctreePointCloud::PointCloudPtr; + using PointCloudConstPtr = + typename OctreePointCloud:: + PointCloudConstPtr; + + /** \brief Constructor. + * \param resolution_arg: octree resolution at lowest octree level + * */ + OctreePointCloudOccupancy(const double resolution_arg) + : OctreePointCloud>(resolution_arg) + {} + + /** \brief Empty class constructor. */ + + ~OctreePointCloudOccupancy() {} + + /** \brief Set occupied voxel at point. + * \param point_arg: input point + * */ + void + setOccupiedVoxelAtPoint(const PointT& point_arg) { + OctreeKey key; + + // make sure bounding box is big enough + this->adoptBoundingBoxToPoint(point_arg); - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - /** \brief @b Octree pointcloud occupancy class - * \note This pointcloud octree class generate an octrees from a point cloud (zero-copy). No information is stored at the lead nodes. It can be used of occupancy checks. - * \note The octree pointcloud is initialized with its voxel resolution. Its bounding box is automatically adjusted or can be predefined. - * \note - * \note typename: PointT: type of point used in pointcloud - * \ingroup octree - * \author Julius Kammerl (julius@kammerl.de) - */ - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - template - class OctreePointCloudOccupancy : public OctreePointCloud > - - { - - public: - // public typedefs for single/double buffering - using SingleBuffer = OctreePointCloudOccupancy; - using DoubleBuffer = OctreePointCloudOccupancy; - - // public point cloud typedefs - using PointCloud = typename OctreePointCloud::PointCloud; - using PointCloudPtr = typename OctreePointCloud::PointCloudPtr; - using PointCloudConstPtr = typename OctreePointCloud::PointCloudConstPtr; - - /** \brief Constructor. - * \param resolution_arg: octree resolution at lowest octree level - * */ - OctreePointCloudOccupancy (const double resolution_arg) : - OctreePointCloud > (resolution_arg) - { - } - - /** \brief Empty class constructor. */ - - ~OctreePointCloudOccupancy () - { - } - - /** \brief Set occupied voxel at point. - * \param point_arg: input point - * */ - void setOccupiedVoxelAtPoint( const PointT& point_arg ) { - OctreeKey key; - - // make sure bounding box is big enough - this->adoptBoundingBoxToPoint (point_arg); - - // generate key - this->genOctreeKeyforPoint (point_arg, key); - - // add point to octree at key - this->createLeaf (key); - } - - /** \brief Set occupied voxels at all points from point cloud. - * \param cloud_arg: input point cloud - * */ - void setOccupiedVoxelsAtPointsFromCloud( PointCloudPtr cloud_arg ) { - for (std::size_t i = 0; i < cloud_arg->points.size (); i++) - { - // check for NaNs - if (isFinite(cloud_arg->points[i])) { - // set voxel at point - this->setOccupiedVoxelAtPoint (cloud_arg->points[i]); - } - } - } - - }; + // generate key + this->genOctreeKeyforPoint(point_arg, key); + + // add point to octree at key + this->createLeaf(key); + } + + /** \brief Set occupied voxels at all points from point cloud. + * \param cloud_arg: input point cloud + * */ + void + setOccupiedVoxelsAtPointsFromCloud(PointCloudPtr cloud_arg) + { + for (std::size_t i = 0; i < cloud_arg->points.size(); i++) { + // check for NaNs + if (isFinite(cloud_arg->points[i])) { + // set voxel at point + this->setOccupiedVoxelAtPoint(cloud_arg->points[i]); + } + } } +}; +} // namespace octree -} +} // namespace pcl -#define PCL_INSTANTIATE_OctreePointCloudOccupancy(T) template class PCL_EXPORTS pcl::octree::OctreePointCloudOccupancy; +#define PCL_INSTANTIATE_OctreePointCloudOccupancy(T) \ + template class PCL_EXPORTS pcl::octree::OctreePointCloudOccupancy; diff --git a/octree/include/pcl/octree/octree_pointcloud_pointvector.h b/octree/include/pcl/octree/octree_pointcloud_pointvector.h index ea9c1c2c..47cf8076 100644 --- a/octree/include/pcl/octree/octree_pointcloud_pointvector.h +++ b/octree/include/pcl/octree/octree_pointcloud_pointvector.h @@ -40,50 +40,50 @@ #include -namespace pcl -{ - namespace octree - { +namespace pcl { +namespace octree { - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - /** \brief @b Octree pointcloud point vector class - * \note This pointcloud octree class generate an octrees from a point cloud (zero-copy). Every leaf node contains a list of point indices of the dataset given by \a setInputCloud. - * \note The octree pointcloud is initialized with its voxel resolution. Its bounding box is automatically adjusted or can be predefined. - * \note - * \note typename: PointT: type of point used in pointcloud - * \ingroup octree - * \author Julius Kammerl (julius@kammerl.de) - */ - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - template > - class OctreePointCloudPointVector : public OctreePointCloud - { - - public: - // public typedefs for single/double buffering - using SingleBuffer = OctreePointCloudPointVector >; - // typedef OctreePointCloudPointVector > DoubleBuffer; +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** \brief @b Octree pointcloud point vector class + * \note This pointcloud octree class generate an octrees from a point cloud + * (zero-copy). Every leaf node contains a list of point indices of the dataset given by + * \a setInputCloud. + * \note The octree pointcloud is initialized with its voxel resolution. Its bounding + * box is automatically adjusted or can be predefined. + * \tparam PointT type of point used in pointcloud + * \ingroup octree + * \author Julius Kammerl (julius@kammerl.de) + */ +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template > +class OctreePointCloudPointVector +: public OctreePointCloud { - /** \brief Constructor. - * \param resolution_arg: octree resolution at lowest octree level - * */ - OctreePointCloudPointVector (const double resolution_arg) : - OctreePointCloud (resolution_arg) - { - } +public: + // public typedefs for single/double buffering + using SingleBuffer = + OctreePointCloudPointVector>; + // typedef OctreePointCloudPointVector > DoubleBuffer; - /** \brief Empty class constructor. */ - ~OctreePointCloudPointVector () - { - } + /** \brief Constructor. + * \param resolution_arg: octree resolution at lowest octree level + * */ + OctreePointCloudPointVector(const double resolution_arg) + : OctreePointCloud(resolution_arg) + {} - }; - } -} + /** \brief Empty class constructor. */ + ~OctreePointCloudPointVector() {} +}; +} // namespace octree +} // namespace pcl -#define PCL_INSTANTIATE_OctreePointCloudPointVector(T) template class PCL_EXPORTS pcl::octree::OctreePointCloudPointVector; +#define PCL_INSTANTIATE_OctreePointCloudPointVector(T) \ + template class PCL_EXPORTS pcl::octree::OctreePointCloudPointVector; diff --git a/octree/include/pcl/octree/octree_pointcloud_singlepoint.h b/octree/include/pcl/octree/octree_pointcloud_singlepoint.h index 9e5ba8d2..a307bc89 100644 --- a/octree/include/pcl/octree/octree_pointcloud_singlepoint.h +++ b/octree/include/pcl/octree/octree_pointcloud_singlepoint.h @@ -40,51 +40,51 @@ #include -namespace pcl -{ - namespace octree - { - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - /** \brief @b Octree pointcloud single point class - * \note This pointcloud octree class generate an octrees from a point cloud (zero-copy). Every leaf node contains a single point index from the dataset given by \a setInputCloud. - * \note The octree pointcloud is initialized with its voxel resolution. Its bounding box is automatically adjusted or can be predefined. - * \note - * \note typename: PointT: type of point used in pointcloud - * \ingroup octree - * \author Julius Kammerl (julius@kammerl.de) - */ - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - template > +namespace pcl { +namespace octree { +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** \brief @b Octree pointcloud single point class + * \note This pointcloud octree class generate an octrees from a point cloud + * (zero-copy). Every leaf node contains a single point index from the dataset given by + * \a setInputCloud. + * \note The octree pointcloud is initialized with its voxel resolution. Its bounding + * box is automatically adjusted or can be predefined. + * \tparam PointT type of point used in pointcloud + * \ingroup octree + * \author Julius Kammerl (julius@kammerl.de) + */ +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template > - class OctreePointCloudSinglePoint : public OctreePointCloud - { +class OctreePointCloudSinglePoint +: public OctreePointCloud { - public: - // public typedefs for single/double buffering - using SingleBuffer = OctreePointCloudSinglePoint >; +public: + // public typedefs for single/double buffering + using SingleBuffer = + OctreePointCloudSinglePoint>; // typedef OctreePointCloudSinglePoint > DoubleBuffer; - - /** \brief Constructor. - * \param resolution_arg: octree resolution at lowest octree level - * */ - OctreePointCloudSinglePoint (const double resolution_arg) : - OctreePointCloud (resolution_arg) - { - } + // Octree2BufBase > DoubleBuffer; - /** \brief Empty class constructor. */ - ~OctreePointCloudSinglePoint () - { - } + /** \brief Constructor. + * \param resolution_arg: octree resolution at lowest octree level + * */ + OctreePointCloudSinglePoint(const double resolution_arg) + : OctreePointCloud(resolution_arg) + {} - }; + /** \brief Empty class constructor. */ + ~OctreePointCloudSinglePoint() {} +}; - } -} +} // namespace octree +} // namespace pcl -#define PCL_INSTANTIATE_OctreePointCloudSinglePoint(T) template class PCL_EXPORTS pcl::octree::OctreePointCloudSinglePoint; +#define PCL_INSTANTIATE_OctreePointCloudSinglePoint(T) \ + template class PCL_EXPORTS pcl::octree::OctreePointCloudSinglePoint; diff --git a/octree/include/pcl/octree/octree_pointcloud_voxelcentroid.h b/octree/include/pcl/octree/octree_pointcloud_voxelcentroid.h index b0001bb5..55100492 100644 --- a/octree/include/pcl/octree/octree_pointcloud_voxelcentroid.h +++ b/octree/include/pcl/octree/octree_pointcloud_voxelcentroid.h @@ -41,191 +41,189 @@ #include -namespace pcl -{ - namespace octree +namespace pcl { +namespace octree { +/** \brief @b Octree pointcloud voxel centroid leaf node class + * \note This class implements a leaf node that calculates the mean centroid of all + * points added this octree container. + * \author Julius Kammerl (julius@kammerl.de) + */ +template +class OctreePointCloudVoxelCentroidContainer : public OctreeContainerBase { +public: + /** \brief Class initialization. */ + OctreePointCloudVoxelCentroidContainer() { this->reset(); } + + /** \brief Empty class deconstructor. */ + ~OctreePointCloudVoxelCentroidContainer() {} + + /** \brief deep copy function */ + virtual OctreePointCloudVoxelCentroidContainer* + deepCopy() const + { + return (new OctreePointCloudVoxelCentroidContainer(*this)); + } + + /** \brief Equal comparison operator - set to false + */ + // param[in] OctreePointCloudVoxelCentroidContainer to compare with + bool + operator==(const OctreeContainerBase&) const override + { + return (false); + } + + /** \brief Add new point to voxel. + * \param[in] new_point the new point to add + */ + void + addPoint(const PointT& new_point) + { + using namespace pcl::common; + + ++point_counter_; + + point_sum_ += new_point; + } + + /** \brief Calculate centroid of voxel. + * \param[out] centroid_arg the resultant centroid of the voxel + */ + void + getCentroid(PointT& centroid_arg) const + { + using namespace pcl::common; + + if (point_counter_) { + centroid_arg = point_sum_; + centroid_arg /= static_cast(point_counter_); + } + else { + centroid_arg *= 0.0f; + } + } + + /** \brief Reset leaf container. */ + void + reset() override + { + using namespace pcl::common; + + point_counter_ = 0; + point_sum_ *= 0.0f; + } + +private: + unsigned int point_counter_; + PointT point_sum_; +}; + +/** \brief @b Octree pointcloud voxel centroid class + * \note This class generate an octrees from a point cloud (zero-copy). It provides a + * vector of centroids for all occupied voxels. + * \note The octree pointcloud is initialized with its voxel resolution. Its bounding + * box is automatically adjusted or can be predefined. + * \tparam PointT type of point used in pointcloud + * \ingroup octree + * \author Julius Kammerl (julius@kammerl.de) + */ +template , + typename BranchContainerT = OctreeContainerEmpty> +class OctreePointCloudVoxelCentroid +: public OctreePointCloud { +public: + using Ptr = shared_ptr>; + using ConstPtr = + shared_ptr>; + + using OctreeT = OctreePointCloud; + using LeafNode = typename OctreeT::LeafNode; + using BranchNode = typename OctreeT::BranchNode; + + /** \brief OctreePointCloudVoxelCentroids class constructor. + * \param[in] resolution_arg octree resolution at lowest octree level + */ + OctreePointCloudVoxelCentroid(const double resolution_arg) + : OctreePointCloud(resolution_arg) + {} + + /** \brief Empty class deconstructor. */ + + ~OctreePointCloudVoxelCentroid() {} + + /** \brief Add DataT object to leaf node at octree key. + * \param pointIdx_arg + */ + void + addPointIdx(const int pointIdx_arg) override + { + OctreeKey key; + + assert(pointIdx_arg < static_cast(this->input_->points.size())); + + const PointT& point = this->input_->points[pointIdx_arg]; + + // make sure bounding box is big enough + this->adoptBoundingBoxToPoint(point); + + // generate key + this->genOctreeKeyforPoint(point, key); + + // add point to octree at key + LeafContainerT* container = this->createLeaf(key); + container->addPoint(point); + } + + /** \brief Get centroid for a single voxel addressed by a PointT point. + * \param[in] point_arg point addressing a voxel in octree + * \param[out] voxel_centroid_arg centroid is written to this PointT reference + * \return "true" if voxel is found; "false" otherwise + */ + bool + getVoxelCentroidAtPoint(const PointT& point_arg, PointT& voxel_centroid_arg) const; + + /** \brief Get centroid for a single voxel addressed by a PointT point from input + * cloud. + * \param[in] point_idx_arg point index from input cloud addressing a voxel in octree + * \param[out] voxel_centroid_arg centroid is written to this PointT reference + * \return "true" if voxel is found; "false" otherwise + */ + inline bool + getVoxelCentroidAtPoint(const int& point_idx_arg, PointT& voxel_centroid_arg) const { - /** \brief @b Octree pointcloud voxel centroid leaf node class - * \note This class implements a leaf node that calculates the mean centroid of all points added this octree container. - * \author Julius Kammerl (julius@kammerl.de) - */ - template - class OctreePointCloudVoxelCentroidContainer : public OctreeContainerBase - { - public: - /** \brief Class initialization. */ - OctreePointCloudVoxelCentroidContainer () - { - this->reset(); - } - - /** \brief Empty class deconstructor. */ - ~OctreePointCloudVoxelCentroidContainer () - { - } - - /** \brief deep copy function */ - virtual OctreePointCloudVoxelCentroidContainer * - deepCopy () const - { - return (new OctreePointCloudVoxelCentroidContainer (*this)); - } - - /** \brief Equal comparison operator - set to false - */ - // param[in] OctreePointCloudVoxelCentroidContainer to compare with - bool operator==(const OctreeContainerBase&) const override - { - return ( false ); - } - - /** \brief Add new point to voxel. - * \param[in] new_point the new point to add - */ - void - addPoint (const PointT& new_point) - { - using namespace pcl::common; - - ++point_counter_; - - point_sum_ += new_point; - } - - /** \brief Calculate centroid of voxel. - * \param[out] centroid_arg the resultant centroid of the voxel - */ - void - getCentroid (PointT& centroid_arg) const - { - using namespace pcl::common; - - if (point_counter_) - { - centroid_arg = point_sum_; - centroid_arg /= static_cast (point_counter_); - } - else - { - centroid_arg *= 0.0f; - } - } - - /** \brief Reset leaf container. */ - void - reset () override - { - using namespace pcl::common; - - point_counter_ = 0; - point_sum_ *= 0.0f; - } - - private: - unsigned int point_counter_; - PointT point_sum_; - }; - - /** \brief @b Octree pointcloud voxel centroid class - * \note This class generate an octrees from a point cloud (zero-copy). It provides a vector of centroids for all occupied voxels. - * \note The octree pointcloud is initialized with its voxel resolution. Its bounding box is automatically adjusted or can be predefined. - * \note - * \note typename: PointT: type of point used in pointcloud - * - * \ingroup octree - * \author Julius Kammerl (julius@kammerl.de) - */ - template , - typename BranchContainerT = OctreeContainerEmpty > - class OctreePointCloudVoxelCentroid : public OctreePointCloud - { - public: - using Ptr = shared_ptr >; - using ConstPtr = shared_ptr >; - - using OctreeT = OctreePointCloud; - using LeafNode = typename OctreeT::LeafNode; - using BranchNode = typename OctreeT::BranchNode; - - /** \brief OctreePointCloudVoxelCentroids class constructor. - * \param[in] resolution_arg octree resolution at lowest octree level - */ - OctreePointCloudVoxelCentroid (const double resolution_arg) : - OctreePointCloud (resolution_arg) - { - } - - /** \brief Empty class deconstructor. */ - - ~OctreePointCloudVoxelCentroid () - { - } - - /** \brief Add DataT object to leaf node at octree key. - * \param pointIdx_arg - */ - void - addPointIdx (const int pointIdx_arg) override - { - OctreeKey key; - - assert (pointIdx_arg < static_cast (this->input_->points.size ())); - - const PointT& point = this->input_->points[pointIdx_arg]; - - // make sure bounding box is big enough - this->adoptBoundingBoxToPoint (point); - - // generate key - this->genOctreeKeyforPoint (point, key); - - // add point to octree at key - LeafContainerT* container = this->createLeaf(key); - container->addPoint (point); - - } - - /** \brief Get centroid for a single voxel addressed by a PointT point. - * \param[in] point_arg point addressing a voxel in octree - * \param[out] voxel_centroid_arg centroid is written to this PointT reference - * \return "true" if voxel is found; "false" otherwise - */ - bool - getVoxelCentroidAtPoint (const PointT& point_arg, PointT& voxel_centroid_arg) const; - - /** \brief Get centroid for a single voxel addressed by a PointT point from input cloud. - * \param[in] point_idx_arg point index from input cloud addressing a voxel in octree - * \param[out] voxel_centroid_arg centroid is written to this PointT reference - * \return "true" if voxel is found; "false" otherwise - */ - inline bool - getVoxelCentroidAtPoint (const int& point_idx_arg, PointT& voxel_centroid_arg) const - { - // get centroid at point - return (this->getVoxelCentroidAtPoint (this->input_->points[point_idx_arg], voxel_centroid_arg)); - } - - /** \brief Get PointT vector of centroids for all occupied voxels. - * \param[out] voxel_centroid_list_arg results are written to this vector of PointT elements - * \return number of occupied voxels - */ - std::size_t - 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] 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 - */ - void - getVoxelCentroidsRecursive (const BranchNode* branch_arg, - OctreeKey& key_arg, - typename OctreePointCloud::AlignedPointTVector &voxel_centroid_list_arg) const; - - }; + // get centroid at point + return (this->getVoxelCentroidAtPoint(this->input_->points[point_idx_arg], + voxel_centroid_arg)); } -} -// Note: Don't precompile this octree type to speed up compilation. It's probably rarely used. + /** \brief Get PointT vector of centroids for all occupied voxels. + * \param[out] voxel_centroid_list_arg results are written to this vector of PointT + * elements + * \return number of occupied voxels + */ + std::size_t + 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] 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 + */ + void + getVoxelCentroidsRecursive( + const BranchNode* branch_arg, + OctreeKey& key_arg, + typename OctreePointCloud:: + AlignedPointTVector& voxel_centroid_list_arg) const; +}; +} // namespace octree +} // namespace pcl + +// Note: Don't precompile this octree type to speed up compilation. It's probably rarely +// used. #include diff --git a/octree/include/pcl/octree/octree_search.h b/octree/include/pcl/octree/octree_search.h index a658cfcc..47b78182 100644 --- a/octree/include/pcl/octree/octree_search.h +++ b/octree/include/pcl/octree/octree_search.h @@ -42,549 +42,617 @@ #include -namespace pcl -{ - namespace octree +namespace pcl { +namespace octree { +/** \brief @b Octree pointcloud search class + * \note This class provides several methods for spatial neighbor search based on octree + * structure + * \tparam PointT type of point used in pointcloud + * \ingroup octree + * \author Julius Kammerl (julius@kammerl.de) + */ +template +class OctreePointCloudSearch +: public OctreePointCloud { +public: + // public typedefs + using IndicesPtr = shared_ptr>; + using IndicesConstPtr = shared_ptr>; + + using PointCloud = pcl::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + + // Boost shared pointers + using Ptr = + shared_ptr>; + using ConstPtr = shared_ptr< + const OctreePointCloudSearch>; + + // Eigen aligned allocator + using AlignedPointTVector = std::vector>; + + using OctreeT = OctreePointCloud; + using LeafNode = typename OctreeT::LeafNode; + using BranchNode = typename OctreeT::BranchNode; + + /** \brief Constructor. + * \param[in] resolution octree resolution at lowest octree level + */ + OctreePointCloudSearch(const double resolution) + : OctreePointCloud(resolution) + {} + + /** \brief Search for neighbors within a voxel at given point + * \param[in] point point addressing a leaf node voxel + * \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& 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] 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& point_idx_data); + + /** \brief Search for k-nearest neighbors at the query point. + * \param[in] cloud the point cloud data + * \param[in] index the index in \a cloud representing the query point + * \param[in] k the number of neighbors to search for + * \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 + */ + inline int + nearestKSearch(const PointCloud& cloud, + int index, + int k, + std::vector& k_indices, + std::vector& k_sqr_distances) { - /** \brief @b Octree pointcloud search class - * \note This class provides several methods for spatial neighbor search based on octree structure - * \note typename: PointT: type of point used in pointcloud - * \ingroup octree - * \author Julius Kammerl (julius@kammerl.de) - */ - template - class OctreePointCloudSearch : public OctreePointCloud + return (nearestKSearch(cloud[index], k, k_indices, k_sqr_distances)); + } + + /** \brief Search for k-nearest neighbors at given query point. + * \param[in] p_q the given query point + * \param[in] k the number of neighbors to search for + * \param[out] k_indices the resultant indices of the neighboring points (must be + * resized to k a priori!) + * \param[out] k_sqr_distances the resultant squared distances to the neighboring + * points (must be resized to k a priori!) + * \return number of neighbors found + */ + int + nearestKSearch(const PointT& p_q, + int k, + std::vector& k_indices, + std::vector& k_sqr_distances); + + /** \brief Search for k-nearest neighbors at query point + * \param[in] index index representing the query point in the dataset given by \a + * setInputCloud. If indices were given in setInputCloud, index will be the position + * in the indices vector. + * \param[in] k the number of neighbors to search for + * \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 + */ + int + 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 + * \param[in] query_index the index in \a cloud representing the query point + * \param[out] result_index the resultant index of the neighbor point + * \param[out] sqr_distance the resultant squared distance to the neighboring point + * \return number of neighbors found + */ + inline void + approxNearestSearch(const PointCloud& cloud, + int query_index, + int& result_index, + float& sqr_distance) + { + return (approxNearestSearch(cloud.points[query_index], result_index, sqr_distance)); + } + + /** \brief Search for approx. nearest neighbor at the query point. + * \param[in] p_q the given query point + * \param[out] result_index the resultant index of the neighbor point + * \param[out] sqr_distance the resultant squared distance to the neighboring point + */ + void + approxNearestSearch(const PointT& p_q, int& result_index, float& sqr_distance); + + /** \brief Search for approx. nearest neighbor at the query point. + * \param[in] query_index index representing the query point in the dataset given by + * \a setInputCloud. If indices were given in setInputCloud, index will be the + * position in the indices vector. + * \param[out] result_index the resultant index of the neighbor point + * \param[out] sqr_distance the resultant squared distance to the neighboring point + * \return number of neighbors found + */ + void + approxNearestSearch(int query_index, int& result_index, float& sqr_distance); + + /** \brief Search for all neighbors of query point that are within a given radius. + * \param[in] cloud the point cloud data + * \param[in] index the index in \a cloud representing the query point + * \param[in] radius the radius of the sphere bounding all of p_q's neighbors + * \param[out] k_indices the resultant indices of the neighboring points + * \param[out] k_sqr_distances the resultant squared distances to the neighboring + * points + * \param[in] max_nn if given, bounds the maximum returned neighbors to this value + * \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) + { + return ( + radiusSearch(cloud.points[index], radius, k_indices, k_sqr_distances, max_nn)); + } + + /** \brief Search for all neighbors of query point that are within a given radius. + * \param[in] p_q the given query point + * \param[in] radius the radius of the sphere bounding all of p_q's neighbors + * \param[out] k_indices the resultant indices of the neighboring points + * \param[out] k_sqr_distances the resultant squared distances to the neighboring + * points + * \param[in] max_nn if given, bounds the maximum returned neighbors to this value + * \return number of neighbors found in radius + */ + int + radiusSearch(const PointT& p_q, + const double radius, + std::vector& k_indices, + std::vector& k_sqr_distances, + unsigned int max_nn = 0) const; + + /** \brief Search for all neighbors of query point that are within a given radius. + * \param[in] index index representing the query point in the dataset given by \a + * setInputCloud. If indices were given in setInputCloud, index will be the position + * in the indices vector + * \param[in] radius radius of the sphere bounding all of p_q's neighbors + * \param[out] k_indices the resultant indices of the neighboring points + * \param[out] k_sqr_distances the resultant squared distances to the neighboring + * points + * \param[in] max_nn if given, bounds the maximum returned neighbors to this value + * \return number of neighbors found in radius + */ + int + radiusSearch(int index, + const double radius, + std::vector& k_indices, + std::vector& k_sqr_distances, + unsigned int max_nn = 0) const; + + /** \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] 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& 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] 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 max_voxel_count = 0) const; + + /** \brief Search for points within rectangular search area + * Points exactly on the edges of the search rectangle are included. + * \param[in] min_pt lower corner of search area + * \param[in] max_pt upper corner of search area + * \param[out] k_indices the resultant point indices + * \return number of points found within search area + */ + int + boxSearch(const Eigen::Vector3f& min_pt, + const Eigen::Vector3f& max_pt, + std::vector& k_indices) const; + +protected: + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + // Octree-based search routines & helpers + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /** \brief @b Priority queue entry for branch nodes + * \note This class defines priority queue entries for the nearest neighbor search. + * \author Julius Kammerl (julius@kammerl.de) + */ + class prioBranchQueueEntry { + public: + /** \brief Empty constructor */ + prioBranchQueueEntry() : node(), point_distance(0) {} + + /** \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 { - public: - // public typedefs - using IndicesPtr = shared_ptr >; - using IndicesConstPtr = shared_ptr >; - - using PointCloud = pcl::PointCloud; - using PointCloudPtr = typename PointCloud::Ptr; - using PointCloudConstPtr = typename PointCloud::ConstPtr; - - // Boost shared pointers - using Ptr = shared_ptr >; - using ConstPtr = shared_ptr >; - - // Eigen aligned allocator - using AlignedPointTVector = std::vector >; - - using OctreeT = OctreePointCloud; - using LeafNode = typename OctreeT::LeafNode; - using BranchNode = typename OctreeT::BranchNode; - - /** \brief Constructor. - * \param[in] resolution octree resolution at lowest octree level - */ - OctreePointCloudSearch (const double resolution) : - OctreePointCloud (resolution) - { - } - - /** \brief Search for neighbors within a voxel at given point - * \param[in] point point addressing a leaf node voxel - * \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& 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] 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& point_idx_data); - - /** \brief Search for k-nearest neighbors at the query point. - * \param[in] cloud the point cloud data - * \param[in] index the index in \a cloud representing the query point - * \param[in] k the number of neighbors to search for - * \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 - */ - inline int - nearestKSearch (const PointCloud &cloud, int index, int k, std::vector &k_indices, - std::vector &k_sqr_distances) - { - return (nearestKSearch (cloud[index], k, k_indices, k_sqr_distances)); - } - - /** \brief Search for k-nearest neighbors at given query point. - * \param[in] p_q the given query point - * \param[in] k the number of neighbors to search for - * \param[out] k_indices the resultant indices of the neighboring points (must be resized to k a priori!) - * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to k a priori!) - * \return number of neighbors found - */ - int - nearestKSearch (const PointT &p_q, int k, std::vector &k_indices, - std::vector &k_sqr_distances); - - /** \brief Search for k-nearest neighbors at query point - * \param[in] index index representing the query point in the dataset given by \a setInputCloud. - * If indices were given in setInputCloud, index will be the position in the indices vector. - * \param[in] k the number of neighbors to search for - * \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 - */ - int - 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 - * \param[in] query_index the index in \a cloud representing the query point - * \param[out] result_index the resultant index of the neighbor point - * \param[out] sqr_distance the resultant squared distance to the neighboring point - * \return number of neighbors found - */ - inline void - approxNearestSearch (const PointCloud &cloud, int query_index, int &result_index, float &sqr_distance) - { - return (approxNearestSearch (cloud.points[query_index], result_index, sqr_distance)); - } - - /** \brief Search for approx. nearest neighbor at the query point. - * \param[in] p_q the given query point - * \param[out] result_index the resultant index of the neighbor point - * \param[out] sqr_distance the resultant squared distance to the neighboring point - */ - void - approxNearestSearch (const PointT &p_q, int &result_index, float &sqr_distance); - - /** \brief Search for approx. nearest neighbor at the query point. - * \param[in] query_index index representing the query point in the dataset given by \a setInputCloud. - * If indices were given in setInputCloud, index will be the position in the indices vector. - * \param[out] result_index the resultant index of the neighbor point - * \param[out] sqr_distance the resultant squared distance to the neighboring point - * \return number of neighbors found - */ - void - approxNearestSearch (int query_index, int &result_index, float &sqr_distance); - - /** \brief Search for all neighbors of query point that are within a given radius. - * \param[in] cloud the point cloud data - * \param[in] index the index in \a cloud representing the query point - * \param[in] radius the radius of the sphere bounding all of p_q's neighbors - * \param[out] k_indices the resultant indices of the neighboring points - * \param[out] k_sqr_distances the resultant squared distances to the neighboring points - * \param[in] max_nn if given, bounds the maximum returned neighbors to this value - * \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) - { - return (radiusSearch (cloud.points[index], radius, k_indices, k_sqr_distances, max_nn)); - } - - /** \brief Search for all neighbors of query point that are within a given radius. - * \param[in] p_q the given query point - * \param[in] radius the radius of the sphere bounding all of p_q's neighbors - * \param[out] k_indices the resultant indices of the neighboring points - * \param[out] k_sqr_distances the resultant squared distances to the neighboring points - * \param[in] max_nn if given, bounds the maximum returned neighbors to this value - * \return number of neighbors found in radius - */ - int - radiusSearch (const PointT &p_q, const double radius, std::vector &k_indices, - std::vector &k_sqr_distances, unsigned int max_nn = 0) const; - - /** \brief Search for all neighbors of query point that are within a given radius. - * \param[in] index index representing the query point in the dataset given by \a setInputCloud. - * If indices were given in setInputCloud, index will be the position in the indices vector - * \param[in] radius radius of the sphere bounding all of p_q's neighbors - * \param[out] k_indices the resultant indices of the neighboring points - * \param[out] k_sqr_distances the resultant squared distances to the neighboring points - * \param[in] max_nn if given, bounds the maximum returned neighbors to this value - * \return number of neighbors found in radius - */ - int - radiusSearch (int index, const double radius, std::vector &k_indices, - std::vector &k_sqr_distances, unsigned int max_nn = 0) const; - - /** \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] 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 &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] 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 max_voxel_count = 0) const; - - - /** \brief Search for points within rectangular search area - * Points exactly on the edges of the search rectangle are included. - * \param[in] min_pt lower corner of search area - * \param[in] max_pt upper corner of search area - * \param[out] k_indices the resultant point indices - * \return number of points found within search area - */ - int - boxSearch (const Eigen::Vector3f &min_pt, const Eigen::Vector3f &max_pt, std::vector &k_indices) const; - - protected: - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - // Octree-based search routines & helpers - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - /** \brief @b Priority queue entry for branch nodes - * \note This class defines priority queue entries for the nearest neighbor search. - * \author Julius Kammerl (julius@kammerl.de) - */ - class prioBranchQueueEntry - { - public: - /** \brief Empty constructor */ - prioBranchQueueEntry () : - node (), point_distance (0) - { - } - - /** \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->point_distance > rhs.point_distance); - } - - /** \brief Pointer to octree node. */ - const OctreeNode* node; - - /** \brief Distance to query point. */ - float point_distance; - - /** \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: - - /** \brief Empty constructor */ - prioPointQueueEntry () : - point_idx_ (0), point_distance_ (0) - { - } - - /** \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->point_distance_ < rhs.point_distance_); - } - - /** \brief Index representing a point in the dataset given by \a setInputCloud. */ - int point_idx_; - - /** \brief Distance to query point. */ - float point_distance_; - }; - - /** \brief Helper function to calculate the squared distance between two points - * \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& point_a, const PointT& point_b) const; - - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - // Recursive search routine methods - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - - /** \brief Recursive search method that explores the octree and finds neighbors within a given radius - * \param[in] point query point - * \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] 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 - */ - void - getNeighborsWithinRadiusRecursive (const PointT& point, const double radiusSquared, - const BranchNode* node, const OctreeKey& key, - 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 - * \param[in] point query point - * \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] 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 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] 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 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] 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] 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 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 - * \param[in] min_pt lower corner of search area - * \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] 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 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] 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] max_voxel_count stop raycasting when this many voxels intersected (0: disable) - * \return number of voxels found - */ - int - 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 max_voxel_count) const; - - /** \brief Initialize raytracing algorithm - * \param origin - * \param direction - * \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 &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 - const float epsilon = 1e-10f; - if (direction.x () == 0.0) - direction.x () = epsilon; - if (direction.y () == 0.0) - direction.y () = epsilon; - if (direction.z () == 0.0) - direction.z () = epsilon; - - // Voxel childIdx remapping - a = 0; - - // Handle negative axis direction vector - if (direction.x () < 0.0) - { - 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->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->min_z_) + static_cast (this->max_z_) - origin.z (); - direction.z () = -direction.z (); - a |= 1; - } - 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] 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 min_x, double min_y, double min_z, double mid_x, double mid_y, double mid_z) const - { - int currNode = 0; - - if (min_x > min_y) - { - if (min_x > min_z) - { - // max(min_x, min_y, min_z) is min_x. Entry plane is YZ. - if (mid_y < min_x) - currNode |= 2; - if (mid_z < min_x) - currNode |= 1; - } - else - { - // max(min_x, min_y, min_z) is min_z. Entry plane is XY. - if (mid_x < min_z) - currNode |= 4; - if (mid_y < min_z) - currNode |= 2; - } - } - else - { - if (min_y > min_z) - { - // max(min_x, min_y, min_z) is min_y. Entry plane is XZ. - if (mid_x < min_y) - currNode |= 4; - if (mid_z < min_y) - currNode |= 1; - } - else - { - // max(min_x, min_y, min_z) is min_z. Entry plane is XY. - if (mid_x < min_z) - currNode |= 4; - if (mid_y < min_z) - currNode |= 2; - } - } - - return currNode; - } - - /** \brief Get the next visited node given the current node upper - * bounding box corner. This function accepts three float values, and - * three int values. The function returns the ith integer where the - * ith float value is the minimum of the three float values. - * \param[in] x current nodes X coordinate of upper bounding box corner - * \param[in] y current nodes Y coordinate of upper bounding box corner - * \param[in] z current nodes Z coordinate of upper bounding box corner - * \param[in] a next node if exit Plane YZ - * \param[in] b next node if exit Plane XZ - * \param[in] c next node if exit Plane XY - * \return the next child node ray will enter or 8 if exiting - */ - inline int - getNextIntersectedNode (double x, double y, double z, int a, int b, int c) const - { - if (x < y) - { - if (x < z) - return a; - return c; - } - if (y < z) - return b; - return c; - } - - }; + return (this->point_distance > rhs.point_distance); + } + + /** \brief Pointer to octree node. */ + const OctreeNode* node; + + /** \brief Distance to query point. */ + float point_distance; + + /** \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: + /** \brief Empty constructor */ + prioPointQueueEntry() : point_idx_(0), point_distance_(0) {} + + /** \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->point_distance_ < rhs.point_distance_); + } + + /** \brief Index representing a point in the dataset given by \a setInputCloud. */ + int point_idx_; + + /** \brief Distance to query point. */ + float point_distance_; + }; + + /** \brief Helper function to calculate the squared distance between two points + * \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& point_a, const PointT& point_b) const; + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + // Recursive search routine methods + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + /** \brief Recursive search method that explores the octree and finds neighbors within + * a given radius + * \param[in] point query point \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] 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 + */ + void + getNeighborsWithinRadiusRecursive(const PointT& point, + const double radiusSquared, + const BranchNode* node, + const OctreeKey& key, + 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 + * \param[in] point query point + * \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] 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 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] 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 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] 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] 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 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 + * \param[in] min_pt lower corner of search area + * \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] 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 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] 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] max_voxel_count stop raycasting when this many voxels intersected (0: + * disable) + * \return number of voxels found + */ + int + 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 max_voxel_count) const; + + /** \brief Initialize raytracing algorithm + * \param origin + * \param direction + * \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& 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 + const float epsilon = 1e-10f; + if (direction.x() == 0.0) + direction.x() = epsilon; + if (direction.y() == 0.0) + direction.y() = epsilon; + if (direction.z() == 0.0) + direction.z() = epsilon; + + // Voxel childIdx remapping + a = 0; + + // Handle negative axis direction vector + if (direction.x() < 0.0) { + 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->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->min_z_) + static_cast(this->max_z_) - + origin.z(); + direction.z() = -direction.z(); + a |= 1; + } + 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] 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 min_x, + double min_y, + double min_z, + double mid_x, + double mid_y, + double mid_z) const + { + int currNode = 0; + + if (min_x > min_y) { + if (min_x > min_z) { + // max(min_x, min_y, min_z) is min_x. Entry plane is YZ. + if (mid_y < min_x) + currNode |= 2; + if (mid_z < min_x) + currNode |= 1; + } + else { + // max(min_x, min_y, min_z) is min_z. Entry plane is XY. + if (mid_x < min_z) + currNode |= 4; + if (mid_y < min_z) + currNode |= 2; + } + } + else { + if (min_y > min_z) { + // max(min_x, min_y, min_z) is min_y. Entry plane is XZ. + if (mid_x < min_y) + currNode |= 4; + if (mid_z < min_y) + currNode |= 1; + } + else { + // max(min_x, min_y, min_z) is min_z. Entry plane is XY. + if (mid_x < min_z) + currNode |= 4; + if (mid_y < min_z) + currNode |= 2; + } + } + + return currNode; + } + + /** \brief Get the next visited node given the current node upper + * bounding box corner. This function accepts three float values, and + * three int values. The function returns the ith integer where the + * ith float value is the minimum of the three float values. + * \param[in] x current nodes X coordinate of upper bounding box corner + * \param[in] y current nodes Y coordinate of upper bounding box corner + * \param[in] z current nodes Z coordinate of upper bounding box corner + * \param[in] a next node if exit Plane YZ + * \param[in] b next node if exit Plane XZ + * \param[in] c next node if exit Plane XY + * \return the next child node ray will enter or 8 if exiting + */ + inline int + getNextIntersectedNode(double x, double y, double z, int a, int b, int c) const + { + if (x < y) { + if (x < z) + return a; + return c; + } + if (y < z) + return b; + return c; } -} +}; +} // namespace octree +} // namespace pcl #ifdef PCL_NO_PRECOMPILE #include diff --git a/octree/src/octree_inst.cpp b/octree/src/octree_inst.cpp index 90a6a84c..ae5e5898 100644 --- a/octree/src/octree_inst.cpp +++ b/octree/src/octree_inst.cpp @@ -42,18 +42,16 @@ template class PCL_EXPORTS pcl::octree::OctreeBase; template class PCL_EXPORTS pcl::octree::Octree2BufBase; +template class PCL_EXPORTS + pcl::octree::OctreeBase; -template class PCL_EXPORTS pcl::octree::OctreeBase< - pcl::octree::OctreeContainerPointIndices, - pcl::octree::OctreeContainerEmpty >; +template class PCL_EXPORTS + pcl::octree::Octree2BufBase; -template class PCL_EXPORTS pcl::octree::Octree2BufBase< - pcl::octree::OctreeContainerPointIndices, - pcl::octree::OctreeContainerEmpty >; - -template class PCL_EXPORTS pcl::octree::OctreeBase< - pcl::octree::OctreeContainerEmpty, - pcl::octree::OctreeContainerEmpty >; +template class PCL_EXPORTS pcl::octree::OctreeBase; #ifndef PCL_NO_PRECOMPILE #include @@ -65,7 +63,6 @@ PCL_INSTANTIATE(OctreePointCloudDoubleBufferWithLeafDataTVector, PCL_XYZ_POINT_T PCL_INSTANTIATE(OctreePointCloudSearch, PCL_XYZ_POINT_TYPES) - // PCL_INSTANTIATE(OctreePointCloudSingleBufferWithLeafDataT, PCL_XYZ_POINT_TYPES) PCL_INSTANTIATE(OctreePointCloudSingleBufferWithEmptyLeaf, PCL_XYZ_POINT_TYPES) @@ -84,5 +81,4 @@ PCL_INSTANTIATE(OctreePointCloudSingleBufferWithEmptyLeaf, PCL_XYZ_POINT_TYPES) // PCL_INSTANTIATE(OctreePointCloudVoxelCentroid, PCL_XYZ_POINT_TYPES) // PCL_INSTANTIATE(OctreePointCloudAdjacency, PCL_XYZ_POINT_TYPES) -#endif // PCL_NO_PRECOMPILE - +#endif // PCL_NO_PRECOMPILE diff --git a/outofcore/include/pcl/outofcore/impl/lru_cache.hpp b/outofcore/include/pcl/outofcore/impl/lru_cache.hpp index a60f6a74..a5c29e91 100644 --- a/outofcore/include/pcl/outofcore/impl/lru_cache.hpp +++ b/outofcore/include/pcl/outofcore/impl/lru_cache.hpp @@ -2,8 +2,11 @@ #ifndef __PCL_OUTOFCORE_LRU_CACHE__ #define __PCL_OUTOFCORE_LRU_CACHE__ +#include #include #include +#include +#include template class LRUCacheItem @@ -103,8 +106,8 @@ public: } size -= tail_size; - key_it++; - evict_count++; + ++key_it; + ++evict_count; } // Evict enough items to make room for the new item diff --git a/outofcore/include/pcl/outofcore/impl/octree_disk_container.hpp b/outofcore/include/pcl/outofcore/impl/octree_disk_container.hpp index a12cafd1..21196c0a 100644 --- a/outofcore/include/pcl/outofcore/impl/octree_disk_container.hpp +++ b/outofcore/include/pcl/outofcore/impl/octree_disk_container.hpp @@ -555,11 +555,11 @@ namespace pcl Eigen::Vector4f origin; Eigen::Quaternionf orientation; - int pcd_version; - + if (boost::filesystem::exists (disk_storage_filename_)) { // PCL_INFO ("[pcl::outofcore::OutofcoreOctreeDiskContainer::%s] Reading points from disk from %s.\n", __FUNCTION__ , disk_storage_filename_->c_str ()); + int pcd_version; int res = reader.read (disk_storage_filename_, *dst, origin, orientation, pcd_version); (void)res; assert (res != -1); diff --git a/outofcore/src/visualization/outofcore_cloud.cpp b/outofcore/src/visualization/outofcore_cloud.cpp index 4f78fc28..46f1b0d2 100644 --- a/outofcore/src/visualization/outofcore_cloud.cpp +++ b/outofcore/src/visualization/outofcore_cloud.cpp @@ -52,8 +52,6 @@ OutofcoreCloud::PcdQueue OutofcoreCloud::pcd_queue; void OutofcoreCloud::pcdReaderThread () { - - std::string pcd_file; std::size_t timestamp = 0; while (true) diff --git a/outofcore/tools/CMakeLists.txt b/outofcore/tools/CMakeLists.txt index cf60f66c..53c63e39 100644 --- a/outofcore/tools/CMakeLists.txt +++ b/outofcore/tools/CMakeLists.txt @@ -1,3 +1,5 @@ +set(SUBSYS_NAME tools) + # pcl_outofcore_process PCL_ADD_EXECUTABLE(pcl_outofcore_process COMPONENT ${SUBSYS_NAME} SOURCES outofcore_process.cpp) target_link_libraries(pcl_outofcore_process pcl_common pcl_filters pcl_io pcl_octree pcl_outofcore) diff --git a/people/include/pcl/people/impl/head_based_subcluster.hpp b/people/include/pcl/people/impl/head_based_subcluster.hpp index 1b91816d..6b550e15 100644 --- a/people/include/pcl/people/impl/head_based_subcluster.hpp +++ b/people/include/pcl/people/impl/head_based_subcluster.hpp @@ -178,7 +178,7 @@ pcl::people::HeadBasedSubclustering::mergeClustersCloseInFloorCoordinate { used_clusters[cluster] = true; for(std::vector::const_iterator points_iterator = input_clusters[cluster].getIndices().indices.begin(); - points_iterator != input_clusters[cluster].getIndices().indices.end(); points_iterator++) + points_iterator != input_clusters[cluster].getIndices().indices.end(); ++points_iterator) { point_indices.indices.push_back(*points_iterator); } @@ -214,7 +214,7 @@ pcl::people::HeadBasedSubclustering::createSubClusters (pcl::people::Per } // Associate cluster points to one of the maximum: - for(std::vector::const_iterator points_iterator = cluster.getIndices().indices.begin(); points_iterator != cluster.getIndices().indices.end(); points_iterator++) + for(std::vector::const_iterator points_iterator = cluster.getIndices().indices.begin(); points_iterator != cluster.getIndices().indices.end(); ++points_iterator) { PointT* current_point = &cloud_->points[*points_iterator]; // current point cloud point Eigen::Vector3f p_current_eigen(current_point->x, current_point->y, current_point->z); // conversion to eigen diff --git a/people/include/pcl/people/impl/height_map_2d.hpp b/people/include/pcl/people/impl/height_map_2d.hpp index 6976fce0..89c66183 100644 --- a/people/include/pcl/people/impl/height_map_2d.hpp +++ b/people/include/pcl/people/impl/height_map_2d.hpp @@ -85,7 +85,7 @@ pcl::people::HeightMap2D::compute (pcl::people::PersonCluster& c buckets_.resize(std::size_t((cluster.getMax()(1) - cluster.getMin()(1)) / bin_size_) + 1, 0); buckets_cloud_indices_.resize(buckets_.size(), 0); - for(std::vector::const_iterator pit = cluster.getIndices().indices.begin(); pit != cluster.getIndices().indices.end(); pit++) + for(std::vector::const_iterator pit = cluster.getIndices().indices.begin(); pit != cluster.getIndices().indices.end(); ++pit) { PointT* p = &cloud_->points[*pit]; int index; @@ -121,7 +121,6 @@ pcl::people::HeightMap2D::searchLocalMaxima () // Search for local maxima: maxima_number_ = 0; int left = buckets_[0]; // current left element - int right = 0; // current right element float offset = 0; // used to center the maximum to the right place maxima_indices_.resize(std::size_t(buckets_.size()), 0); maxima_cloud_indices_.resize(std::size_t(buckets_.size()), 0); @@ -138,7 +137,7 @@ pcl::people::HeightMap2D::searchLocalMaxima () int i = 1; while (i < (static_cast (buckets_.size()) - 1)) { - right = buckets_[i+1]; + int right = buckets_[i+1]; // current right element if ((buckets_[i] > left) && (buckets_[i] > right)) { // Search where to insert the new element (in an ordered array): @@ -211,7 +210,6 @@ pcl::people::HeightMap2D::filterMaxima () for (int i = 0; i < maxima_number_; i++) { bool good_maximum = true; - float distance = 0; PointT* p_current = &cloud_->points[maxima_cloud_indices_[i]]; // pointcloud point referring to the current maximum Eigen::Vector3f p_current_eigen(p_current->x, p_current->y, p_current->z); // conversion to eigen @@ -227,7 +225,7 @@ pcl::people::HeightMap2D::filterMaxima () p_previous_eigen -= ground_coeffs_.head(3) * t; // projection of the point on the groundplane // distance of the projection of the points on the groundplane: - distance = (p_current_eigen-p_previous_eigen).norm(); + float distance = (p_current_eigen-p_previous_eigen).norm(); if (distance < min_dist_between_maxima_) { good_maximum = false; diff --git a/people/include/pcl/people/impl/person_classifier.hpp b/people/include/pcl/people/impl/person_classifier.hpp index 68d33673..efce6bfc 100644 --- a/people/include/pcl/people/impl/person_classifier.hpp +++ b/people/include/pcl/people/impl/person_classifier.hpp @@ -71,10 +71,9 @@ pcl::people::PersonClassifier::loadSVMFromFile (std::string svm_filename getline (SVM_file,line); // read SVM_weights line tok_pos = line.find_first_of('[', 0); // search for token "[" std::size_t tok_end_pos = line.find_first_of(']', 0); // search for token "]" , end of SVM weights - std::size_t prev_tok_pos; while (tok_pos < tok_end_pos) // while end of SVM_weights is not reached { - prev_tok_pos = tok_pos; + std::size_t prev_tok_pos = tok_pos; tok_pos = line.find_first_of(',', prev_tok_pos+1); // search for token "," SVM_weights_.push_back(std::atof(line.substr(prev_tok_pos+1, tok_pos-prev_tok_pos-1).c_str())); } diff --git a/people/include/pcl/people/impl/person_cluster.hpp b/people/include/pcl/people/impl/person_cluster.hpp index 44683ae1..f0746751 100644 --- a/people/include/pcl/people/impl/person_cluster.hpp +++ b/people/include/pcl/people/impl/person_cluster.hpp @@ -177,7 +177,7 @@ pcl::people::PersonCluster::init ( float min_z = c_z_; float max_x = c_x_; float max_z = c_z_; - for (std::vector::const_iterator pit = points_indices_.indices.begin(); pit != points_indices_.indices.end(); pit++) + for (std::vector::const_iterator pit = points_indices_.indices.begin(); pit != points_indices_.indices.end(); ++pit) { PointT* p = &input_cloud->points[*pit]; @@ -216,7 +216,7 @@ pcl::people::PersonCluster::init ( float min_z = c_z_; float max_y = c_y_; float max_z = c_z_; - for (std::vector::const_iterator pit = points_indices_.indices.begin(); pit != points_indices_.indices.end(); pit++) + for (std::vector::const_iterator pit = points_indices_.indices.begin(); pit != points_indices_.indices.end(); ++pit) { PointT* p = &input_cloud->points[*pit]; diff --git a/recognition/include/pcl/recognition/color_modality.h b/recognition/include/pcl/recognition/color_modality.h index bf06ab10..9b7c7040 100644 --- a/recognition/include/pcl/recognition/color_modality.h +++ b/recognition/include/pcl/recognition/color_modality.h @@ -4,7 +4,7 @@ * Point Cloud Library (PCL) - www.pointclouds.org * Copyright (c) 2010-2011, Willow Garage, Inc. * - * All rights reserved. + * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -45,6 +45,10 @@ #include #include +#include +#include +#include +#include namespace pcl { diff --git a/recognition/include/pcl/recognition/impl/hv/hv_go.hpp b/recognition/include/pcl/recognition/impl/hv/hv_go.hpp index e537d6af..143fe57e 100644 --- a/recognition/include/pcl/recognition/impl/hv/hv_go.hpp +++ b/recognition/include/pcl/recognition/impl/hv/hv_go.hpp @@ -564,7 +564,6 @@ bool pcl::GlobalHypothesesVerification::addModel(typename pcl::P std::vector explained_indices; std::vector outliers_weight; std::vector explained_indices_distances; - std::vector unexplained_indices_weights; std::vector nn_indices; std::vector nn_distances; 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 55692dc9..89f83867 100644 --- a/recognition/include/pcl/recognition/impl/ransac_based/simple_octree.hpp +++ b/recognition/include/pcl/recognition/impl/ransac_based/simple_octree.hpp @@ -8,6 +8,9 @@ #ifndef SIMPLE_OCTREE_HPP_ #define SIMPLE_OCTREE_HPP_ +#include +#include + //=============================================================================================================================== template inline @@ -277,15 +280,13 @@ pcl::recognition::SimpleOctree::createLeaf (S } Node* node = root_; - const Scalar *c; - int id; // Go down to the right leaf for ( int l = 0 ; l < tree_levels_ ; ++l ) { node->createChildren (); - c = node->getCenter (); - id = 0; + const Scalar *c = node->getCenter (); + int id = 0; if ( x >= c[0] ) id |= 4; if ( y >= c[1] ) id |= 2; @@ -333,8 +334,6 @@ pcl::recognition::SimpleOctree::getFullLeaf ( } Node* node = root_; - const Scalar *c; - int id; // Go down to the right leaf for ( int l = 0 ; l < tree_levels_ ; ++l ) @@ -342,8 +341,8 @@ pcl::recognition::SimpleOctree::getFullLeaf ( if ( !node->hasChildren () ) return (nullptr); - c = node->getCenter (); - id = 0; + const Scalar *c = node->getCenter (); + int id = 0; if ( x >= c[0] ) id |= 4; if ( y >= c[1] ) id |= 2; diff --git a/recognition/include/pcl/recognition/mask_map.h b/recognition/include/pcl/recognition/mask_map.h index 01fbabd0..54074614 100644 --- a/recognition/include/pcl/recognition/mask_map.h +++ b/recognition/include/pcl/recognition/mask_map.h @@ -78,7 +78,7 @@ public: return (data_.data()); } - [[deprecated("Use new version diff getDifferenceMask(mask0, mask1)")]] + PCL_DEPRECATED("Use new version diff getDifferenceMask(mask0, mask1)") static void getDifferenceMask(const MaskMap& mask0, const MaskMap& mask1, MaskMap& diff_mask); diff --git a/recognition/include/pcl/recognition/ransac_based/orr_graph.h b/recognition/include/pcl/recognition/ransac_based/orr_graph.h index a3978f55..cf118ba2 100644 --- a/recognition/include/pcl/recognition/ransac_based/orr_graph.h +++ b/recognition/include/pcl/recognition/ransac_based/orr_graph.h @@ -45,6 +45,10 @@ #pragma once +#include +#include +#include +#include #include namespace pcl diff --git a/recognition/include/pcl/recognition/ransac_based/simple_octree.h b/recognition/include/pcl/recognition/ransac_based/simple_octree.h index cdd34740..38d8e26e 100644 --- a/recognition/include/pcl/recognition/ransac_based/simple_octree.h +++ b/recognition/include/pcl/recognition/ransac_based/simple_octree.h @@ -46,7 +46,9 @@ #pragma once #include + #include +#include namespace pcl { diff --git a/recognition/include/pcl/recognition/surface_normal_modality.h b/recognition/include/pcl/recognition/surface_normal_modality.h index 50757b17..fd2e1309 100644 --- a/recognition/include/pcl/recognition/surface_normal_modality.h +++ b/recognition/include/pcl/recognition/surface_normal_modality.h @@ -4,7 +4,7 @@ * Point Cloud Library (PCL) - www.pointclouds.org * Copyright (c) 2010-2011, Willow Garage, Inc. * - * All rights reserved. + * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -45,6 +45,14 @@ #include #include +#include +#include +#include +// #include +#include +#include +#include + namespace pcl { diff --git a/registration/include/pcl/registration/impl/correspondence_rejection_sample_consensus.hpp b/registration/include/pcl/registration/impl/correspondence_rejection_sample_consensus.hpp index 5518717d..d8a4073b 100644 --- a/registration/include/pcl/registration/impl/correspondence_rejection_sample_consensus.hpp +++ b/registration/include/pcl/registration/impl/correspondence_rejection_sample_consensus.hpp @@ -74,9 +74,6 @@ pcl::registration::CorrespondenceRejectorSampleConsensus::getRemainingCo target_indices[i] = original_correspondences[i].index_match; } - // from pcl/registration/icp.hpp: - std::vector source_indices_good; - std::vector target_indices_good; { // From the set of correspondences found, attempt to remove outliers // Create the registration model 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 a8494340..12357dfe 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 @@ -78,10 +78,6 @@ pcl::registration::CorrespondenceRejectorSampleConsensus2D::getRemaining target_indices[i] = original_correspondences[i].index_match; } - // from pcl/registration/icp.hpp: - std::vector source_indices_good; - std::vector target_indices_good; - // From the set of correspondences found, attempt to remove outliers typename pcl::SampleConsensusModelRegistration2D::Ptr model (new pcl::SampleConsensusModelRegistration2D (input_, source_indices)); // Pass the target_indices diff --git a/registration/include/pcl/registration/impl/correspondence_types.hpp b/registration/include/pcl/registration/impl/correspondence_types.hpp index 503966e9..bc89e93c 100644 --- a/registration/include/pcl/registration/impl/correspondence_types.hpp +++ b/registration/include/pcl/registration/impl/correspondence_types.hpp @@ -37,13 +37,14 @@ * $Id$ * */ -#ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_TYPES_H_ -#define PCL_REGISTRATION_IMPL_CORRESPONDENCE_TYPES_H_ -#include +#pragma once + #include -////////////////////////////////////////////////////////////////////////////////////////// +#include +#include + inline void pcl::registration::getCorDistMeanStd (const pcl::Correspondences &correspondences, double &mean, double &stddev) { @@ -62,7 +63,6 @@ pcl::registration::getCorDistMeanStd (const pcl::Correspondences &correspondence stddev = sqrt (variance); } -////////////////////////////////////////////////////////////////////////////////////////// inline void pcl::registration::getQueryIndices (const pcl::Correspondences& correspondences, std::vector& indices) { @@ -71,7 +71,7 @@ pcl::registration::getQueryIndices (const pcl::Correspondences& correspondences, indices[i] = correspondences[i].index_query; } -////////////////////////////////////////////////////////////////////////////////////////// + inline void pcl::registration::getMatchIndices (const pcl::Correspondences& correspondences, std::vector& indices) { @@ -79,5 +79,3 @@ pcl::registration::getMatchIndices (const pcl::Correspondences& correspondences, for (std::size_t i = 0; i < correspondences.size (); ++i) indices[i] = correspondences[i].index_match; } - -#endif /* PCL_REGISTRATION_IMPL_CORRESPONDENCE_TYPES_H_ */ diff --git a/registration/include/pcl/registration/impl/elch.hpp b/registration/include/pcl/registration/impl/elch.hpp index c52eec71..4b234f5f 100644 --- a/registration/include/pcl/registration/impl/elch.hpp +++ b/registration/include/pcl/registration/impl/elch.hpp @@ -64,14 +64,13 @@ pcl::registration::ELCH::loopOptimizerAlgorithm (LOAGraph &g, double *we int *p_min = new int[num_vertices (g)]; double *d = new double[num_vertices (g)]; double *d_min = new double[num_vertices (g)]; - double dist; bool do_swap = false; std::list::iterator start_min, end_min; // process all junctions while (!crossings.empty ()) { - dist = -1; + double dist = -1; // find shortest crossing for all vertices on the loop for (auto crossings_it = crossings.begin (); crossings_it != crossings.end (); ) { @@ -136,12 +135,11 @@ pcl::registration::ELCH::loopOptimizerAlgorithm (LOAGraph &g, double *we delete[] d_min; boost::graph_traits::adjacency_iterator adjacent_it, adjacent_it_end; - int s; // error propagation while (!branches.empty ()) { - s = branches.front (); + int s = branches.front (); branches.pop_front (); for (std::tie (adjacent_it, adjacent_it_end) = adjacent_vertices (s, g); adjacent_it != adjacent_it_end; ++adjacent_it) diff --git a/registration/include/pcl/registration/impl/ia_fpcs.hpp b/registration/include/pcl/registration/impl/ia_fpcs.hpp index 8fff0c7c..d758c14d 100644 --- a/registration/include/pcl/registration/impl/ia_fpcs.hpp +++ b/registration/include/pcl/registration/impl/ia_fpcs.hpp @@ -179,11 +179,11 @@ pcl::registration::FPCSInitialAlignment base_indices (4); - float ratio[2]; all_candidates[i] = candidates; if (!abort) { + float ratio[2]; // select four coplanar point base if (selectBase (base_indices, ratio) == 0) { @@ -442,20 +442,20 @@ pcl::registration::FPCSInitialAlignment temp (base_indices.begin (), base_indices.end ()); // loop over all combinations of base points - for (std::vector ::const_iterator i = copy.begin (), i_e = copy.end (); i != i_e; i++) - for (std::vector ::const_iterator j = copy.begin (), j_e = copy.end (); j != j_e; j++) + for (std::vector ::const_iterator i = copy.begin (), i_e = copy.end (); i != i_e; ++i) + for (std::vector ::const_iterator j = copy.begin (), j_e = copy.end (); j != j_e; ++j) { if (i == j) continue; - for (std::vector ::const_iterator k = copy.begin (), k_e = copy.end (); k != k_e; k++) + for (std::vector ::const_iterator k = copy.begin (), k_e = copy.end (); k != k_e; ++k) { if (k == j || k == i) continue; std::vector ::const_iterator l = copy.begin (); while (l == i || l == j || l == k) - l++; + ++l; temp[0] = *i; temp[1] = *j; diff --git a/registration/include/pcl/registration/impl/ia_kfpcs.hpp b/registration/include/pcl/registration/impl/ia_kfpcs.hpp index 3d27ad53..7f08fd54 100644 --- a/registration/include/pcl/registration/impl/ia_kfpcs.hpp +++ b/registration/include/pcl/registration/impl/ia_kfpcs.hpp @@ -221,7 +221,7 @@ pcl::registration::KFPCSInitialAlignment fitness_score == FLT_MAX) @@ -236,7 +236,7 @@ pcl::registration::KFPCSInitialAlignment (0, 0)).angle (); const float translation3d = diff.block <3, 1> (0, 3).norm (); unique = angle3d > min_angle3d && translation3d > min_translation3d; - it++; + ++it; } // add candidate to best candidates @@ -260,7 +260,7 @@ pcl::registration::KFPCSInitialAlignment fitness_score > t) @@ -275,7 +275,7 @@ pcl::registration::KFPCSInitialAlignment (0, 0)).angle (); const float translation3d = diff.block <3, 1> (0, 3).norm (); unique = angle3d > min_angle3d && translation3d > min_translation3d; - it++; + ++it; } // add candidate to best candidates diff --git a/registration/include/pcl/registration/impl/ia_ransac.hpp b/registration/include/pcl/registration/impl/ia_ransac.hpp index 4a357b36..2026ccb1 100644 --- a/registration/include/pcl/registration/impl/ia_ransac.hpp +++ b/registration/include/pcl/registration/impl/ia_ransac.hpp @@ -211,7 +211,7 @@ pcl::SampleConsensusInitialAlignment::comput std::vector sample_indices (nr_samples_); std::vector corresponding_indices (nr_samples_); PointCloudSource input_transformed; - float error, lowest_error (0); + float lowest_error (0); final_transformation_ = guess; int i_iter = 0; @@ -237,7 +237,7 @@ pcl::SampleConsensusInitialAlignment::comput // Transform the data and compute the error transformPointCloud (*input_, input_transformed, transformation_); - error = computeErrorMetric (input_transformed, static_cast (corr_dist_threshold_)); + float error = computeErrorMetric (input_transformed, static_cast (corr_dist_threshold_)); // If the new error is lower, update the final transformation if (i_iter == 0 || error < lowest_error) diff --git a/registration/include/pcl/registration/impl/lum.hpp b/registration/include/pcl/registration/impl/lum.hpp index b57665ce..9a54279a 100644 --- a/registration/include/pcl/registration/impl/lum.hpp +++ b/registration/include/pcl/registration/impl/lum.hpp @@ -234,10 +234,11 @@ pcl::registration::LUM::compute () { // Attempt to use the forward edge, otherwise use backward edge, otherwise there was no edge Edge e; - bool present1, present2; + bool present1; std::tie (e, present1) = edge (vi, vj, *slam_graph_); if (!present1) { + bool present2; std::tie (e, present2) = edge (vj, vi, *slam_graph_); if (!present2) continue; diff --git a/registration/include/pcl/registration/impl/ndt.hpp b/registration/include/pcl/registration/impl/ndt.hpp index 2de0de7b..d5bfd774 100644 --- a/registration/include/pcl/registration/impl/ndt.hpp +++ b/registration/include/pcl/registration/impl/ndt.hpp @@ -109,7 +109,6 @@ pcl::NormalDistributionsTransform::computeTransformati Eigen::Matrix hessian; double score = 0; - double delta_p_norm; // Calculate derivates of initial transform vector, subsequent derivative calculations are done in the step length determination. score = computeDerivatives (score_gradient, hessian, output, p); @@ -125,7 +124,7 @@ pcl::NormalDistributionsTransform::computeTransformati delta_p = sv.solve (-score_gradient); //Calculate step length with guarnteed sufficient decrease [More, Thuente 1994] - delta_p_norm = delta_p.norm (); + double delta_p_norm = delta_p.norm (); if (delta_p_norm == 0 || std::isnan(delta_p_norm)) { @@ -206,7 +205,7 @@ pcl::NormalDistributionsTransform::computeDerivatives std::vector distances; target_cells_.radiusSearch (x_trans_pt, resolution_, neighborhood, distances); - for (typename std::vector::iterator neighborhood_it = neighborhood.begin (); neighborhood_it != neighborhood.end (); neighborhood_it++) + for (typename std::vector::iterator neighborhood_it = neighborhood.begin (); neighborhood_it != neighborhood.end (); ++neighborhood_it) { cell = *neighborhood_it; x_pt = input_->points[idx]; @@ -421,7 +420,7 @@ pcl::NormalDistributionsTransform::computeHessian (Eig std::vector distances; target_cells_.radiusSearch (x_trans_pt, resolution_, neighborhood, distances); - for (typename std::vector::iterator neighborhood_it = neighborhood.begin (); neighborhood_it != neighborhood.end (); neighborhood_it++) + for (typename std::vector::iterator neighborhood_it = neighborhood.begin (); neighborhood_it != neighborhood.end (); ++neighborhood_it) { cell = *neighborhood_it; diff --git a/registration/include/pcl/registration/impl/ndt_2d.hpp b/registration/include/pcl/registration/impl/ndt_2d.hpp index bc02f92f..c34bdf35 100644 --- a/registration/include/pcl/registration/impl/ndt_2d.hpp +++ b/registration/include/pcl/registration/impl/ndt_2d.hpp @@ -233,10 +233,9 @@ namespace pcl normal_distributions_ (cells_[0], cells_[1]) { // sort through all points, assigning them to distributions: - NormalDist* n; std::size_t used_points = 0; for (std::size_t i = 0; i < cloud->size (); i++) - if ((n = normalDistForPoint (cloud->at (i)))) + if (NormalDist* n = normalDistForPoint (cloud->at (i))) { n->addIdx (i); used_points++; diff --git a/registration/include/pcl/registration/impl/pyramid_feature_matching.hpp b/registration/include/pcl/registration/impl/pyramid_feature_matching.hpp index b496f95e..5dfd6c49 100644 --- a/registration/include/pcl/registration/impl/pyramid_feature_matching.hpp +++ b/registration/include/pcl/registration/impl/pyramid_feature_matching.hpp @@ -69,7 +69,7 @@ pcl::PyramidFeatureHistogram::comparePyramidFeatureHistograms (con PCL_ERROR ("[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] The two given pyramids have different numbers of bins on level 0: %u vs %u\n", pyramid_a->hist_levels[0].hist.size (), pyramid_b->hist_levels[0].hist.size ()); return -1; } - float match_count_level = 0.0f, match_count_prev_level = 0.0f; + float match_count_level = 0.0f; for (std::size_t bin_i = 0; bin_i < pyramid_a->hist_levels[0].hist.size (); ++bin_i) { if (pyramid_a->hist_levels[0].hist[bin_i] < pyramid_b->hist_levels[0].hist[bin_i]) @@ -88,7 +88,7 @@ pcl::PyramidFeatureHistogram::comparePyramidFeatureHistograms (con return -1; } - match_count_prev_level = match_count_level; + float match_count_prev_level = match_count_level; match_count_level = 0.0f; for (std::size_t bin_i = 0; bin_i < pyramid_a->hist_levels[level_i].hist.size (); ++bin_i) { diff --git a/sample_consensus/include/pcl/sample_consensus/impl/lmeds.hpp b/sample_consensus/include/pcl/sample_consensus/impl/lmeds.hpp index 308ece2c..dd4809ee 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/lmeds.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/lmeds.hpp @@ -57,7 +57,6 @@ pcl::LeastMedianSquares::computeModel (int debug_verbosity_level) iterations_ = 0; double d_best_penalty = std::numeric_limits::max(); - std::vector best_model; std::vector selection; Eigen::VectorXf model_coefficients; std::vector distances; diff --git a/sample_consensus/include/pcl/sample_consensus/impl/mlesac.hpp b/sample_consensus/include/pcl/sample_consensus/impl/mlesac.hpp index 89c15df6..0a2ba293 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/mlesac.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/mlesac.hpp @@ -59,7 +59,6 @@ pcl::MaximumLikelihoodSampleConsensus::computeModel (int debug_verbosity double d_best_penalty = std::numeric_limits::max(); double k = 1.0; - std::vector best_model; std::vector selection; Eigen::VectorXf model_coefficients; std::vector distances; diff --git a/sample_consensus/include/pcl/sample_consensus/impl/msac.hpp b/sample_consensus/include/pcl/sample_consensus/impl/msac.hpp index c6a5c995..dd2ed9f2 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/msac.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/msac.hpp @@ -58,7 +58,6 @@ pcl::MEstimatorSampleConsensus::computeModel (int debug_verbosity_level) double d_best_penalty = std::numeric_limits::max(); double k = 1.0; - std::vector best_model; std::vector selection; Eigen::VectorXf model_coefficients; std::vector distances; diff --git a/sample_consensus/include/pcl/sample_consensus/impl/ransac.hpp b/sample_consensus/include/pcl/sample_consensus/impl/ransac.hpp index 0da883ed..6cf00feb 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/ransac.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/ransac.hpp @@ -75,6 +75,7 @@ pcl::RandomSampleConsensus::computeModel (int) std::size_t n_inliers_count; unsigned skipped_count = 0; + // suppress infinite loops by just allowing 10 x maximum allowed iterations for invalid model parameters! const unsigned max_skip = max_iterations_ * 10; diff --git a/sample_consensus/include/pcl/sample_consensus/impl/rmsac.hpp b/sample_consensus/include/pcl/sample_consensus/impl/rmsac.hpp index 8a20e19a..50c94025 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/rmsac.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/rmsac.hpp @@ -58,7 +58,6 @@ pcl::RandomizedMEstimatorSampleConsensus::computeModel (int debug_verbos double d_best_penalty = std::numeric_limits::max(); double k = 1.0; - std::vector best_model; std::vector selection; Eigen::VectorXf model_coefficients; std::vector distances; diff --git a/segmentation/include/pcl/segmentation/euclidean_cluster_comparator.h b/segmentation/include/pcl/segmentation/euclidean_cluster_comparator.h index 879ce700..7295978e 100644 --- a/segmentation/include/pcl/segmentation/euclidean_cluster_comparator.h +++ b/segmentation/include/pcl/segmentation/euclidean_cluster_comparator.h @@ -39,6 +39,7 @@ #pragma once +#include #include #include #include @@ -210,7 +211,7 @@ namespace pcl using experimental::EuclideanClusterComparator::setExcludeLabels; /** \brief Default constructor for EuclideanClusterComparator. */ - [[deprecated("remove PointNT from template parameters")]] + PCL_DEPRECATED("remove PointNT from template parameters") EuclideanClusterComparator () : normals_ () , angular_threshold_ (0.0f) @@ -219,19 +220,19 @@ namespace pcl /** \brief Provide a pointer to the input normals. * \param[in] normals the input normal cloud */ - [[deprecated("EuclideadClusterComparator never actually used normals and angular threshold, this function has no effect on the behavior of the comparator. It is deprecated and will be removed in future releases.")]] + PCL_DEPRECATED("EuclideadClusterComparator never actually used normals and angular threshold, this function has no effect on the behavior of the comparator. It is deprecated and will be removed in future releases.") inline void setInputNormals (const PointCloudNConstPtr& normals) { normals_ = normals; } /** \brief Get the input normals. */ - [[deprecated("EuclideadClusterComparator never actually used normals and angular threshold, this function has no effect on the behavior of the comparator. It is deprecated and will be removed in future releases.")]] + PCL_DEPRECATED("EuclideadClusterComparator never actually used normals and angular threshold, this function has no effect on the behavior of the comparator. It is deprecated and will be removed in future releases.") inline PointCloudNConstPtr getInputNormals () const { return (normals_); } /** \brief Set the tolerance in radians for difference in normal direction between neighboring points, to be considered part of the same plane. * \param[in] angular_threshold the tolerance in radians */ - [[deprecated("EuclideadClusterComparator never actually used normals and angular threshold, this function has no effect on the behavior of the comparator. It is deprecated and will be removed in future releases.")]] + PCL_DEPRECATED("EuclideadClusterComparator never actually used normals and angular threshold, this function has no effect on the behavior of the comparator. It is deprecated and will be removed in future releases.") inline void setAngularThreshold (float angular_threshold) { @@ -239,14 +240,14 @@ namespace pcl } /** \brief Get the angular threshold in radians for difference in normal direction between neighboring points, to be considered part of the same plane. */ - [[deprecated("EuclideadClusterComparator never actually used normals and angular threshold, this function has no effect on the behavior of the comparator. It is deprecated and will be removed in future releases.")]] + PCL_DEPRECATED("EuclideadClusterComparator never actually used normals and angular threshold, this function has no effect on the behavior of the comparator. It is deprecated and will be removed in future releases.") inline float getAngularThreshold () const { return (std::acos (angular_threshold_) ); } /** \brief Set labels in the label cloud to exclude. * \param[in] exclude_labels a vector of bools corresponding to whether or not a given label should be considered */ - [[deprecated("use setExcludeLabels(const ExcludeLabelSetConstPtr &) instead")]] + PCL_DEPRECATED("use setExcludeLabels(const ExcludeLabelSetConstPtr &) instead") void setExcludeLabels (const std::vector& exclude_labels) { diff --git a/segmentation/include/pcl/segmentation/impl/lccp_segmentation.hpp b/segmentation/include/pcl/segmentation/impl/lccp_segmentation.hpp index c5841c1d..a43243f9 100644 --- a/segmentation/include/pcl/segmentation/impl/lccp_segmentation.hpp +++ b/segmentation/include/pcl/segmentation/impl/lccp_segmentation.hpp @@ -367,7 +367,6 @@ pcl::LCCPSegmentation::applyKconvexity (const unsigned int k_arg) if (k_arg == 0) return; - bool is_convex; unsigned int kcount = 0; EdgeIterator edge_itr, edge_itr_end, next_edge; @@ -381,7 +380,7 @@ pcl::LCCPSegmentation::applyKconvexity (const unsigned int k_arg) { next_edge++; // next_edge iterator is necessary, because removing an edge invalidates the iterator to the current edge - is_convex = sv_adjacency_list_[*edge_itr].is_convex; + bool is_convex = sv_adjacency_list_[*edge_itr].is_convex; if (is_convex) // If edge is (0-)convex { @@ -430,7 +429,6 @@ pcl::LCCPSegmentation::applyKconvexity (const unsigned int k_arg) template void pcl::LCCPSegmentation::calculateConvexConnections (SupervoxelAdjacencyList& adjacency_list_arg) { - bool is_convex; EdgeIterator edge_itr, edge_itr_end, next_edge; boost::tie (edge_itr, edge_itr_end) = boost::edges (adjacency_list_arg); @@ -443,7 +441,7 @@ pcl::LCCPSegmentation::calculateConvexConnections (SupervoxelAdjacencyLi std::uint32_t target_sv_label = adjacency_list_arg[boost::target (*edge_itr, adjacency_list_arg)]; float normal_difference; - is_convex = connIsConvex (source_sv_label, target_sv_label, normal_difference); + bool is_convex = connIsConvex (source_sv_label, target_sv_label, normal_difference); adjacency_list_arg[*edge_itr].is_convex = is_convex; adjacency_list_arg[*edge_itr].is_valid = is_convex; adjacency_list_arg[*edge_itr].normal_difference = normal_difference; diff --git a/segmentation/include/pcl/segmentation/impl/region_growing.hpp b/segmentation/include/pcl/segmentation/impl/region_growing.hpp index 1a400a82..f65ad66c 100644 --- a/segmentation/include/pcl/segmentation/impl/region_growing.hpp +++ b/segmentation/include/pcl/segmentation/impl/region_growing.hpp @@ -276,13 +276,13 @@ pcl::RegionGrowing::extract (std::vector & c clusters.resize (clusters_.size ()); std::vector::iterator cluster_iter_input = clusters.begin (); - for (std::vector::const_iterator cluster_iter = clusters_.begin (); cluster_iter != clusters_.end (); cluster_iter++) + for (std::vector::const_iterator cluster_iter = clusters_.begin (); cluster_iter != clusters_.end (); ++cluster_iter) { if ((static_cast (cluster_iter->indices.size ()) >= min_pts_per_cluster_) && (static_cast (cluster_iter->indices.size ()) <= max_pts_per_cluster_)) { *cluster_iter_input = *cluster_iter; - cluster_iter_input++; + ++cluster_iter_input; } } diff --git a/segmentation/include/pcl/segmentation/impl/region_growing_rgb.hpp b/segmentation/include/pcl/segmentation/impl/region_growing_rgb.hpp index 5c6bdb1f..14b232ed 100644 --- a/segmentation/include/pcl/segmentation/impl/region_growing_rgb.hpp +++ b/segmentation/include/pcl/segmentation/impl/region_growing_rgb.hpp @@ -203,7 +203,7 @@ pcl::RegionGrowingRGB::extract (std::vector cluster_iter = clusters_.erase (cluster_iter); } else - cluster_iter++; + ++cluster_iter; } clusters.reserve (clusters_.size ()); @@ -399,10 +399,9 @@ pcl::RegionGrowingRGB::applyRegionMergingAlgorithm () float dist_thresh = distance_threshold_; int homogeneous_region_number = 0; - int curr_homogeneous_region = 0; for (int i_seg = 0; i_seg < number_of_segments_; i_seg++) { - curr_homogeneous_region = 0; + int curr_homogeneous_region = 0; if (segment_labels_[i_seg] == -1) { segment_labels_[i_seg] = homogeneous_region_number; @@ -595,9 +594,9 @@ pcl::RegionGrowingRGB::assembleRegions (std::vectorindices.empty ()) && itr1 < itr2) - itr1++; + ++itr1; while ( itr2->indices.empty () && itr1 < itr2) - itr2--; + --itr2; if (itr1 != itr2) itr1->indices.swap (itr2->indices); diff --git a/segmentation/include/pcl/segmentation/impl/supervoxel_clustering.hpp b/segmentation/include/pcl/segmentation/impl/supervoxel_clustering.hpp index 281501cd..eb320d0b 100644 --- a/segmentation/include/pcl/segmentation/impl/supervoxel_clustering.hpp +++ b/segmentation/include/pcl/segmentation/impl/supervoxel_clustering.hpp @@ -585,8 +585,6 @@ pcl::SupervoxelClustering::getLabeledCloud () const pcl::copyPointCloud (*input_,*labeled_cloud); typename pcl::PointCloud ::const_iterator i_input = input_->begin (); - std::vector indices; - std::vector sqr_distances; for (auto i_labeled = labeled_cloud->begin (); i_labeled != labeled_cloud->end (); ++i_labeled,++i_input) { if ( !pcl::isFinite (*i_input)) diff --git a/segmentation/include/pcl/segmentation/organized_multi_plane_segmentation.h b/segmentation/include/pcl/segmentation/organized_multi_plane_segmentation.h index 3d704f59..e73b7a24 100644 --- a/segmentation/include/pcl/segmentation/organized_multi_plane_segmentation.h +++ b/segmentation/include/pcl/segmentation/organized_multi_plane_segmentation.h @@ -41,6 +41,7 @@ #include #include +#include #include #include #include @@ -284,7 +285,7 @@ namespace pcl * \param [in] labels The labels produced by the initial segmentation * \param [in] label_indices The list of indices corresponding to each label */ - [[deprecated("centroids and covariances parameters are not used; they are deprecated and will be removed in future releases")]] + PCL_DEPRECATED("centroids and covariances parameters are not used; they are deprecated and will be removed in future releases") void refine (std::vector& model_coefficients, std::vector& inlier_indices, diff --git a/segmentation/include/pcl/segmentation/segment_differences.h b/segmentation/include/pcl/segmentation/segment_differences.h index f822a7f8..ecd181b8 100755 --- a/segmentation/include/pcl/segmentation/segment_differences.h +++ b/segmentation/include/pcl/segmentation/segment_differences.h @@ -38,6 +38,7 @@ #pragma once #include +#include #include namespace pcl @@ -59,7 +60,7 @@ namespace pcl pcl::PointCloud &output); template - [[deprecated("tgt parameter is not used; it is deprecated and will be removed in future releases")]] + PCL_DEPRECATED("tgt parameter is not used; it is deprecated and will be removed in future releases") inline void getPointCloudDifference ( const pcl::PointCloud &src, const pcl::PointCloud & /* tgt */, diff --git a/segmentation/include/pcl/segmentation/supervoxel_clustering.h b/segmentation/include/pcl/segmentation/supervoxel_clustering.h index b26aa1a1..5ce354c1 100644 --- a/segmentation/include/pcl/segmentation/supervoxel_clustering.h +++ b/segmentation/include/pcl/segmentation/supervoxel_clustering.h @@ -194,7 +194,7 @@ namespace pcl */ SupervoxelClustering (float voxel_resolution, float seed_resolution); - [[deprecated("constructor with flag for using the single camera transform is deprecated. Default behavior is now to use the transform for organized clouds, and not use it for unorganized. Use setUseSingleCameraTransform() to override the defaults.")]] + PCL_DEPRECATED("constructor with flag for using the single camera transform is deprecated. Default behavior is now to use the transform for organized clouds, and not use it for unorganized. Use setUseSingleCameraTransform() to override the defaults.") SupervoxelClustering (float voxel_resolution, float seed_resolution, bool) : SupervoxelClustering (voxel_resolution, seed_resolution) { } /** \brief This destructor destroys the cloud, normals and search method used for @@ -278,7 +278,7 @@ namespace pcl * color(it's random). Points that are unlabeled will be black * \note This will expand the label_colors_ vector so that it can accommodate all labels */ - [[deprecated("use getLabeledCloud() instead. An example of how to display and save with colorized labels can be found in examples/segmentation/example_supervoxels.cpp")]] + PCL_DEPRECATED("use getLabeledCloud() instead. An example of how to display and save with colorized labels can be found in examples/segmentation/example_supervoxels.cpp") typename pcl::PointCloud::Ptr getColoredCloud () const { diff --git a/simulation/CMakeLists.txt b/simulation/CMakeLists.txt index 00cb8543..e6f58073 100644 --- a/simulation/CMakeLists.txt +++ b/simulation/CMakeLists.txt @@ -47,4 +47,6 @@ PCL_MAKE_PKGCONFIG(${LIB_NAME} COMPONENT ${SUBSYS_NAME} DESC ${SUBSYS_DESC} PCL_ # Install include files PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}" ${incs}) -add_subdirectory(tools) +if(BUILD_tools) + add_subdirectory(tools) +endif() diff --git a/simulation/tools/CMakeLists.txt b/simulation/tools/CMakeLists.txt index ac766574..77008984 100644 --- a/simulation/tools/CMakeLists.txt +++ b/simulation/tools/CMakeLists.txt @@ -1,3 +1,5 @@ +set(SUBSYS_NAME tools) + find_package(GLEW) find_package(GLUT) diff --git a/stereo/include/pcl/stereo/stereo_grabber.h b/stereo/include/pcl/stereo/stereo_grabber.h index 2ed21452..02263d98 100644 --- a/stereo/include/pcl/stereo/stereo_grabber.h +++ b/stereo/include/pcl/stereo/stereo_grabber.h @@ -88,7 +88,7 @@ public: } /** \brief Virtual destructor. */ - ~StereoGrabberBase() throw(); + ~StereoGrabberBase() noexcept; /** \brief Starts playing the list of Stereo images if frames_per_second is > 0. * Otherwise it works as a trigger: publishes only the next pair in the list. */ diff --git a/stereo/src/stereo_grabber.cpp b/stereo/src/stereo_grabber.cpp index a3a2ec8f..1729a72e 100644 --- a/stereo/src/stereo_grabber.cpp +++ b/stereo/src/stereo_grabber.cpp @@ -152,7 +152,7 @@ pcl::StereoGrabberBase::StereoGrabberBase( {} /////////////////////////////////////////////////////////////////////////////////////////// -pcl::StereoGrabberBase::~StereoGrabberBase() throw() +pcl::StereoGrabberBase::~StereoGrabberBase() noexcept { stop(); delete impl_; diff --git a/surface/include/pcl/surface/3rdparty/poisson4/octree_poisson.hpp b/surface/include/pcl/surface/3rdparty/poisson4/octree_poisson.hpp index 147982d9..1ea060c0 100644 --- a/surface/include/pcl/surface/3rdparty/poisson4/octree_poisson.hpp +++ b/surface/include/pcl/surface/3rdparty/poisson4/octree_poisson.hpp @@ -861,7 +861,7 @@ namespace pcl template OctNode* OctNode::__faceNeighbor(int dir,int off,int forceChildren){ if(!parent){return NULL;} - int pIndex=int(this-parent->children); + int pIndex=int(this-(parent->children)); pIndex^=(1<children[pIndex];} else{ @@ -877,7 +877,7 @@ namespace pcl template const OctNode* OctNode::__faceNeighbor(int dir,int off) const { if(!parent){return NULL;} - int pIndex=int(this-parent->children); + int pIndex=int(this-(parent->children)); pIndex^=(1<children[pIndex];} else{ @@ -912,7 +912,7 @@ namespace pcl template const OctNode* OctNode::__edgeNeighbor(int o,const int i[2],const int idx[2]) const{ if(!parent){return NULL;} - int pIndex=int(this-parent->children); + int pIndex=int(this-(parent->children)); int aIndex,x[DIMENSION]; Cube::FactorCornerIndex(pIndex,x[0],x[1],x[2]); @@ -941,7 +941,7 @@ namespace pcl template OctNode* OctNode::__edgeNeighbor(int o,const int i[2],const int idx[2],int forceChildren){ if(!parent){return NULL;} - int pIndex=int(this-parent->children); + int pIndex=int(this-(parent->children)); int aIndex,x[DIMENSION]; Cube::FactorCornerIndex(pIndex,x[0],x[1],x[2]); @@ -977,7 +977,7 @@ namespace pcl int pIndex,aIndex=0; if(!parent){return NULL;} - pIndex=int(this-parent->children); + pIndex=int(this-(parent->children)); aIndex=(cornerIndex ^ pIndex); // The disagreement bits pIndex=(~pIndex)&7; // The antipodal point if(aIndex==7){ // Agree on no bits @@ -1025,7 +1025,7 @@ namespace pcl int pIndex,aIndex=0; if(!parent){return NULL;} - pIndex=int(this-parent->children); + pIndex=int(this-(parent->children)); aIndex=(cornerIndex ^ pIndex); // The disagreement bits pIndex=(~pIndex)&7; // The antipodal point if(aIndex==7){ // Agree on no bits diff --git a/surface/include/pcl/surface/impl/concave_hull.hpp b/surface/include/pcl/surface/impl/concave_hull.hpp index 65b731ec..f8f945f2 100644 --- a/surface/include/pcl/surface/impl/concave_hull.hpp +++ b/surface/include/pcl/surface/impl/concave_hull.hpp @@ -251,7 +251,6 @@ pcl::ConcaveHull::performReconstruction (PointCloud &alpha_shape, std: std::vector qhid_to_pcidx (max_vertex_id); int num_facets = qh num_facets; - int dd = 0; if (dim_ == 3) { @@ -268,7 +267,6 @@ pcl::ConcaveHull::performReconstruction (PointCloud &alpha_shape, std: vertexT *anyVertex = static_cast (facet->vertices->e[0].p); double *center = facet->center; double r = qh_pointdist (anyVertex->point,center,dim_); - facetT *neighb; if (voronoi_centers_) { @@ -288,7 +286,7 @@ pcl::ConcaveHull::performReconstruction (PointCloud &alpha_shape, std: ridgeT *ridge, **ridgep; FOREACHridge_ (facet->ridges) { - neighb = otherfacet_ (ridge, facet); + facetT *neighb = otherfacet_ (ridge, facet); if ((neighb->visitid != qh visit_id)) qh_setappend (&triangles_set, ridge); } @@ -386,6 +384,7 @@ pcl::ConcaveHull::performReconstruction (PointCloud &alpha_shape, std: if (voronoi_centers_) voronoi_centers_->points.resize (num_facets); + int dd = 0; FORALLfacets { // Facets are the delaunay triangles (2d) @@ -469,7 +468,6 @@ pcl::ConcaveHull::performReconstruction (PointCloud &alpha_shape, std: alpha_shape.points.resize (vertices); - std::vector > connected; PointCloud alpha_shape_sorted; alpha_shape_sorted.points.resize (vertices); diff --git a/surface/include/pcl/surface/impl/gp3.hpp b/surface/include/pcl/surface/impl/gp3.hpp index 4fab4658..728e5a12 100644 --- a/surface/include/pcl/surface/impl/gp3.hpp +++ b/surface/include/pcl/surface/impl/gp3.hpp @@ -135,7 +135,6 @@ pcl::GreedyProjectionTriangulation::reconstructPolygons (std::vector

> uvn_nn (nnn_); Eigen::Vector2f uvn_s; @@ -281,7 +280,7 @@ pcl::GreedyProjectionTriangulation::reconstructPolygons (std::vector

::reconstructPolygons (std::vector

::iterator first_gap_after = angleIdx.end (); std::vector::iterator last_gap_before = angleIdx.begin (); int nr_gaps = 0; - for (std::vector::iterator it = angleIdx.begin (); it != angleIdx.end () - 1; it++) + for (std::vector::iterator it = angleIdx.begin (); it != angleIdx.end () - 1; ++it) { if (gaps[*it]) { @@ -813,7 +812,7 @@ pcl::GreedyProjectionTriangulation::reconstructPolygons (std::vector

to_erase; - for (std::vector::iterator it = angleIdx.begin()+1; it != angleIdx.end()-1; it++) + for (std::vector::iterator it = angleIdx.begin()+1; it != angleIdx.end()-1; ++it) { if (gaps[*(it-1)]) angle_so_far = 0; @@ -845,13 +844,13 @@ pcl::GreedyProjectionTriangulation::reconstructPolygons (std::vector

::iterator prev_it; int erased_idx = static_cast (to_erase.size ()) -1; - for (prev_it = it-1; (erased_idx != -1) && (it != angleIdx.begin()); it--) + for (prev_it = it-1; (erased_idx != -1) && (it != angleIdx.begin()); --it) if (*it == to_erase[erased_idx]) erased_idx--; else break; bool can_delete = true; - for (std::vector::iterator curr_it = prev_it+1; curr_it != it+1; curr_it++) + for (std::vector::iterator curr_it = prev_it+1; curr_it != it+1; ++curr_it) { tmp_ = coords_[angles_[*curr_it].index] - proj_qp_; X[0] = tmp_.dot(u_); @@ -881,7 +880,7 @@ pcl::GreedyProjectionTriangulation::reconstructPolygons (std::vector

::iterator iter = angleIdx.begin(); iter != angleIdx.end(); iter++) + for (std::vector::iterator iter = angleIdx.begin(); iter != angleIdx.end(); ++iter) if (it == *iter) { angleIdx.erase(iter); @@ -894,7 +893,7 @@ pcl::GreedyProjectionTriangulation::reconstructPolygons (std::vector

::iterator it = angleIdx.begin()+1; it != angleIdx.end()-1; it++) + for (std::vector::iterator it = angleIdx.begin()+1; it != angleIdx.end()-1; ++it) { current_index_ = angles_[*it].index; diff --git a/surface/include/pcl/surface/impl/texture_mapping.hpp b/surface/include/pcl/surface/impl/texture_mapping.hpp index 4ae4eb07..a6d60716 100644 --- a/surface/include/pcl/surface/impl/texture_mapping.hpp +++ b/surface/include/pcl/surface/impl/texture_mapping.hpp @@ -254,11 +254,10 @@ pcl::TextureMapping::mapTexture2MeshUV (pcl::TextureMesh &tex_mesh) // processing for each face for (std::size_t i = 0; i < tex_mesh.tex_polygons[m].size (); ++i) { - std::size_t idx; Eigen::Vector2f tmp_VT; for (std::size_t j = 0; j < tex_mesh.tex_polygons[m][i].vertices.size (); ++j) { - idx = tex_mesh.tex_polygons[m][i].vertices[j]; + std::size_t idx = tex_mesh.tex_polygons[m][i].vertices[j]; memcpy (&x_, &tex_mesh.cloud.data[idx * point_size + tex_mesh.cloud.fields[0].offset], sizeof(float)); memcpy (&y_, &tex_mesh.cloud.data[idx * point_size + tex_mesh.cloud.fields[1].offset], sizeof(float)); memcpy (&z_, &tex_mesh.cloud.data[idx * point_size + tex_mesh.cloud.fields[2].offset], sizeof(float)); @@ -302,9 +301,6 @@ pcl::TextureMapping::mapMultipleTexturesToMeshUV (pcl::TextureMesh &te // convert mesh's cloud to pcl format for ease pcl::fromPCLPointCloud2 (tex_mesh.cloud, *originalCloud); - // texture coordinates for each mesh - std::vector > > texture_map; - for (std::size_t m = 0; m < cams.size (); ++m) { // get current camera parameters diff --git a/surface/include/pcl/surface/marching_cubes.h b/surface/include/pcl/surface/marching_cubes.h index 12a315ae..5a499be5 100644 --- a/surface/include/pcl/surface/marching_cubes.h +++ b/surface/include/pcl/surface/marching_cubes.h @@ -438,7 +438,7 @@ namespace pcl std::vector grid_; /** \brief The grid resolution */ - int res_x_, res_y_, res_z_; + int res_x_ = 32, res_y_ = 32, res_z_ = 32; /** \brief bounding box */ Eigen::Array3f upper_boundary_; diff --git a/surface/include/pcl/surface/mls.h b/surface/include/pcl/surface/mls.h index e4a3484f..232a3975 100644 --- a/surface/include/pcl/surface/mls.h +++ b/surface/include/pcl/surface/mls.h @@ -356,7 +356,7 @@ namespace pcl /** \brief Sets whether the surface and normal are approximated using a polynomial, or only via tangent estimation. * \param[in] polynomial_fit set to true for polynomial fit */ - [[deprecated("use setPolynomialOrder() instead")]] + PCL_DEPRECATED("use setPolynomialOrder() instead") inline void setPolynomialFit (bool polynomial_fit) { @@ -374,7 +374,7 @@ namespace pcl } /** \brief Get the polynomial_fit value (true if the surface and normal are approximated using a polynomial). */ - [[deprecated("use getPolynomialOrder() instead")]] + PCL_DEPRECATED("use getPolynomialOrder() instead") inline bool getPolynomialFit () const { return (order_ > 1); } @@ -746,7 +746,7 @@ namespace pcl }; template - using MovingLeastSquaresOMP [[deprecated("use MovingLeastSquares instead, it supports OpenMP now")]] = MovingLeastSquares; + using MovingLeastSquaresOMP PCL_DEPRECATED("use MovingLeastSquares instead, it supports OpenMP now") = MovingLeastSquares; } #ifdef PCL_NO_PRECOMPILE diff --git a/surface/src/3rdparty/opennurbs/opennurbs_annotation2.cpp b/surface/src/3rdparty/opennurbs/opennurbs_annotation2.cpp index 0c8d4ea6..1e3d2b08 100644 --- a/surface/src/3rdparty/opennurbs/opennurbs_annotation2.cpp +++ b/surface/src/3rdparty/opennurbs/opennurbs_annotation2.cpp @@ -16,6 +16,8 @@ #include "pcl/surface/3rdparty/opennurbs/opennurbs.h" +#include + // This define is up here so anybody who want's to defeat the // bozo vaccine has to be doing it on purpose. #define BOZO_VACCINE_699FCC4262D4488c9109F1B7A37CE926 @@ -5820,7 +5822,7 @@ void ON_Annotation2Text::SetText(const wchar_t* s) // SDKBREAK Oct 30, 07 - LW // This function should not be used any longer -[[deprecated("Use the version that takes a model transform argument")]] +PCL_DEPRECATED("Use the version that takes a model transform argument") bool ON_Annotation2::GetTextXform( ON_RECT /*gdi_text_rect*/, const ON_Font& /*font*/, @@ -5860,7 +5862,7 @@ bool ON_Annotation2::GetTextXform( // New function added Oct 30, 07 - LW // To use model xform to draw annotation in blocks correctly #if 0 -[[deprecated("Use the version that takes a dimstyle pointer")]] +PCL_DEPRECATED("Use the version that takes a dimstyle pointer") bool ON_Annotation2::GetTextXform( ON_RECT gdi_text_rect, const ON_Font& font, @@ -5989,7 +5991,7 @@ static bool GetLeaderEndAndDirection( const ON_Annotation2* pAnn, // SDKBREAK Oct 30, 07 - LW // This function should not be used any longer -[[deprecated("Use the version that takes a model transform argument")]] +PCL_DEPRECATED("Use the version that takes a model transform argument") bool ON_Annotation2::GetTextXform( ON_RECT /*gdi_text_rect*/, int /*gdi_height_of_I*/, diff --git a/surface/src/on_nurbs/closing_boundary.cpp b/surface/src/on_nurbs/closing_boundary.cpp index 5a1cf201..8b25eaf2 100644 --- a/surface/src/on_nurbs/closing_boundary.cpp +++ b/surface/src/on_nurbs/closing_boundary.cpp @@ -390,7 +390,6 @@ ClosingBoundary::optimizeBoundary (std::vector &nurbs_list, std FittingSurface fit (&data_list[n1], nurbs_list[n1]); FittingSurface::Parameter paramFP (1.0, param.smoothness, 0.0, 1.0, param.smoothness, 0.0); - std::vector wBnd, wInt; for (std::size_t i = 0; i < data_list[n1].boundary.size (); i++) data_list[n1].boundary_weight.push_back (param.boundary_weight); for (std::size_t i = 0; i < data_list[n1].interior.size (); i++) diff --git a/surface/src/on_nurbs/sparse_mat.cpp b/surface/src/on_nurbs/sparse_mat.cpp index 77b8cb10..82fdeaf5 100644 --- a/surface/src/on_nurbs/sparse_mat.cpp +++ b/surface/src/on_nurbs/sparse_mat.cpp @@ -207,9 +207,6 @@ SparseMat::nonzeros () void SparseMat::printLong () { - std::map >::iterator it_row; - std::map::iterator it_col; - int si, sj; size (si, sj); diff --git a/test/2d/test_2d.cpp b/test/2d/test_2d.cpp index 0686c15b..2301318a 100644 --- a/test/2d/test_2d.cpp +++ b/test/2d/test_2d.cpp @@ -43,7 +43,7 @@ #include #include -#include +#include #include #include diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index f5ebd675..ad5492cf 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -16,9 +16,18 @@ include_directories(SYSTEM ${GTEST_INCLUDE_DIRS} ${GTEST_SRC_DIR}) add_library(pcl_gtest STATIC ${GTEST_SRC_DIR}/src/gtest-all.cc) enable_testing() -add_custom_target(tests "${CMAKE_CTEST_COMMAND}" "-V" VERBATIM) + +if(MSVC) + # VS needs -C config to run correct + add_custom_target(tests "${CMAKE_CTEST_COMMAND}" -C $<$:Debug>$<$:Release> -V -T Test VERBATIM) +else() + add_custom_target(tests "${CMAKE_CTEST_COMMAND}" -V -T Test VERBATIM) +endif() + set_target_properties(tests PROPERTIES FOLDER "Tests") +include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include") + add_subdirectory(2d) add_subdirectory(common) add_subdirectory(features) diff --git a/test/common/CMakeLists.txt b/test/common/CMakeLists.txt index 39dced36..904016c2 100644 --- a/test/common/CMakeLists.txt +++ b/test/common/CMakeLists.txt @@ -17,6 +17,7 @@ PCL_ADD_TEST(common_test_wrappers test_wrappers FILES test_wrappers.cpp LINK_WIT PCL_ADD_TEST(common_test_macros test_macros FILES test_macros.cpp LINK_WITH pcl_gtest pcl_common) PCL_ADD_TEST(common_vector_average test_vector_average FILES test_vector_average.cpp LINK_WITH pcl_gtest) PCL_ADD_TEST(common_common test_common FILES test_common.cpp LINK_WITH pcl_gtest pcl_common) +PCL_ADD_TEST(common_parse test_parse FILES test_parse.cpp LINK_WITH pcl_gtest pcl_common) PCL_ADD_TEST(common_geometry test_geometry FILES test_geometry.cpp LINK_WITH pcl_gtest pcl_common) PCL_ADD_TEST(common_copy_point test_copy_point FILES test_copy_point.cpp LINK_WITH pcl_gtest pcl_common) PCL_ADD_TEST(common_transforms test_transforms FILES test_transforms.cpp LINK_WITH pcl_gtest pcl_common) diff --git a/test/common/test_bearing_angle_image.cpp b/test/common/test_bearing_angle_image.cpp index 9ae2fbc0..3fb1fb7a 100644 --- a/test/common/test_bearing_angle_image.cpp +++ b/test/common/test_bearing_angle_image.cpp @@ -40,7 +40,7 @@ * \author: Qinghua Li (qinghua__li@163.com) */ -#include +#include #include #include diff --git a/test/common/test_centroid.cpp b/test/common/test_centroid.cpp index 27b07be8..fa21ab7c 100644 --- a/test/common/test_centroid.cpp +++ b/test/common/test_centroid.cpp @@ -36,7 +36,7 @@ * */ -#include +#include #include #include #include diff --git a/test/common/test_colors.cpp b/test/common/test_colors.cpp index e21d89d2..d08ef9c7 100644 --- a/test/common/test_colors.cpp +++ b/test/common/test_colors.cpp @@ -33,7 +33,7 @@ * */ -#include +#include #include #include diff --git a/test/common/test_common.cpp b/test/common/test_common.cpp index 1b040744..8d9ff286 100644 --- a/test/common/test_common.cpp +++ b/test/common/test_common.cpp @@ -37,7 +37,7 @@ * */ -#include +#include #include #include #include @@ -308,7 +308,7 @@ TEST (PCL, PointTypes) template class XYZPointTypesTest : public ::testing::Test { }; using XYZPointTypes = ::testing::Types; -TYPED_TEST_CASE(XYZPointTypesTest, XYZPointTypes); +TYPED_TEST_SUITE(XYZPointTypesTest, XYZPointTypes); TYPED_TEST(XYZPointTypesTest, GetVectorXfMap) { TypeParam pt; @@ -329,7 +329,7 @@ TYPED_TEST(XYZPointTypesTest, GetArrayXfMap) template class NormalPointTypesTest : public ::testing::Test { }; using NormalPointTypes = ::testing::Types; -TYPED_TEST_CASE(NormalPointTypesTest, NormalPointTypes); +TYPED_TEST_SUITE(NormalPointTypesTest, NormalPointTypes); TYPED_TEST(NormalPointTypesTest, GetNormalVectorXfMap) { TypeParam pt; @@ -341,7 +341,7 @@ TYPED_TEST(NormalPointTypesTest, GetNormalVectorXfMap) template class RGBPointTypesTest : public ::testing::Test { }; using RGBPointTypes = ::testing::Types; -TYPED_TEST_CASE(RGBPointTypesTest, RGBPointTypes); +TYPED_TEST_SUITE(RGBPointTypesTest, RGBPointTypes); TYPED_TEST(RGBPointTypesTest, GetRGBVectorXi) { TypeParam pt; pt.r = 1; pt.g = 2; pt.b = 3; pt.a = 4; diff --git a/test/common/test_copy_make_borders.cpp b/test/common/test_copy_make_borders.cpp index 83c8dfde..2f8963f7 100644 --- a/test/common/test_copy_make_borders.cpp +++ b/test/common/test_copy_make_borders.cpp @@ -33,7 +33,7 @@ * */ -#include +#include #include #include #include diff --git a/test/common/test_copy_point.cpp b/test/common/test_copy_point.cpp index bdbee4f5..20cfcd89 100644 --- a/test/common/test_copy_point.cpp +++ b/test/common/test_copy_point.cpp @@ -33,7 +33,7 @@ * */ -#include +#include #include diff --git a/test/common/test_eigen.cpp b/test/common/test_eigen.cpp index d5eaf899..6d1f9403 100644 --- a/test/common/test_eigen.cpp +++ b/test/common/test_eigen.cpp @@ -37,7 +37,7 @@ * */ -#include +#include #include diff --git a/test/common/test_gaussian.cpp b/test/common/test_gaussian.cpp index 6b589b4f..2cb8ee2f 100644 --- a/test/common/test_gaussian.cpp +++ b/test/common/test_gaussian.cpp @@ -35,7 +35,7 @@ */ /** \author Nizar Sallem */ -#include +#include #include TEST(PCL, GaussianKernel) diff --git a/test/common/test_generator.cpp b/test/common/test_generator.cpp index e0147496..37103247 100644 --- a/test/common/test_generator.cpp +++ b/test/common/test_generator.cpp @@ -35,7 +35,7 @@ * */ -#include +#include #include #include #include diff --git a/test/common/test_geometry.cpp b/test/common/test_geometry.cpp index cc44ff6d..e2e10204 100644 --- a/test/common/test_geometry.cpp +++ b/test/common/test_geometry.cpp @@ -37,7 +37,7 @@ * */ -#include +#include #include #include @@ -46,7 +46,7 @@ using namespace pcl; ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template class XYZPointTypesTest : public ::testing::Test { }; using XYZPointTypes = ::testing::Types; -TYPED_TEST_CASE(XYZPointTypesTest, XYZPointTypes); +TYPED_TEST_SUITE(XYZPointTypesTest, XYZPointTypes); TYPED_TEST(XYZPointTypesTest, Distance) { diff --git a/test/common/test_intensity.cpp b/test/common/test_intensity.cpp index f43c86ce..74026aec 100644 --- a/test/common/test_intensity.cpp +++ b/test/common/test_intensity.cpp @@ -37,7 +37,7 @@ * */ -#include +#include #include #include #include diff --git a/test/common/test_io.cpp b/test/common/test_io.cpp index 9c118d4f..e42c78f8 100644 --- a/test/common/test_io.cpp +++ b/test/common/test_io.cpp @@ -35,7 +35,7 @@ * */ -#include +#include #include #include #include diff --git a/test/common/test_macros.cpp b/test/common/test_macros.cpp index 8e308381..7fc62e70 100644 --- a/test/common/test_macros.cpp +++ b/test/common/test_macros.cpp @@ -34,7 +34,7 @@ * $Id$ */ -#include +#include #include #include #include diff --git a/test/common/test_operators.cpp b/test/common/test_operators.cpp index fcca9a56..fdd43f79 100644 --- a/test/common/test_operators.cpp +++ b/test/common/test_operators.cpp @@ -35,7 +35,7 @@ * */ -#include +#include #include #include #include diff --git a/test/common/test_parse.cpp b/test/common/test_parse.cpp new file mode 100644 index 00000000..f21c2a89 --- /dev/null +++ b/test/common/test_parse.cpp @@ -0,0 +1,191 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2020-, 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 + +using namespace std; + +/////////////////////////////////////////////////////////////////////////////////////////// +TEST (PCL, parse_double) +{ + const char arg0[] = {"test_name"}; + const char arg1[] = {"double"}; + const char arg2_1[] = {"-3.14e+0"}; + const char arg2_2[] = {"-3.14f+0"}; + const char arg2_3[] = {"3.14e+309"}; + + const char* argv_1[] = { &arg0[0], &arg1[0], &arg2_1[0], nullptr}; + const char* argv_2[] = { &arg0[0], &arg1[0], &arg2_2[0], nullptr}; + const char* argv_3[] = { &arg0[0], &arg1[0], &arg2_3[0], nullptr}; + const int argc = static_cast (sizeof (argv_1)/sizeof (argv_1[0])) - 1; + + int index = -1; + double value = 0; + + index = pcl::console::parse_argument (argc, argv_1, "double", value); + EXPECT_DOUBLE_EQ(-3.14, value); + EXPECT_EQ(1, index); + + index = pcl::console::parse_argument (argc, argv_2, "double", value); + EXPECT_EQ(-1, index); + + index = pcl::console::parse_argument (argc, argv_3, "double", value); + EXPECT_EQ(-1, index); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +TEST (PCL, parse_float) +{ + const char arg0[] = {"test_name"}; + const char arg1[] = {"float"}; + const char arg2_1[] = {"-3.14e+0"}; + const char arg2_2[] = {"-3.14f+0"}; + const char arg2_3[] = {"3.14e+39"}; + + const char* argv_1[] = { &arg0[0], &arg1[0], &arg2_1[0], nullptr}; + const char* argv_2[] = { &arg0[0], &arg1[0], &arg2_2[0], nullptr}; + const char* argv_3[] = { &arg0[0], &arg1[0], &arg2_3[0], nullptr}; + const int argc = static_cast (sizeof (argv_1)/sizeof (argv_1[0])) - 1; + + int index = -1; + float value = 0; + + index = pcl::console::parse_argument (argc, argv_1, "float", value); + EXPECT_FLOAT_EQ(-3.14, value); + EXPECT_EQ(1, index); + + index = pcl::console::parse_argument (argc, argv_2, "float", value); + EXPECT_EQ(-1, index); + + index = pcl::console::parse_argument (argc, argv_3, "float", value); + EXPECT_EQ(-1, index); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +TEST (PCL, parse_longint) +{ + const char arg0[] = {"test_name"}; + const char arg1[] = {"long_int"}; + const char arg2_1[] = {"-314"}; + const char arg2_2[] = {"3.14"}; + const char arg2_3[] = {"18446744073709551615"}; + + const char* argv_1[] = { &arg0[0], &arg1[0], &arg2_1[0], nullptr}; + const char* argv_2[] = { &arg0[0], &arg1[0], &arg2_2[0], nullptr}; + const char* argv_3[] = { &arg0[0], &arg1[0], &arg2_3[0], nullptr}; + const int argc = static_cast (sizeof (argv_1)/sizeof (argv_1[0])) - 1; + + int index = -1; + long int value = 0; + + index = pcl::console::parse_argument (argc, argv_1, "long_int", value); + EXPECT_EQ(-314, value); + EXPECT_EQ(1, index); + + index = pcl::console::parse_argument (argc, argv_2, "long_int", value); + EXPECT_EQ(-1, index); + + index = pcl::console::parse_argument (argc, argv_3, "long_int", value); + EXPECT_EQ(-1, index); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +TEST (PCL, parse_unsignedint) +{ + const char arg0[] = {"test_name"}; + const char arg1[] = {"unsigned_int"}; + const char arg2_1[] = {"314"}; + const char arg2_2[] = {"-314"}; + const char arg2_3[] = {"18446744073709551615"}; + + const char* argv_1[] = { &arg0[0], &arg1[0], &arg2_1[0], nullptr}; + const char* argv_2[] = { &arg0[0], &arg1[0], &arg2_2[0], nullptr}; + const char* argv_3[] = { &arg0[0], &arg1[0], &arg2_3[0], nullptr}; + const int argc = static_cast (sizeof (argv_1)/sizeof (argv_1[0])) - 1; + + int index = -1; + unsigned int value = 53; + + index = pcl::console::parse_argument (argc, argv_1, "unsigned_int", value); + EXPECT_EQ(314, value); + EXPECT_EQ(1, index); + + index = pcl::console::parse_argument (argc, argv_2, "unsigned_int", value); + EXPECT_EQ(-1, index); + + index = pcl::console::parse_argument (argc, argv_3, "unsigned_int", value); + EXPECT_EQ(-1, index); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +TEST (PCL, parse_int) +{ + const char arg0[] = {"test_name"}; + const char arg1[] = {"int"}; + const char arg2_1[] = {"-314"}; + const char arg2_2[] = {"3.14"}; + const char arg2_3[] = {"18446744073709551615"}; + + const char* argv_1[] = { &arg0[0], &arg1[0], &arg2_1[0], nullptr}; + const char* argv_2[] = { &arg0[0], &arg1[0], &arg2_2[0], nullptr}; + const char* argv_3[] = { &arg0[0], &arg1[0], &arg2_3[0], nullptr}; + const int argc = static_cast (sizeof (argv_1)/sizeof (argv_1[0])) - 1; + + int index = -1; + int value = 0; + + index = pcl::console::parse_argument (argc, argv_1, "int", value); + EXPECT_EQ(-314, value); + EXPECT_EQ(1, index); + + index = pcl::console::parse_argument (argc, argv_2, "int", value); + EXPECT_EQ(-1, index); + + index = pcl::console::parse_argument (argc, argv_3, "int", value); + EXPECT_EQ(-1, index); +} + +/* ---[ */ +int +main (int argc, char** argv) +{ + testing::InitGoogleTest (&argc, argv); + return (RUN_ALL_TESTS ()); +} +/* ]--- */ diff --git a/test/common/test_pca.cpp b/test/common/test_pca.cpp index 76100a80..9c3a64e4 100644 --- a/test/common/test_pca.cpp +++ b/test/common/test_pca.cpp @@ -36,7 +36,7 @@ /** \author Nizar Sallem */ -#include +#include #include #include #include diff --git a/test/common/test_plane_intersection.cpp b/test/common/test_plane_intersection.cpp index 27f573a9..06db46ac 100644 --- a/test/common/test_plane_intersection.cpp +++ b/test/common/test_plane_intersection.cpp @@ -34,7 +34,7 @@ * $Id: test_plane_intersection.cpp 5686 2012-05-11 20:59:00Z gioia $ */ -#include +#include #include #include #include diff --git a/test/common/test_point_type_conversion.cpp b/test/common/test_point_type_conversion.cpp index c2ba6a42..0cb5dcaf 100644 --- a/test/common/test_point_type_conversion.cpp +++ b/test/common/test_point_type_conversion.cpp @@ -37,7 +37,7 @@ * */ -#include +#include #include #include #include diff --git a/test/common/test_polygon_mesh.cpp b/test/common/test_polygon_mesh.cpp index cdca6818..6c0d56e8 100644 --- a/test/common/test_polygon_mesh.cpp +++ b/test/common/test_polygon_mesh.cpp @@ -35,7 +35,7 @@ * */ -#include +#include #include #include @@ -131,4 +131,4 @@ main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); return (RUN_ALL_TESTS()); -} \ No newline at end of file +} diff --git a/test/common/test_spring.cpp b/test/common/test_spring.cpp index 992beb09..ffb1de66 100644 --- a/test/common/test_spring.cpp +++ b/test/common/test_spring.cpp @@ -34,7 +34,7 @@ * $Id$ */ -#include +#include #include #include #include diff --git a/test/common/test_transforms.cpp b/test/common/test_transforms.cpp index a4c7f9b9..b58e8fb9 100644 --- a/test/common/test_transforms.cpp +++ b/test/common/test_transforms.cpp @@ -37,7 +37,7 @@ * */ -#include +#include #include #include @@ -107,7 +107,7 @@ class Transforms : public ::testing::Test PCL_MAKE_ALIGNED_OPERATOR_NEW; }; -TYPED_TEST_CASE (Transforms, TransformTypes); +TYPED_TEST_SUITE (Transforms, TransformTypes); TYPED_TEST (Transforms, PointCloudXYZDense) { diff --git a/test/common/test_type_traits.cpp b/test/common/test_type_traits.cpp index 40f2240d..a2bace35 100644 --- a/test/common/test_type_traits.cpp +++ b/test/common/test_type_traits.cpp @@ -36,11 +36,10 @@ */ -#include #include #include -#include +#include TEST (TypeTraits, HasCustomAllocatorTrait) { diff --git a/test/common/test_vector_average.cpp b/test/common/test_vector_average.cpp index 7da2041c..988dfd41 100644 --- a/test/common/test_vector_average.cpp +++ b/test/common/test_vector_average.cpp @@ -38,7 +38,7 @@ #include #include #include -#include +#include #include using namespace pcl; diff --git a/test/common/test_wrappers.cpp b/test/common/test_wrappers.cpp index 94e2da1b..99fe1953 100644 --- a/test/common/test_wrappers.cpp +++ b/test/common/test_wrappers.cpp @@ -37,7 +37,7 @@ * */ -#include +#include #include #include #include diff --git a/test/features/test_base_feature.cpp b/test/features/test_base_feature.cpp index 655fff95..b8a37f7c 100644 --- a/test/features/test_base_feature.cpp +++ b/test/features/test_base_feature.cpp @@ -37,7 +37,7 @@ * */ -#include +#include #include #include #include diff --git a/test/features/test_board_estimation.cpp b/test/features/test_board_estimation.cpp index caccc4c8..d3864d50 100644 --- a/test/features/test_board_estimation.cpp +++ b/test/features/test_board_estimation.cpp @@ -37,7 +37,7 @@ * */ -#include +#include #include #include #include diff --git a/test/features/test_boundary_estimation.cpp b/test/features/test_boundary_estimation.cpp index 35ac26c4..407ebefc 100644 --- a/test/features/test_boundary_estimation.cpp +++ b/test/features/test_boundary_estimation.cpp @@ -37,7 +37,7 @@ * */ -#include +#include #include #include #include diff --git a/test/features/test_brisk.cpp b/test/features/test_brisk.cpp index b24caf13..f17a062e 100644 --- a/test/features/test_brisk.cpp +++ b/test/features/test_brisk.cpp @@ -36,7 +36,7 @@ */ -#include +#include #include #include #include diff --git a/test/features/test_cppf_estimation.cpp b/test/features/test_cppf_estimation.cpp index aa4edc70..69e4cfb1 100755 --- a/test/features/test_cppf_estimation.cpp +++ b/test/features/test_cppf_estimation.cpp @@ -37,7 +37,7 @@ * */ -#include +#include #include #include #include diff --git a/test/features/test_curvatures_estimation.cpp b/test/features/test_curvatures_estimation.cpp index 20bb4fed..d0a90778 100644 --- a/test/features/test_curvatures_estimation.cpp +++ b/test/features/test_curvatures_estimation.cpp @@ -37,7 +37,7 @@ * */ -#include +#include #include #include #include diff --git a/test/features/test_cvfh_estimation.cpp b/test/features/test_cvfh_estimation.cpp index 224ad0d2..57c08185 100644 --- a/test/features/test_cvfh_estimation.cpp +++ b/test/features/test_cvfh_estimation.cpp @@ -37,7 +37,7 @@ * */ -#include +#include #include #include #include diff --git a/test/features/test_flare_estimation.cpp b/test/features/test_flare_estimation.cpp index a389b4b2..bd85ee43 100644 --- a/test/features/test_flare_estimation.cpp +++ b/test/features/test_flare_estimation.cpp @@ -37,7 +37,7 @@ * */ -#include +#include #include #include #include diff --git a/test/features/test_gasd_estimation.cpp b/test/features/test_gasd_estimation.cpp index e8e3e251..b6f462f1 100644 --- a/test/features/test_gasd_estimation.cpp +++ b/test/features/test_gasd_estimation.cpp @@ -36,7 +36,7 @@ * */ -#include +#include #include #include #include diff --git a/test/features/test_gradient_estimation.cpp b/test/features/test_gradient_estimation.cpp index 30bb432f..26acde5e 100644 --- a/test/features/test_gradient_estimation.cpp +++ b/test/features/test_gradient_estimation.cpp @@ -37,7 +37,7 @@ * */ -#include +#include #include #include #include diff --git a/test/features/test_grsd_estimation.cpp b/test/features/test_grsd_estimation.cpp index 677a40aa..532f2000 100644 --- a/test/features/test_grsd_estimation.cpp +++ b/test/features/test_grsd_estimation.cpp @@ -38,7 +38,7 @@ * */ -#include +#include #include #include #include diff --git a/test/features/test_ii_normals.cpp b/test/features/test_ii_normals.cpp index 5677cce7..c5e5154d 100644 --- a/test/features/test_ii_normals.cpp +++ b/test/features/test_ii_normals.cpp @@ -35,7 +35,7 @@ * */ -#include +#include #include #include diff --git a/test/features/test_invariants_estimation.cpp b/test/features/test_invariants_estimation.cpp index 946cc880..f8d2d5cc 100644 --- a/test/features/test_invariants_estimation.cpp +++ b/test/features/test_invariants_estimation.cpp @@ -37,7 +37,7 @@ * */ -#include +#include #include #include #include diff --git a/test/features/test_moment_of_inertia_estimation.cpp b/test/features/test_moment_of_inertia_estimation.cpp index fd5515b8..804ac328 100644 --- a/test/features/test_moment_of_inertia_estimation.cpp +++ b/test/features/test_moment_of_inertia_estimation.cpp @@ -37,7 +37,7 @@ * */ -#include +#include #include #include #include diff --git a/test/features/test_narf.cpp b/test/features/test_narf.cpp index 5e83e41e..c1c2087f 100644 --- a/test/features/test_narf.cpp +++ b/test/features/test_narf.cpp @@ -36,7 +36,7 @@ #include #include -#include +#include #include #include diff --git a/test/features/test_normal_estimation.cpp b/test/features/test_normal_estimation.cpp index 104739af..ba1ea939 100644 --- a/test/features/test_normal_estimation.cpp +++ b/test/features/test_normal_estimation.cpp @@ -37,10 +37,11 @@ * */ -#include +#include #include #include #include +#include #include using namespace pcl; @@ -181,6 +182,66 @@ TEST (PCL, NormalEstimation) EXPECT_EQ (normals->points.size (), indices.size ()); } +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// There was an issue where the vectors for the indices and the +// distances of the neighbor search weren't initialized correctly +// due to a wrong usage of OpenMP. +// See PR #3614 for details. +// This tests if the vectors are initialized correctly. +template +class DummySearch : public pcl::search::Search +{ + public: + virtual int nearestKSearch (const PointT &point, int k, std::vector &k_indices, + std::vector &k_sqr_distances ) const + { + (void)point; + + EXPECT_GE (k_indices.size(), k); + EXPECT_GE (k_sqr_distances.size(), k); + + return k; + } + + virtual int radiusSearch (const PointT& point, double radius, std::vector& k_indices, + std::vector& k_sqr_distances, unsigned int max_nn = 0 ) const + { + (void)point; + (void)radius; + (void)k_indices; + (void)k_sqr_distances; + + return max_nn; + } +}; + + +TEST (PCL, NormalEstimationOpenMPInitVectors) +{ + NormalEstimationOMP n (4); // instantiate 4 threads + + DummySearch::Ptr dummy (new DummySearch); + + // Object + PointCloud::Ptr normals (new PointCloud ()); + + // set parameters + PointCloud::Ptr cloudptr = cloud.makeShared (); + n.setInputCloud (cloudptr); + EXPECT_EQ (n.getInputCloud (), cloudptr); + pcl::IndicesPtr indicesptr (new pcl::Indices (indices)); + n.setIndices (indicesptr); + EXPECT_EQ (n.getIndices (), indicesptr); + n.setSearchMethod (dummy); + EXPECT_EQ (n.getSearchMethod (), dummy); + n.setKSearch (static_cast (indices.size ())); + + // This will cause a call to DummySearch::nearestKSearch + // which checks if the vectors for the indices and the + // distances are correctly set up. See PR #3614. + n.compute (*normals); +} + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// TEST (PCL, NormalEstimationOpenMP) { @@ -213,6 +274,83 @@ TEST (PCL, NormalEstimationOpenMP) } } +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// This tests the indexing issue from #3573 +// In certain cases when you used a subset of the indices +// and set the depth dependent smoothing to false the +// IntegralImageNormalEstimation could crash or produce +// incorrect normals. +// This test reproduces the issue. +TEST (PCL, IntegralImageNormalEstimationIndexingIssue) +{ + PointCloud::Ptr cloudptr(new PointCloud()); + + cloudptr->width = 100; + cloudptr->height = 100; + cloudptr->points.clear(); + cloudptr->points.resize(cloudptr->width * cloudptr->height); + + int centerX = cloudptr->width >> 1; + int centerY = cloudptr->height >> 1; + + int idx = 0; + for (int ypos = -centerY; ypos < centerY; ypos++) + { + for (int xpos = -centerX; xpos < centerX; xpos++) + { + double z = xpos < 0.0 ? 1.0 : 0.0; + double y = ypos; + double x = xpos; + + cloudptr->points[idx++] = PointXYZ(float(x), float(y), float(z)); + } + } + + pcl::IndicesPtr indicesptr (new pcl::Indices ()); + indicesptr->resize(cloudptr->size() / 2); + for (int i = 0; i < cloudptr->size() / 2; ++i) + { + (*indicesptr)[i] = i + cloudptr->size() / 2; + } + + IntegralImageNormalEstimation n; + + // Object + PointCloud::Ptr normals (new PointCloud ()); + + // set parameters + n.setInputCloud (cloudptr); + n.setIndices (indicesptr); + n.setDepthDependentSmoothing (false); + + // estimate + n.compute (*normals); + + std::vector normalsVec; + normalsVec.resize(normals->size()); + for( int i = 0; i < normals->size(); ++i ) + { + normalsVec[i].x = normals->points[i].normal_x; + normalsVec[i].y = normals->points[i].normal_y; + normalsVec[i].z = normals->points[i].normal_z; + } + + for (const auto &point : normals->points) + { + if (std::isnan( point.normal_x ) || + std::isnan( point.normal_y ) || + std::isnan( point.normal_z )) + { + continue; + } + + EXPECT_NEAR (point.normal[0], 0.0, 1e-4); + EXPECT_NEAR (point.normal[1], 0.0, 1e-4); + EXPECT_NEAR (point.normal[2], -1.0, 1e-4); + EXPECT_TRUE (std::isnan(point.curvature)); + } +} + /* ---[ */ int main (int argc, char** argv) diff --git a/test/features/test_pfh_estimation.cpp b/test/features/test_pfh_estimation.cpp index f0ce72f4..973f5814 100644 --- a/test/features/test_pfh_estimation.cpp +++ b/test/features/test_pfh_estimation.cpp @@ -42,7 +42,7 @@ #define PCL_NO_PRECOMPILE #endif -#include +#include #include #include #include @@ -294,7 +294,7 @@ struct FPFHTest > using FPFHEstimatorTypes = ::testing::Types , FPFHEstimationOMP >; -TYPED_TEST_CASE (FPFHTest, FPFHEstimatorTypes); +TYPED_TEST_SUITE (FPFHTest, FPFHEstimatorTypes); // This is a copy of the old FPFHEstimation test which will now // be applied to both FPFHEstimation and FPFHEstimationOMP diff --git a/test/features/test_ppf_estimation.cpp b/test/features/test_ppf_estimation.cpp index 0ef01b05..2b5c43d9 100644 --- a/test/features/test_ppf_estimation.cpp +++ b/test/features/test_ppf_estimation.cpp @@ -37,7 +37,7 @@ * */ -#include +#include #include #include #include diff --git a/test/features/test_ptr.cpp b/test/features/test_ptr.cpp index 9780a224..1dcc13b9 100644 --- a/test/features/test_ptr.cpp +++ b/test/features/test_ptr.cpp @@ -37,7 +37,7 @@ * */ -#include +#include #include #include #include diff --git a/test/features/test_rift_estimation.cpp b/test/features/test_rift_estimation.cpp index 8f54b6d8..248d1241 100644 --- a/test/features/test_rift_estimation.cpp +++ b/test/features/test_rift_estimation.cpp @@ -37,7 +37,7 @@ * */ -#include +#include #include #include diff --git a/test/features/test_rops_estimation.cpp b/test/features/test_rops_estimation.cpp index 9b1c81bc..87bf0b9a 100644 --- a/test/features/test_rops_estimation.cpp +++ b/test/features/test_rops_estimation.cpp @@ -37,7 +37,7 @@ * */ -#include +#include #include #include #include diff --git a/test/features/test_rsd_estimation.cpp b/test/features/test_rsd_estimation.cpp index 60e1d605..b74d4a9e 100644 --- a/test/features/test_rsd_estimation.cpp +++ b/test/features/test_rsd_estimation.cpp @@ -38,7 +38,7 @@ * */ -#include +#include #include #include #include diff --git a/test/features/test_shot_estimation.cpp b/test/features/test_shot_estimation.cpp index f49ca2c4..c23d665b 100644 --- a/test/features/test_shot_estimation.cpp +++ b/test/features/test_shot_estimation.cpp @@ -37,7 +37,7 @@ * */ -#include +#include #include #include #include @@ -371,7 +371,7 @@ struct SHOTShapeTest > using SHOTEstimatorTypes = ::testing::Types , SHOTEstimationOMP >; -TYPED_TEST_CASE (SHOTShapeTest, SHOTEstimatorTypes); +TYPED_TEST_SUITE (SHOTShapeTest, SHOTEstimatorTypes); // This is a copy of the old SHOTShapeEstimation test which will now // be applied to both SHOTEstimation and SHOTEstimationOMP @@ -560,7 +560,7 @@ struct SHOTShapeAndColorTest, SHOTColorEstimationOMP >; -TYPED_TEST_CASE (SHOTShapeAndColorTest, SHOTColorEstimatorTypes); +TYPED_TEST_SUITE (SHOTShapeAndColorTest, SHOTColorEstimatorTypes); // This is a copy of the old SHOTShapeAndColorEstimation test which will now // be applied to both SHOTColorEstimation and SHOTColorEstimationOMP diff --git a/test/features/test_shot_lrf_estimation.cpp b/test/features/test_shot_lrf_estimation.cpp index d8f25375..27c82383 100644 --- a/test/features/test_shot_lrf_estimation.cpp +++ b/test/features/test_shot_lrf_estimation.cpp @@ -37,7 +37,7 @@ * */ -#include +#include #include #include #include diff --git a/test/features/test_spin_estimation.cpp b/test/features/test_spin_estimation.cpp index b659b76b..e95ede69 100644 --- a/test/features/test_spin_estimation.cpp +++ b/test/features/test_spin_estimation.cpp @@ -37,7 +37,7 @@ * */ -#include +#include #include #include #include diff --git a/test/filters/test_bilateral.cpp b/test/filters/test_bilateral.cpp index ababfc3e..a9438917 100644 --- a/test/filters/test_bilateral.cpp +++ b/test/filters/test_bilateral.cpp @@ -38,7 +38,7 @@ * */ -#include +#include #include #include #include diff --git a/test/filters/test_clipper.cpp b/test/filters/test_clipper.cpp index 07bc1d82..98485104 100644 --- a/test/filters/test_clipper.cpp +++ b/test/filters/test_clipper.cpp @@ -38,7 +38,7 @@ * */ -#include +#include #include #include #include @@ -776,4 +776,4 @@ main (int argc, char** argv) testing::InitGoogleTest (&argc, argv); return (RUN_ALL_TESTS ()); } -/* ]--- */ \ No newline at end of file +/* ]--- */ diff --git a/test/filters/test_convolution.cpp b/test/filters/test_convolution.cpp index 89a20940..53a6cbf1 100644 --- a/test/filters/test_convolution.cpp +++ b/test/filters/test_convolution.cpp @@ -37,7 +37,7 @@ * */ -#include +#include #include #include #include diff --git a/test/filters/test_filters.cpp b/test/filters/test_filters.cpp index 7f3fe71d..d2bb3a62 100644 --- a/test/filters/test_filters.cpp +++ b/test/filters/test_filters.cpp @@ -38,7 +38,7 @@ * */ -#include +#include #include #include #include diff --git a/test/filters/test_grid_minimum.cpp b/test/filters/test_grid_minimum.cpp index adbfb58a..07685620 100644 --- a/test/filters/test_grid_minimum.cpp +++ b/test/filters/test_grid_minimum.cpp @@ -39,7 +39,7 @@ * */ -#include +#include #include #include diff --git a/test/filters/test_local_maximum.cpp b/test/filters/test_local_maximum.cpp index b2b710bd..dc69cca8 100644 --- a/test/filters/test_local_maximum.cpp +++ b/test/filters/test_local_maximum.cpp @@ -39,7 +39,7 @@ * */ -#include +#include #include #include diff --git a/test/filters/test_model_outlier_removal.cpp b/test/filters/test_model_outlier_removal.cpp index 6ed21098..24d5da03 100644 --- a/test/filters/test_model_outlier_removal.cpp +++ b/test/filters/test_model_outlier_removal.cpp @@ -35,7 +35,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include +#include #include #include diff --git a/test/filters/test_morphological.cpp b/test/filters/test_morphological.cpp index ee56eca2..de96baa7 100644 --- a/test/filters/test_morphological.cpp +++ b/test/filters/test_morphological.cpp @@ -39,7 +39,7 @@ * */ -#include +#include #include #include diff --git a/test/filters/test_sampling.cpp b/test/filters/test_sampling.cpp index 35a13a28..04dacaad 100644 --- a/test/filters/test_sampling.cpp +++ b/test/filters/test_sampling.cpp @@ -38,7 +38,7 @@ * */ -#include +#include #include #include #include diff --git a/test/filters/test_uniform_sampling.cpp b/test/filters/test_uniform_sampling.cpp index 9cd001a2..f09a95c3 100644 --- a/test/filters/test_uniform_sampling.cpp +++ b/test/filters/test_uniform_sampling.cpp @@ -34,7 +34,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include +#include #include #include diff --git a/test/geometry/test_iterator.cpp b/test/geometry/test_iterator.cpp index 8e193d68..58e1f24f 100644 --- a/test/geometry/test_iterator.cpp +++ b/test/geometry/test_iterator.cpp @@ -33,7 +33,7 @@ * */ -#include +#include #include #include #include diff --git a/test/geometry/test_mesh.cpp b/test/geometry/test_mesh.cpp index 520de0f2..0b6df7d9 100644 --- a/test/geometry/test_mesh.cpp +++ b/test/geometry/test_mesh.cpp @@ -40,7 +40,7 @@ #include -#include +#include // Needed for one test. Must be defined before including the mesh. #define PCL_GEOMETRY_MESH_BASE_TEST_DELETE_FACE_MANIFOLD_2 diff --git a/test/geometry/test_mesh_circulators.cpp b/test/geometry/test_mesh_circulators.cpp index 4cac9bca..5c4ba540 100644 --- a/test/geometry/test_mesh_circulators.cpp +++ b/test/geometry/test_mesh_circulators.cpp @@ -38,7 +38,7 @@ * */ -#include +#include #include #include diff --git a/test/geometry/test_mesh_conversion.cpp b/test/geometry/test_mesh_conversion.cpp index 4a04db62..1e95033a 100644 --- a/test/geometry/test_mesh_conversion.cpp +++ b/test/geometry/test_mesh_conversion.cpp @@ -38,7 +38,7 @@ * */ -#include +#include #include #include @@ -152,7 +152,7 @@ using NonManifoldMeshTraits = MeshTraits; using MeshTraitsTypes = testing::Types ; -TYPED_TEST_CASE (TestMeshConversion, MeshTraitsTypes); +TYPED_TEST_SUITE (TestMeshConversion, MeshTraitsTypes); //////////////////////////////////////////////////////////////////////////////// diff --git a/test/geometry/test_mesh_data.cpp b/test/geometry/test_mesh_data.cpp index deb3e971..6bd229ee 100644 --- a/test/geometry/test_mesh_data.cpp +++ b/test/geometry/test_mesh_data.cpp @@ -38,7 +38,7 @@ * */ -#include +#include #include "test_mesh_common_functions.h" #include diff --git a/test/geometry/test_mesh_get_boundary.cpp b/test/geometry/test_mesh_get_boundary.cpp index 6fc82c7f..17fab967 100644 --- a/test/geometry/test_mesh_get_boundary.cpp +++ b/test/geometry/test_mesh_get_boundary.cpp @@ -38,7 +38,7 @@ * */ -#include +#include #include #include @@ -68,7 +68,7 @@ class TestGetBoundary : public testing::Test using Mesh = MeshT; }; -TYPED_TEST_CASE (TestGetBoundary, MeshTypes); +TYPED_TEST_SUITE (TestGetBoundary, MeshTypes); //////////////////////////////////////////////////////////////////////////////// diff --git a/test/geometry/test_mesh_indices.cpp b/test/geometry/test_mesh_indices.cpp index 39e098cd..e864519a 100644 --- a/test/geometry/test_mesh_indices.cpp +++ b/test/geometry/test_mesh_indices.cpp @@ -41,7 +41,7 @@ #include #include -#include +#include #include @@ -63,7 +63,7 @@ class TestMeshIndicesTyped : public testing::Test using MeshIndexTypes = testing::Types ; -TYPED_TEST_CASE (TestMeshIndicesTyped, MeshIndexTypes); +TYPED_TEST_SUITE (TestMeshIndicesTyped, MeshIndexTypes); //////////////////////////////////////////////////////////////////////////////// diff --git a/test/geometry/test_mesh_io.cpp b/test/geometry/test_mesh_io.cpp index d4161a3a..142457b3 100644 --- a/test/geometry/test_mesh_io.cpp +++ b/test/geometry/test_mesh_io.cpp @@ -42,7 +42,7 @@ #include #include -#include +#include #include #include diff --git a/test/geometry/test_polygon_mesh.cpp b/test/geometry/test_polygon_mesh.cpp index 160b45e1..c5ea8bd4 100644 --- a/test/geometry/test_polygon_mesh.cpp +++ b/test/geometry/test_polygon_mesh.cpp @@ -41,7 +41,7 @@ #include #include -#include +#include #include @@ -82,7 +82,7 @@ class TestPolygonMesh : public testing::Test using Mesh = MeshT; }; -TYPED_TEST_CASE (TestPolygonMesh, PolygonMeshTypes); +TYPED_TEST_SUITE (TestPolygonMesh, PolygonMeshTypes); //////////////////////////////////////////////////////////////////////////////// diff --git a/test/geometry/test_quad_mesh.cpp b/test/geometry/test_quad_mesh.cpp index 09be99a3..794674ab 100644 --- a/test/geometry/test_quad_mesh.cpp +++ b/test/geometry/test_quad_mesh.cpp @@ -41,7 +41,7 @@ #include #include -#include +#include #include @@ -80,7 +80,7 @@ class TestQuadMesh : public testing::Test using Mesh = MeshT; }; -TYPED_TEST_CASE (TestQuadMesh, QuadMeshTypes); +TYPED_TEST_SUITE (TestQuadMesh, QuadMeshTypes); //////////////////////////////////////////////////////////////////////////////// diff --git a/test/geometry/test_triangle_mesh.cpp b/test/geometry/test_triangle_mesh.cpp index cb1b6a7b..af1b25dc 100644 --- a/test/geometry/test_triangle_mesh.cpp +++ b/test/geometry/test_triangle_mesh.cpp @@ -41,7 +41,7 @@ #include #include -#include +#include #include @@ -80,7 +80,7 @@ class TestTriangleMesh : public testing::Test using Mesh = MeshT; }; -TYPED_TEST_CASE (TestTriangleMesh, TriangleMeshTypes); +TYPED_TEST_SUITE (TestTriangleMesh, TriangleMeshTypes); //////////////////////////////////////////////////////////////////////////////// diff --git a/test/include/pcl/test/gtest.h b/test/include/pcl/test/gtest.h new file mode 100644 index 00000000..594b08a4 --- /dev/null +++ b/test/include/pcl/test/gtest.h @@ -0,0 +1,65 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2019-, 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. + */ + +#pragma once + +#include + +/** + * \file pcl/test/gtest.h + * + * \brief Defines all the PCL test macros used + * \ingroup test + */ + +/** + * \brief Macro choose between TYPED_TEST_CASE and TYPED_TEST_SUITE depending on the GTest version + * + * \ingroup test + */ +#if !defined(TYPED_TEST_SUITE) + #define TYPED_TEST_SUITE TYPED_TEST_CASE +#endif + +/** + * \brief Macro choose between INSTANTIATE_TEST_CASE_P and INSTANTIATE_TEST_SUITE_P depending on the GTest version + * + * \ingroup test + */ +#if !defined(INSTANTIATE_TEST_SUITE_P) + #define INSTANTIATE_TEST_SUITE_P INSTANTIATE_TEST_CASE_P +#endif + diff --git a/test/io/test_buffers.cpp b/test/io/test_buffers.cpp index f9eb1fc0..ed2b27b0 100644 --- a/test/io/test_buffers.cpp +++ b/test/io/test_buffers.cpp @@ -35,7 +35,7 @@ * */ -#include +#include #include #include @@ -84,7 +84,7 @@ class BuffersTest : public ::testing::Test }; using DataTypes = ::testing::Types; -TYPED_TEST_CASE (BuffersTest, DataTypes); +TYPED_TEST_SUITE (BuffersTest, DataTypes); TYPED_TEST (BuffersTest, SingleBuffer) { diff --git a/test/io/test_grabbers.cpp b/test/io/test_grabbers.cpp index 80854c99..b280a018 100644 --- a/test/io/test_grabbers.cpp +++ b/test/io/test_grabbers.cpp @@ -1,4 +1,4 @@ -#include +#include #include #include #include diff --git a/test/io/test_io.cpp b/test/io/test_io.cpp index 8ff839a1..dca0a776 100644 --- a/test/io/test_io.cpp +++ b/test/io/test_io.cpp @@ -37,7 +37,7 @@ * */ -#include +#include #include #include #include @@ -1319,7 +1319,7 @@ TEST (PCL, Locale) ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template class AutoIOTest : public testing::Test { }; using PCLXyzNormalPointTypes = ::testing::Types; -TYPED_TEST_CASE (AutoIOTest, PCLXyzNormalPointTypes); +TYPED_TEST_SUITE (AutoIOTest, PCLXyzNormalPointTypes); TYPED_TEST (AutoIOTest, AutoLoadCloudFiles) { PointCloud cloud; diff --git a/test/io/test_iterators.cpp b/test/io/test_iterators.cpp index 2612d12c..85d73cce 100644 --- a/test/io/test_iterators.cpp +++ b/test/io/test_iterators.cpp @@ -34,7 +34,7 @@ * */ -#include +#include #include diff --git a/test/io/test_octree_compression.cpp b/test/io/test_octree_compression.cpp index 8b97fcab..fcbc8782 100644 --- a/test/io/test_octree_compression.cpp +++ b/test/io/test_octree_compression.cpp @@ -33,7 +33,7 @@ * POSSIBILITY OF SUCH DAMAGE. * */ -#include +#include #include #include #include diff --git a/test/io/test_ply_io.cpp b/test/io/test_ply_io.cpp index 139e848d..ecee8d11 100644 --- a/test/io/test_ply_io.cpp +++ b/test/io/test_ply_io.cpp @@ -40,7 +40,7 @@ #include #include #include -#include +#include ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// TEST (PCL, PLYReaderWriter) @@ -288,7 +288,7 @@ TEST_F (PLYColorTest, LoadPLYFileColoredASCIIIntoPolygonMesh) ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template class PLYPointCloudTest : public PLYColorTest { }; using RGBPointTypes = ::testing::Types; -TYPED_TEST_CASE (PLYPointCloudTest, RGBPointTypes); +TYPED_TEST_SUITE (PLYPointCloudTest, RGBPointTypes); TYPED_TEST (PLYPointCloudTest, LoadPLYFileColoredASCIIIntoPointCloud) { int res; @@ -316,7 +316,7 @@ template struct PLYCoordinatesIsDenseTest : public PLYTest {}; using XYZPointTypes = ::testing::Types; -TYPED_TEST_CASE (PLYCoordinatesIsDenseTest, XYZPointTypes); +TYPED_TEST_SUITE (PLYCoordinatesIsDenseTest, XYZPointTypes); TYPED_TEST (PLYCoordinatesIsDenseTest, NaNInCoordinates) { @@ -410,7 +410,7 @@ template struct PLYNormalsIsDenseTest : public PLYTest {}; using NormalPointTypes = ::testing::Types; -TYPED_TEST_CASE (PLYNormalsIsDenseTest, NormalPointTypes); +TYPED_TEST_SUITE (PLYNormalsIsDenseTest, NormalPointTypes); TYPED_TEST (PLYNormalsIsDenseTest, NaNInNormals) { diff --git a/test/io/test_ply_mesh_io.cpp b/test/io/test_ply_mesh_io.cpp index 5b4d8aae..360976d6 100644 --- a/test/io/test_ply_mesh_io.cpp +++ b/test/io/test_ply_mesh_io.cpp @@ -38,7 +38,7 @@ * */ -#include +#include #include #include #include diff --git a/test/io/test_point_cloud_image_extractors.cpp b/test/io/test_point_cloud_image_extractors.cpp index 9e0d0f2b..57bb8768 100644 --- a/test/io/test_point_cloud_image_extractors.cpp +++ b/test/io/test_point_cloud_image_extractors.cpp @@ -39,7 +39,7 @@ #include -#include +#include #include #include diff --git a/test/io/test_range_coder.cpp b/test/io/test_range_coder.cpp index 9f07006b..c17fd738 100644 --- a/test/io/test_range_coder.cpp +++ b/test/io/test_range_coder.cpp @@ -42,7 +42,7 @@ #include #include -#include +#include #include #include @@ -96,74 +96,74 @@ TEST (PCL, Adaptive_Range_Coder_Test) ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// TEST (PCL, Static_Range_Coder_Test) { - std::stringstream sstream; - std::vector inputCharData; - std::vector outputCharData; + // Run test for different vector sizes + for (unsigned int vectorSize: { 253, 10000 }) + { + std::stringstream sstream; + std::vector inputCharData; + std::vector outputCharData; - std::vector inputIntData; - std::vector outputIntData; + std::vector inputIntData; + std::vector outputIntData; - unsigned long writeByteLen; - unsigned long readByteLen; + unsigned long writeByteLen; + unsigned long readByteLen; - // vector size - const unsigned int vectorSize = 10000; + inputCharData.resize(vectorSize); + outputCharData.resize(vectorSize); - inputCharData.resize(vectorSize); - outputCharData.resize(vectorSize); + inputIntData.resize(vectorSize); + outputIntData.resize(vectorSize); - inputIntData.resize(vectorSize); - outputIntData.resize(vectorSize); + // fill vectors with random data + for (std::size_t i=0; i (rand () & 0xFF); + inputIntData[i] = static_cast (rand () & 0xFFFF); + } - // fill vectors with random data - for (std::size_t i=0; i (rand () & 0xFF); - inputIntData[i] = static_cast (rand () & 0xFFFF); - } + // initialize static range coder + pcl::StaticRangeCoder rangeCoder; - // initialize static range coder - pcl::StaticRangeCoder rangeCoder; + // encode char vector to stringstream + writeByteLen = rangeCoder.encodeCharVectorToStream(inputCharData, sstream); - // encode char vector to stringstream - writeByteLen = rangeCoder.encodeCharVectorToStream(inputCharData, sstream); + // decode stringstream to char vector + readByteLen = rangeCoder.decodeStreamToCharVector(sstream, outputCharData); - // decode stringstream to char vector - readByteLen = rangeCoder.decodeStreamToCharVector(sstream, outputCharData); + // compare amount of bytes that are read and written to/from stream + EXPECT_EQ (writeByteLen, readByteLen); + EXPECT_EQ (writeByteLen, sstream.str().length()); - // compare amount of bytes that are read and written to/from stream - EXPECT_EQ (writeByteLen, readByteLen); - EXPECT_EQ (writeByteLen, sstream.str().length()); - - // compare input and output vector - should be identical - EXPECT_EQ (inputCharData.size(), outputCharData.size()); - EXPECT_EQ (inputCharData.size(), vectorSize); + // compare input and output vector - should be identical + EXPECT_EQ (inputCharData.size(), outputCharData.size()); + EXPECT_EQ (inputCharData.size(), vectorSize); - for (std::size_t i=0; i +#include #include // For debug #include #include diff --git a/test/keypoints/test_iss_3d.cpp b/test/keypoints/test_iss_3d.cpp index 734226c9..11d54f68 100644 --- a/test/keypoints/test_iss_3d.cpp +++ b/test/keypoints/test_iss_3d.cpp @@ -34,7 +34,7 @@ /** \author Gioia Ballin */ -#include +#include #include #include diff --git a/test/keypoints/test_keypoints.cpp b/test/keypoints/test_keypoints.cpp index 53247929..59551a1a 100644 --- a/test/keypoints/test_keypoints.cpp +++ b/test/keypoints/test_keypoints.cpp @@ -35,7 +35,7 @@ /** \author Michael Dixon */ -#include +#include #include #include diff --git a/test/octree/test_octree.cpp b/test/octree/test_octree.cpp index 11310797..6d7b1005 100644 --- a/test/octree/test_octree.cpp +++ b/test/octree/test_octree.cpp @@ -36,7 +36,7 @@ * * */ -#include +#include #include diff --git a/test/octree/test_octree_iterator.cpp b/test/octree/test_octree_iterator.cpp index 6eb6939e..5493fb27 100644 --- a/test/octree/test_octree_iterator.cpp +++ b/test/octree/test_octree_iterator.cpp @@ -40,7 +40,7 @@ #include #include #include -#include +#include using pcl::octree::OctreeBase; using pcl::octree::OctreeIteratorBase; @@ -154,7 +154,7 @@ using OctreeIteratorTypes = testing::Types OctreeLeafNodeDepthFirstIterator >, OctreeFixedDepthIterator >, OctreeLeafNodeBreadthFirstIterator > >; -TYPED_TEST_CASE (OctreeIteratorTest, OctreeIteratorTypes); +TYPED_TEST_SUITE (OctreeIteratorTest, OctreeIteratorTypes); TYPED_TEST (OctreeIteratorTest, CopyConstructor) { diff --git a/test/outofcore/test_outofcore.cpp b/test/outofcore/test_outofcore.cpp index 602ac88c..35893211 100644 --- a/test/outofcore/test_outofcore.cpp +++ b/test/outofcore/test_outofcore.cpp @@ -42,7 +42,7 @@ * Stephen Fox (foxstephend@gmail.com) */ -#include +#include #include #include diff --git a/test/people/test_people_groundBasedPeopleDetectionApp.cpp b/test/people/test_people_groundBasedPeopleDetectionApp.cpp index fe68de4b..8df64db7 100644 --- a/test/people/test_people_groundBasedPeopleDetectionApp.cpp +++ b/test/people/test_people_groundBasedPeopleDetectionApp.cpp @@ -45,7 +45,7 @@ * In Proceedings of the International Conference on Intelligent Robots and Systems (IROS) 2012, Vilamoura (Portugal), 2012. */ -#include +#include #include #include #include diff --git a/test/recognition/test_recognition_cg.cpp b/test/recognition/test_recognition_cg.cpp index a3adce59..e997f3f4 100644 --- a/test/recognition/test_recognition_cg.cpp +++ b/test/recognition/test_recognition_cg.cpp @@ -36,7 +36,7 @@ * $Id: $ * */ -#include +#include #include #include #include diff --git a/test/recognition/test_recognition_ism.cpp b/test/recognition/test_recognition_ism.cpp index 258ea83f..5006dde7 100644 --- a/test/recognition/test_recognition_ism.cpp +++ b/test/recognition/test_recognition_ism.cpp @@ -37,7 +37,7 @@ * */ -#include +#include #include #include #include diff --git a/test/registration/test_correspondence_estimation.cpp b/test/registration/test_correspondence_estimation.cpp index 7081fc0b..2192f3d7 100644 --- a/test/registration/test_correspondence_estimation.cpp +++ b/test/registration/test_correspondence_estimation.cpp @@ -35,7 +35,7 @@ * */ -#include +#include #include #include #include diff --git a/test/registration/test_correspondence_rejectors.cpp b/test/registration/test_correspondence_rejectors.cpp index ff9e8ec9..4e7ad317 100644 --- a/test/registration/test_correspondence_rejectors.cpp +++ b/test/registration/test_correspondence_rejectors.cpp @@ -39,7 +39,7 @@ #include -#include +#include #include #include #include diff --git a/test/registration/test_fpcs_ia.cpp b/test/registration/test_fpcs_ia.cpp index 26275b18..07842f98 100644 --- a/test/registration/test_fpcs_ia.cpp +++ b/test/registration/test_fpcs_ia.cpp @@ -35,7 +35,7 @@ * */ -#include +#include #include #include diff --git a/test/registration/test_kfpcs_ia.cpp b/test/registration/test_kfpcs_ia.cpp index e241b487..fa264ea7 100644 --- a/test/registration/test_kfpcs_ia.cpp +++ b/test/registration/test_kfpcs_ia.cpp @@ -35,7 +35,7 @@ * */ -#include +#include #include #include diff --git a/test/registration/test_ndt.cpp b/test/registration/test_ndt.cpp index 82ab4974..b34246e6 100644 --- a/test/registration/test_ndt.cpp +++ b/test/registration/test_ndt.cpp @@ -38,7 +38,7 @@ * */ -#include +#include #include #include diff --git a/test/registration/test_registration.cpp b/test/registration/test_registration.cpp index 36b1819f..a8cd9eb2 100644 --- a/test/registration/test_registration.cpp +++ b/test/registration/test_registration.cpp @@ -37,7 +37,7 @@ * */ -#include +#include #include #include diff --git a/test/registration/test_registration_api.cpp b/test/registration/test_registration_api.cpp index e48bdf91..8818cfc2 100644 --- a/test/registration/test_registration_api.cpp +++ b/test/registration/test_registration_api.cpp @@ -37,7 +37,7 @@ * */ -#include +#include #include diff --git a/test/registration/test_sac_ia.cpp b/test/registration/test_sac_ia.cpp index eb40dca5..23629b7f 100644 --- a/test/registration/test_sac_ia.cpp +++ b/test/registration/test_sac_ia.cpp @@ -38,7 +38,7 @@ * */ -#include +#include #include #include diff --git a/test/registration/test_warps.cpp b/test/registration/test_warps.cpp index be4b64b7..fde72dc2 100644 --- a/test/registration/test_warps.cpp +++ b/test/registration/test_warps.cpp @@ -35,7 +35,7 @@ * */ -#include +#include #include #include diff --git a/test/sample_consensus/test_sample_consensus.cpp b/test/sample_consensus/test_sample_consensus.cpp index 438f7050..3d32408c 100644 --- a/test/sample_consensus/test_sample_consensus.cpp +++ b/test/sample_consensus/test_sample_consensus.cpp @@ -36,7 +36,7 @@ * */ -#include +#include #include #include @@ -92,7 +92,7 @@ using sacTypes = ::testing::Types< RandomizedMEstimatorSampleConsensus, MaximumLikelihoodSampleConsensus >; -TYPED_TEST_CASE(SacTest, sacTypes); +TYPED_TEST_SUITE(SacTest, sacTypes); TYPED_TEST(SacTest, InfiniteLoop) { diff --git a/test/sample_consensus/test_sample_consensus_line_models.cpp b/test/sample_consensus/test_sample_consensus_line_models.cpp index 941a1a25..2af4ff3e 100644 --- a/test/sample_consensus/test_sample_consensus_line_models.cpp +++ b/test/sample_consensus/test_sample_consensus_line_models.cpp @@ -36,7 +36,7 @@ * */ -#include +#include #include diff --git a/test/sample_consensus/test_sample_consensus_plane_models.cpp b/test/sample_consensus/test_sample_consensus_plane_models.cpp index 429d41d6..38101503 100644 --- a/test/sample_consensus/test_sample_consensus_plane_models.cpp +++ b/test/sample_consensus/test_sample_consensus_plane_models.cpp @@ -36,7 +36,7 @@ * */ -#include +#include #include #include diff --git a/test/sample_consensus/test_sample_consensus_quadric_models.cpp b/test/sample_consensus/test_sample_consensus_quadric_models.cpp index 8751c9a9..e3ff89b6 100644 --- a/test/sample_consensus/test_sample_consensus_quadric_models.cpp +++ b/test/sample_consensus/test_sample_consensus_quadric_models.cpp @@ -36,7 +36,7 @@ * */ -#include +#include #include #include diff --git a/test/search/test_auto_search.cpp b/test/search/test_auto_search.cpp index 49722ebb..66087a74 100644 --- a/test/search/test_auto_search.cpp +++ b/test/search/test_auto_search.cpp @@ -37,7 +37,7 @@ * */ #include -#include +#include #include #include #include diff --git a/test/search/test_flann_search.cpp b/test/search/test_flann_search.cpp index b3f28dde..df666e67 100644 --- a/test/search/test_flann_search.cpp +++ b/test/search/test_flann_search.cpp @@ -37,7 +37,7 @@ */ #include -#include +#include #include #include #include diff --git a/test/search/test_kdtree.cpp b/test/search/test_kdtree.cpp index 501ca85c..028bbbf5 100644 --- a/test/search/test_kdtree.cpp +++ b/test/search/test_kdtree.cpp @@ -36,7 +36,7 @@ * */ #include -#include +#include #include #include #include diff --git a/test/search/test_octree.cpp b/test/search/test_octree.cpp index aa19f779..b2a7f2ef 100644 --- a/test/search/test_octree.cpp +++ b/test/search/test_octree.cpp @@ -35,7 +35,7 @@ * * */ -#include +#include #include #include #include diff --git a/test/search/test_organized.cpp b/test/search/test_organized.cpp index 73324616..654a74fc 100644 --- a/test/search/test_organized.cpp +++ b/test/search/test_organized.cpp @@ -35,7 +35,7 @@ */ /** \author Julius Kammerl (julius@kammerl.de)*/ -#include +#include #include @@ -189,12 +189,6 @@ TEST (PCL, Organized_Neighbor_Pointcloud_Neighbours_Within_Radius_Search) search::OrganizedNeighbor organizedNeighborSearch; - std::vector k_indices; - std::vector k_sqr_distances; - - std::vector k_indices_bruteforce; - std::vector k_sqr_distances_bruteforce; - // typical focal length from kinect constexpr double oneOverFocalLength = 0.0018; diff --git a/test/search/test_organized_index.cpp b/test/search/test_organized_index.cpp index 192b109b..262dd592 100644 --- a/test/search/test_organized_index.cpp +++ b/test/search/test_organized_index.cpp @@ -35,7 +35,7 @@ * * */ -#include +#include #include #include #include @@ -477,12 +477,6 @@ TEST (PCL, Organized_Neighbor_Search_Pointcloud_Neighbours_Within_Radius_Search_ pcl::search::Search* organizedNeighborSearch = new pcl::search::OrganizedNeighbor(); - std::vector k_indices; - std::vector k_sqr_distances; - - std::vector k_indices_bruteforce; - std::vector k_sqr_distances_bruteforce; - // typical focal length from kinect unsigned int randomIdx; diff --git a/test/search/test_search.cpp b/test/search/test_search.cpp index a70c5478..348fe834 100644 --- a/test/search/test_search.cpp +++ b/test/search/test_search.cpp @@ -35,7 +35,7 @@ * */ -#include +#include #include diff --git a/test/segmentation/test_non_linear.cpp b/test/segmentation/test_non_linear.cpp index 9f1e6285..14101827 100644 --- a/test/segmentation/test_non_linear.cpp +++ b/test/segmentation/test_non_linear.cpp @@ -37,7 +37,7 @@ * */ -#include +#include #include #include #include diff --git a/test/segmentation/test_random_walker.cpp b/test/segmentation/test_random_walker.cpp index a968f0f6..89c6e37b 100644 --- a/test/segmentation/test_random_walker.cpp +++ b/test/segmentation/test_random_walker.cpp @@ -45,7 +45,7 @@ #include #include -#include +#include #include @@ -235,7 +235,7 @@ TEST_P (RandomWalkerTest, GetPotentials) } } -INSTANTIATE_TEST_CASE_P (VariousGraphs, +INSTANTIATE_TEST_SUITE_P (VariousGraphs, RandomWalkerTest, ::testing::Values ("graph0.info", "graph1.info", diff --git a/test/segmentation/test_segmentation.cpp b/test/segmentation/test_segmentation.cpp index b379f6e7..5121ecc5 100644 --- a/test/segmentation/test_segmentation.cpp +++ b/test/segmentation/test_segmentation.cpp @@ -37,7 +37,7 @@ * */ -#include +#include #include #include #include diff --git a/test/surface/test_concave_hull.cpp b/test/surface/test_concave_hull.cpp index 0f5f9bc2..7c56b22c 100644 --- a/test/surface/test_concave_hull.cpp +++ b/test/surface/test_concave_hull.cpp @@ -37,7 +37,7 @@ * */ -#include +#include #include #include diff --git a/test/surface/test_convex_hull.cpp b/test/surface/test_convex_hull.cpp index f2d63b34..cccc78f9 100644 --- a/test/surface/test_convex_hull.cpp +++ b/test/surface/test_convex_hull.cpp @@ -37,7 +37,7 @@ * */ -#include +#include #include diff --git a/test/surface/test_ear_clipping.cpp b/test/surface/test_ear_clipping.cpp index 66446252..33c55638 100644 --- a/test/surface/test_ear_clipping.cpp +++ b/test/surface/test_ear_clipping.cpp @@ -37,7 +37,7 @@ * */ -#include +#include #include #include diff --git a/test/surface/test_gp3.cpp b/test/surface/test_gp3.cpp index f609f605..03131277 100644 --- a/test/surface/test_gp3.cpp +++ b/test/surface/test_gp3.cpp @@ -37,7 +37,7 @@ * */ -#include +#include #include #include diff --git a/test/surface/test_grid_projection.cpp b/test/surface/test_grid_projection.cpp index 51b7aee8..ead9fd81 100644 --- a/test/surface/test_grid_projection.cpp +++ b/test/surface/test_grid_projection.cpp @@ -37,7 +37,7 @@ * */ -#include +#include #include #include diff --git a/test/surface/test_marching_cubes.cpp b/test/surface/test_marching_cubes.cpp index e4ca075a..319afc4f 100644 --- a/test/surface/test_marching_cubes.cpp +++ b/test/surface/test_marching_cubes.cpp @@ -37,7 +37,7 @@ * */ -#include +#include #include #include diff --git a/test/surface/test_moving_least_squares.cpp b/test/surface/test_moving_least_squares.cpp index d2e04a5a..5949f056 100644 --- a/test/surface/test_moving_least_squares.cpp +++ b/test/surface/test_moving_least_squares.cpp @@ -37,7 +37,7 @@ * */ -#include +#include #include #include diff --git a/test/surface/test_organized_fast_mesh.cpp b/test/surface/test_organized_fast_mesh.cpp index 3b40d8cd..d4e86650 100644 --- a/test/surface/test_organized_fast_mesh.cpp +++ b/test/surface/test_organized_fast_mesh.cpp @@ -37,7 +37,7 @@ * */ -#include +#include #include #include diff --git a/test/surface/test_poisson.cpp b/test/surface/test_poisson.cpp index 8ac4623a..abefc4d0 100644 --- a/test/surface/test_poisson.cpp +++ b/test/surface/test_poisson.cpp @@ -37,7 +37,7 @@ * */ -#include +#include #include #include diff --git a/test/test_recognition_ransac_based_ORROctree.cpp b/test/test_recognition_ransac_based_ORROctree.cpp index 76d4b709..73a9b5f7 100644 --- a/test/test_recognition_ransac_based_ORROctree.cpp +++ b/test/test_recognition_ransac_based_ORROctree.cpp @@ -38,7 +38,7 @@ * */ -#include +#include #include #include #include diff --git a/test/visualization/test_visualization.cpp b/test/visualization/test_visualization.cpp index bdc91903..a6c4c5c6 100644 --- a/test/visualization/test_visualization.cpp +++ b/test/visualization/test_visualization.cpp @@ -37,7 +37,7 @@ * */ -#include +#include #include #include diff --git a/tools/train_linemod_template.cpp b/tools/train_linemod_template.cpp index 558c9833..d63dfa25 100644 --- a/tools/train_linemod_template.cpp +++ b/tools/train_linemod_template.cpp @@ -65,8 +65,8 @@ printHelp (int, char **argv) { print_error ("Syntax is: %s input1.pcd input2.pcd input3.pcd (etc.)\n", argv[0]); print_info (" where options are:\n"); - print_info (" -min_depth z_min = the depth of the near clipping plane\n"); - print_info (" -max_depth z_max = the depth of the far clipping plane\n"); + print_info (" -min_depth z_min = the depth of the near clipping plane\n"); + print_info (" -max_depth z_max = the depth of the far clipping plane\n"); print_info (" -max_height y_max = the height of the vertical clipping plane\n"); print_info ("Two new template files will be created for each input file. They will append "); print_info ("the following suffixes to the original filename:\n"); @@ -77,7 +77,7 @@ printHelp (int, char **argv) void printElapsedTimeAndNumberOfPoints (double t, int w, int h=1) { - print_info ("[done, "); print_value ("%g", t); print_info (" ms : "); + print_info ("[done, "); print_value ("%g", t); print_info (" ms : "); print_value ("%d", w*h); print_info (" points]\n"); } @@ -104,7 +104,7 @@ maskForegroundPoints (const PointCloudXYZRGBA::ConstPtr & input, float min_depth, float max_depth, float max_height) { std::vector foreground_mask (input->size (), false); - + // Mask off points outside the specified near and far depth thresholds pcl::IndicesPtr indices (new std::vector); for (std::size_t i = 0; i < input->size (); ++i) @@ -130,8 +130,8 @@ maskForegroundPoints (const PointCloudXYZRGBA::ConstPtr & input, seg.setIndices (indices); pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ()); pcl::PointIndices::Ptr inliers (new pcl::PointIndices ()); - seg.segment (*inliers, *coefficients); - + seg.segment (*inliers, *coefficients); + // Mask off the plane inliers for (const int &index : inliers->indices) foreground_mask[index] = false; @@ -152,13 +152,13 @@ maskForegroundPoints (const PointCloudXYZRGBA::ConstPtr & input, } void -trainTemplate (const PointCloudXYZRGBA::ConstPtr & input, const std::vector &foreground_mask, +trainTemplate (const PointCloudXYZRGBA::ConstPtr & input, const std::vector &foreground_mask, pcl::LINEMOD & linemod) { pcl::ColorGradientModality color_grad_mod; color_grad_mod.setInputCloud (input); color_grad_mod.processInputData (); - + pcl::SurfaceNormalModality surface_norm_mod; surface_norm_mod.setInputCloud (input); surface_norm_mod.processInputData (); @@ -223,10 +223,7 @@ compute (const PointCloudXYZRGBA::ConstPtr & input, float min_depth, float max_d trainTemplate (input, foreground_mask, linemod); // Save the LINEMOD template - std::ofstream file_stream; - file_stream.open (template_sqmmt_filename.c_str (), std::ofstream::out | std::ofstream::binary); - linemod.getTemplate (0).serialize (file_stream); - file_stream.close (); + linemod.saveTemplates (template_sqmmt_filename.c_str()); } /* ---[ */ @@ -261,14 +258,34 @@ main (int argc, char** argv) float max_height = std::numeric_limits::max (); parse_argument (argc, argv, "-max_height", max_height); + int error_code = 0; + bool processed_at_least_one_pcd = false; + // Segment and create templates for each input file for (const int &p_file_index : p_file_indices) { // Load input file const std::string input_filename = argv[p_file_index]; PointCloudXYZRGBA::Ptr cloud (new PointCloudXYZRGBA); - if (!loadCloud (input_filename, *cloud)) - return (-1); + + if (!loadCloud (input_filename, *cloud)) + { + error_code = -1; + std::string warn_msg = "Could not load point cloud from file: " + input_filename + "\n"; + print_warn (warn_msg.c_str ()); + continue; + } + + if (!cloud->isOrganized()) + { + std::string warn_msg = "Unorganized point cloud detected. Skipping file " + input_filename + "\n"; + print_warn(warn_msg.c_str()); + continue; + } + else + { + processed_at_least_one_pcd = true; + } // Construct output filenames std::string sqmmt_filename = input_filename; @@ -284,5 +301,10 @@ main (int argc, char** argv) compute (cloud, min_depth, max_depth, max_height, pcd_filename, sqmmt_filename); } -} + if (!processed_at_least_one_pcd) + { + print_error("All input pcd files are unorganized.\n"); + } + return error_code; +} diff --git a/tools/virtual_scanner.cpp b/tools/virtual_scanner.cpp index 8a6b43d0..8baf0a7a 100644 --- a/tools/virtual_scanner.cpp +++ b/tools/virtual_scanner.cpp @@ -174,7 +174,7 @@ main (int argc, char** argv) int subdiv_level = 1; double scan_dist = 3; - std::string fname, base; + std::string fname; char seq[256]; // Compute start/stop for vertical and horizontal diff --git a/tracking/include/pcl/tracking/impl/particle_filter.hpp b/tracking/include/pcl/tracking/impl/particle_filter.hpp index 1fde7ff8..caea903e 100644 --- a/tracking/include/pcl/tracking/impl/particle_filter.hpp +++ b/tracking/include/pcl/tracking/impl/particle_filter.hpp @@ -78,7 +78,7 @@ pcl::tracking::ParticleFilterTracker::genAliasTable (std::vect int k = *(H - 1); a[j] = k; q[k] += q[j] - 1; - L++; + ++L; if ( q[k] < 1.0 ) { *L-- = k; @@ -91,7 +91,6 @@ template void pcl::tracking::ParticleFilterTracker::initParticles (bool reset) { particles_.reset (new PointCloudState ()); - std::vector initial_noise_mean; if (reset) { representative_state_.zero (); diff --git a/visualization/include/pcl/visualization/area_picking_event.h b/visualization/include/pcl/visualization/area_picking_event.h index 0091aca4..ae833990 100644 --- a/visualization/include/pcl/visualization/area_picking_event.h +++ b/visualization/include/pcl/visualization/area_picking_event.h @@ -40,6 +40,8 @@ #include +#include + namespace pcl { namespace visualization diff --git a/visualization/include/pcl/visualization/impl/point_cloud_color_handlers.hpp b/visualization/include/pcl/visualization/impl/point_cloud_color_handlers.hpp index ea97ca54..774ddfda 100644 --- a/visualization/include/pcl/visualization/impl/point_cloud_color_handlers.hpp +++ b/visualization/include/pcl/visualization/impl/point_cloud_color_handlers.hpp @@ -460,7 +460,6 @@ pcl::visualization::PointCloudColorHandlerRGBAField::getColor () const scalars->SetNumberOfTuples (nr_points); unsigned char* colors = scalars->GetPointer (0); - int j = 0; // If XYZ present, check if the points are invalid int x_idx = -1; for (std::size_t d = 0; d < fields_.size (); ++d) @@ -469,6 +468,7 @@ pcl::visualization::PointCloudColorHandlerRGBAField::getColor () const if (x_idx != -1) { + int j = 0; // Color every point for (vtkIdType cp = 0; cp < nr_points; ++cp) { diff --git a/visualization/include/pcl/visualization/point_cloud_color_handlers.h b/visualization/include/pcl/visualization/point_cloud_color_handlers.h index 9b3f56ea..24310909 100644 --- a/visualization/include/pcl/visualization/point_cloud_color_handlers.h +++ b/visualization/include/pcl/visualization/point_cloud_color_handlers.h @@ -42,6 +42,7 @@ #endif // PCL includes +#include #include #include #include @@ -113,7 +114,7 @@ namespace pcl * This virtual method should not be overriden or used. The default implementation * is provided only for backwards compatibility with handlers that were written * before PCL 1.10.0 and will be removed in future. */ - [[deprecated("use getColor() without parameters instead")]] + PCL_DEPRECATED("use getColor() without parameters instead") virtual bool getColor (vtkSmartPointer &scalars) const { scalars = getColor (); @@ -615,7 +616,7 @@ namespace pcl * This virtual method should not be overriden or used. The default implementation * is provided only for backwards compatibility with handlers that were written * before PCL 1.10.0 and will be removed in future. */ - [[deprecated("use getColor() without parameters instead")]] + PCL_DEPRECATED("use getColor() without parameters instead") virtual bool getColor (vtkSmartPointer &scalars) const { scalars = getColor (); diff --git a/visualization/src/pcl_visualizer.cpp b/visualization/src/pcl_visualizer.cpp index cafdba85..e1966ae4 100644 --- a/visualization/src/pcl_visualizer.cpp +++ b/visualization/src/pcl_visualizer.cpp @@ -4382,6 +4382,7 @@ pcl::visualization::PCLVisualizer::setUseVbos (bool use_vbos) style_->setUseVbos (use_vbos_); #else PCL_WARN ("[PCLVisualizer::setUseVbos] Has no effect when OpenGL version is ≥ 2\n"); + (void) use_vbos; #endif }