From: Jochen Sprickerhof Date: Fri, 22 May 2020 16:29:43 +0000 (+0200) Subject: New upstream version 1.11.0+dfsg X-Git-Tag: archive/raspbian/1.14.0+dfsg-2+rpi1^2~10^2~5 X-Git-Url: https://dgit.raspbian.org/?a=commitdiff_plain;h=66887105e87ea8273ed89b9bd80a40b460304aea;p=pcl.git New upstream version 1.11.0+dfsg --- diff --git a/.ci/azure-pipelines/azure-pipelines.yaml b/.ci/azure-pipelines/azure-pipelines.yaml new file mode 100644 index 00000000..77796e3a --- /dev/null +++ b/.ci/azure-pipelines/azure-pipelines.yaml @@ -0,0 +1,42 @@ +resources: + containers: + - container: fmt + image: pointcloudlibrary/fmt + - container: env1604 + image: pointcloudlibrary/env:16.04 + - container: env2004 + image: pointcloudlibrary/env:20.04 + - container: doc + image: pointcloudlibrary/doc + +stages: + - stage: formatting + displayName: Formatting + jobs: + - template: formatting.yaml + - stage: build_ubuntu + displayName: Build Ubuntu + dependsOn: formatting + jobs: + - template: build-ubuntu.yaml + - stage: build_osx + displayName: Build macOS + dependsOn: formatting + jobs: + - template: build-macos.yaml + - stage: build_windows + displayName: Build Windows + dependsOn: formatting + jobs: + - template: build-windows.yaml + - stage: documentation + displayName: Documentation + dependsOn: [] + jobs: + - template: documentation.yaml + - stage: tutorials + displayName: Tutorials + dependsOn: build_ubuntu + jobs: + - template: tutorials.yaml + diff --git a/.ci/azure-pipelines/build-macos.yaml b/.ci/azure-pipelines/build-macos.yaml new file mode 100644 index 00000000..6176b518 --- /dev/null +++ b/.ci/azure-pipelines/build-macos.yaml @@ -0,0 +1,65 @@ +jobs: + - job: osx + displayName: macOS + pool: + vmImage: '$(VMIMAGE)' + strategy: + matrix: + Catalina 10.15: + VMIMAGE: 'macOS-10.15' + OSX_VERSION: '10.15' + Mojave 10.14: + VMIMAGE: 'macOS-10.14' + OSX_VERSION: '10.14' + timeoutInMinutes: 0 + variables: + BUILD_DIR: '$(Agent.WorkFolder)/build' + GOOGLE_TEST_DIR: '$(Agent.WorkFolder)/googletest' + CMAKE_CXX_FLAGS: '-Wall -Wextra -Werror -Wabi' + steps: + - checkout: self + # find the commit hash on a quick non-forced update too + fetchDepth: 10 + - script: | + brew install pkg-config qt5 libpcap brewsci/science/openni libomp + brew install vtk --with-qt --without-python@2 + brew install --only-dependencies pcl + git clone https://github.com/abseil/googletest.git $GOOGLE_TEST_DIR # the official endpoint changed to abseil/googletest + cd $GOOGLE_TEST_DIR && git checkout release-1.8.1 + displayName: 'Install Dependencies' + - script: | + mkdir $BUILD_DIR && cd $BUILD_DIR + # Surface switched off due to OpenGL deprecation on Mac + cmake $(Build.SourcesDirectory) \ + -DCMAKE_BUILD_TYPE="Release" \ + -DCMAKE_OSX_SYSROOT="/Library/Developer/CommandLineTools/SDKs/MacOSX$(OSX_VERSION).sdk" \ + -DCMAKE_CXX_FLAGS="$CMAKE_CXX_FLAGS" \ + -DGTEST_SRC_DIR="$GOOGLE_TEST_DIR/googletest" \ + -DGTEST_INCLUDE_DIR="$GOOGLE_TEST_DIR/googletest/include" \ + -DQt5_DIR=/usr/local/opt/qt5/lib/cmake/Qt5 \ + -DPCL_ONLY_CORE_POINT_TYPES=ON \ + -DBUILD_simulation=ON \ + -DBUILD_surface=OFF \ + -DBUILD_global_tests=ON \ + -DBUILD_examples=ON \ + -DBUILD_tools=ON \ + -DBUILD_apps=ON \ + -DBUILD_apps_3d_rec_framework=ON \ + -DBUILD_apps_cloud_composer=ON \ + -DBUILD_apps_in_hand_scanner=ON \ + -DBUILD_apps_modeler=ON \ + -DBUILD_apps_point_cloud_editor=ON + displayName: 'CMake Configuration' + - script: | + cd $BUILD_DIR + cmake --build . -- -j2 + displayName: 'Build Library' + - script: cd $BUILD_DIR && cmake --build . -- tests + displayName: 'Run Unit Tests' + - task: PublishTestResults@2 + inputs: + testResultsFormat: 'CTest' + testResultsFiles: '**/Test*.xml' + searchFolder: '$(Agent.WorkFolder)/build' + condition: succeededOrFailed() + diff --git a/.ci/azure-pipelines/build-macos.yml b/.ci/azure-pipelines/build-macos.yml deleted file mode 100644 index cba91569..00000000 --- a/.ci/azure-pipelines/build-macos.yml +++ /dev/null @@ -1,63 +0,0 @@ -jobs: - - job: osx - strategy: - matrix: - macOS Catalina: - VMIMAGE: 'macOS-10.15' - macOS Mojave: - VMIMAGE: 'macOS-10.14' - timeoutInMinutes: 0 - pool: - vmImage: '$(VMIMAGE)' - variables: - BUILD_DIR: '$(Agent.WorkFolder)/build' - GOOGLE_TEST_DIR: '$(Agent.WorkFolder)/googletest' - CMAKE_CXX_FLAGS: '-Wall -Wextra -Wabi -O2' - steps: - - script: | - brew install pkg-config qt5 libpcap brewsci/science/openni - brew install vtk --with-qt --without-python@2 - brew install --only-dependencies pcl - git clone https://github.com/abseil/googletest.git $GOOGLE_TEST_DIR # the official endpoint changed to abseil/googletest - cd $GOOGLE_TEST_DIR && git checkout release-1.8.1 - displayName: 'Install Dependencies' - - 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" \ - -DQt5_DIR=/usr/local/opt/qt5/lib/cmake/Qt5 \ - -DPCL_ONLY_CORE_POINT_TYPES=ON \ - -DBUILD_simulation=ON \ - -DBUILD_surface=OFF \ - -DBUILD_global_tests=ON \ - -DBUILD_examples=ON \ - -DBUILD_tools=ON \ - -DBUILD_apps=ON \ - -DBUILD_apps_3d_rec_framework=ON \ - -DBUILD_apps_cloud_composer=ON \ - -DBUILD_apps_in_hand_scanner=ON \ - -DBUILD_apps_modeler=ON \ - -DBUILD_apps_point_cloud_editor=ON - displayName: 'CMake Configuration' - - script: | - cd $BUILD_DIR - # Compiling some of the test targets with -j2 option leads to pipeline failures - # (presumably out of memory error). Thus we make them separately in a single - # thread mode. Their corresponding modules are built before with the -j2 mode - # to make the process faster. - cmake --build . -- -j2 pcl_filters pcl_registration - cmake --build . -- test_filters test_registration test_registration_api - cmake --build . -- -j2 - displayName: 'Build Library' - - script: cd $BUILD_DIR && cmake --build . -- tests - displayName: 'Run Unit Tests' - - task: PublishTestResults@2 - inputs: - testResultsFormat: 'CTest' - testResultsFiles: '**/Test*.xml' - searchFolder: '$(Agent.WorkFolder)/build' - condition: succeededOrFailed() - diff --git a/.ci/azure-pipelines/build-ubuntu-16-04.yaml b/.ci/azure-pipelines/build-ubuntu-16-04.yaml deleted file mode 100644 index 837a8eab..00000000 --- a/.ci/azure-pipelines/build-ubuntu-16-04.yaml +++ /dev/null @@ -1,51 +0,0 @@ -resources: - containers: - - container: env1604 - image: pointcloudlibrary/env:16.04 - -jobs: - - job: ubuntu1604 - displayName: Ubuntu 16.04 - timeoutInMinutes: 0 - pool: - vmImage: 'Ubuntu 16.04' - container: env1604 - variables: - BUILD_DIR: '$(Agent.BuildDirectory)/build' - steps: - - script: | - mkdir $BUILD_DIR && cd $BUILD_DIR - cmake $(Build.SourcesDirectory) \ - -DPCL_ONLY_CORE_POINT_TYPES=ON \ - -DPCL_WARNINGS_ARE_ERRORS=ON \ - -DBUILD_simulation=ON \ - -DBUILD_surface_on_nurbs=ON \ - -DBUILD_global_tests=ON \ - -DBUILD_examples=ON \ - -DBUILD_tools=ON \ - -DBUILD_apps=ON \ - -DBUILD_apps_3d_rec_framework=ON \ - -DBUILD_apps_cloud_composer=ON \ - -DBUILD_apps_in_hand_scanner=ON \ - -DBUILD_apps_modeler=ON \ - -DBUILD_apps_point_cloud_editor=ON - displayName: 'CMake Configuration' - - script: | - cd $BUILD_DIR - # Compiling some of the test targets with -j2 option leads to pipeline failures - # (presumably out of memory error). Thus we make them separately in a single - # thread mode. Their corresponding modules are built before with the -j2 mode - # to make the process faster. - cmake --build . -- -j2 pcl_filters pcl_registration - cmake --build . -- test_filters test_registration test_registration_api - cmake --build . -- -j2 - displayName: 'Build Library' - - script: | - cd $BUILD_DIR && cmake --build . -- tests - displayName: 'Run Unit Tests' - - task: PublishTestResults@2 - inputs: - testResultsFormat: 'CTest' - testResultsFiles: '**/Test*.xml' - searchFolder: '$(Agent.BuildDirectory)/build' - condition: succeededOrFailed() diff --git a/.ci/azure-pipelines/build-ubuntu-19-10.yaml b/.ci/azure-pipelines/build-ubuntu-19-10.yaml deleted file mode 100644 index 0dd77e05..00000000 --- a/.ci/azure-pipelines/build-ubuntu-19-10.yaml +++ /dev/null @@ -1,52 +0,0 @@ -resources: - containers: - - container: env1910 - image: pointcloudlibrary/env:19.10 - -jobs: - - job: ubuntu1910 - displayName: Ubuntu 19.10 - timeoutInMinutes: 0 - pool: - vmImage: 'Ubuntu 16.04' - container: env1910 - variables: - BUILD_DIR: '$(Agent.BuildDirectory)/build' - CMAKE_CXX_FLAGS: '-Wall -Wextra -O2' - steps: - - script: | - mkdir $BUILD_DIR && cd $BUILD_DIR - cmake $(Build.SourcesDirectory) \ - -DCMAKE_CXX_FLAGS="$CMAKE_CXX_FLAGS" \ - -DPCL_ONLY_CORE_POINT_TYPES=ON \ - -DBUILD_simulation=ON \ - -DBUILD_surface_on_nurbs=ON \ - -DBUILD_global_tests=ON \ - -DBUILD_examples=ON \ - -DBUILD_tools=ON \ - -DBUILD_apps=ON \ - -DBUILD_apps_3d_rec_framework=ON \ - -DBUILD_apps_cloud_composer=ON \ - -DBUILD_apps_in_hand_scanner=ON \ - -DBUILD_apps_modeler=ON \ - -DBUILD_apps_point_cloud_editor=ON - displayName: 'CMake Configuration' - - script: | - cd $BUILD_DIR - # Compiling some of the test targets with -j2 option leads to pipeline failures - # (presumably out of memory error). Thus we make them separately in a single - # thread mode. Their corresponding modules are built before with the -j2 mode - # to make the process faster. - cmake --build . -- -j2 pcl_filters pcl_registration - cmake --build . -- test_filters test_registration test_registration_api - cmake --build . -- -j2 - displayName: 'Build Library' - - script: | - cd $BUILD_DIR && cmake --build . -- tests - displayName: 'Run Unit Tests' - - task: PublishTestResults@2 - inputs: - testResultsFormat: 'CTest' - testResultsFiles: '**/Test*.xml' - searchFolder: '$(Agent.BuildDirectory)/build' - condition: succeededOrFailed() diff --git a/.ci/azure-pipelines/build-ubuntu.yaml b/.ci/azure-pipelines/build-ubuntu.yaml new file mode 100644 index 00000000..aec0d860 --- /dev/null +++ b/.ci/azure-pipelines/build-ubuntu.yaml @@ -0,0 +1,69 @@ +jobs: + - job: ubuntu + displayName: Ubuntu + pool: + vmImage: 'Ubuntu 16.04' + strategy: + matrix: + # 16.04 Clang: + # CONTAINER: env1604 + # CC: clang + # CXX: clang++ + # CMAKE_ARGS: '-DPCL_WARNINGS_ARE_ERRORS=ON' + # 20.04 Clang: + # CONTAINER: env2004 + # CC: clang + # CXX: clang++ + # CMAKE_ARGS: '' + 16.04 GCC: + CONTAINER: env1604 + CC: gcc + CXX: g++ + CMAKE_ARGS: '-DPCL_WARNINGS_ARE_ERRORS=ON' + 20.04 GCC: + CONTAINER: env2004 + CC: gcc + CXX: g++ + CMAKE_ARGS: '' + container: $[ variables['CONTAINER'] ] + timeoutInMinutes: 0 + variables: + BUILD_DIR: '$(Agent.BuildDirectory)/build' + CMAKE_CXX_FLAGS: '-Wall -Wextra' + steps: + - checkout: self + # find the commit hash on a quick non-forced update too + fetchDepth: 10 + - script: | + mkdir $BUILD_DIR && cd $BUILD_DIR + cmake $(Build.SourcesDirectory) $(CMAKE_ARGS) \ + -DCMAKE_BUILD_TYPE="Release" \ + -DCMAKE_CXX_FLAGS="$CMAKE_CXX_FLAGS" \ + -DPCL_ONLY_CORE_POINT_TYPES=ON \ + -DBUILD_simulation=ON \ + -DBUILD_surface_on_nurbs=ON \ + -DBUILD_global_tests=ON \ + -DBUILD_examples=ON \ + -DBUILD_tools=ON \ + -DBUILD_apps=ON \ + -DBUILD_apps_3d_rec_framework=ON \ + -DBUILD_apps_cloud_composer=ON \ + -DBUILD_apps_in_hand_scanner=ON \ + -DBUILD_apps_modeler=ON \ + -DBUILD_apps_point_cloud_editor=ON + # Temporary fix to ensure no tests are skipped + cmake $(Build.SourcesDirectory) + displayName: 'CMake Configuration' + - script: | + cd $BUILD_DIR + cmake --build . -- -j2 + displayName: 'Build Library' + - script: | + cd $BUILD_DIR && cmake --build . -- tests + displayName: 'Run Unit Tests' + - task: PublishTestResults@2 + inputs: + testResultsFormat: 'CTest' + testResultsFiles: '**/Test*.xml' + searchFolder: '$(Agent.BuildDirectory)/build' + condition: succeededOrFailed() diff --git a/.ci/azure-pipelines/build-windows.yaml b/.ci/azure-pipelines/build-windows.yaml new file mode 100644 index 00000000..07cc01a5 --- /dev/null +++ b/.ci/azure-pipelines/build-windows.yaml @@ -0,0 +1,65 @@ +jobs: + - job: vs2017 + displayName: Windows VS2017 Build + pool: + vmImage: 'vs2017-win2016' + strategy: + matrix: + x86: + PLATFORM: 'x86' + ARCHITECTURE: 'x86' + GENERATOR: 'Visual Studio 15 2017' + x64: + PLATFORM: 'x64' + ARCHITECTURE: 'x86_amd64' + GENERATOR: 'Visual Studio 15 2017 Win64' + timeoutInMinutes: 0 + variables: + BUILD_DIR: '$(Agent.WorkFolder)\build' + CONFIGURATION: 'Release' + VCPKG_ROOT: 'C:\vcpkg' + steps: + - checkout: self + # find the commit hash on a quick non-forced update too + fetchDepth: 10 + - script: | + echo ##vso[task.setvariable variable=BOOST_ROOT]%BOOST_ROOT_1_69_0% + displayName: 'Set BOOST_ROOT Environment Variable' + - script: | + echo ##vso[task.prependpath]%BOOST_ROOT_1_69_0%\lib + displayName: 'Include Boost Libraries In System PATH' + - script: | + set + displayName: 'Print Environment Variables' + - script: | + vcpkg.exe install eigen3 flann gtest qhull --triplet %PLATFORM%-windows && vcpkg.exe list + displayName: 'Install C++ Dependencies Via Vcpkg' + - script: | + rmdir %VCPKG_ROOT%\downloads /S /Q + rmdir %VCPKG_ROOT%\packages /S /Q + displayName: 'Free Up Space' + - script: | + mkdir %BUILD_DIR% && cd %BUILD_DIR% + cmake $(Build.SourcesDirectory) ^ + -G"%GENERATOR%" ^ + -DCMAKE_BUILD_TYPE="Release" ^ + -DCMAKE_TOOLCHAIN_FILE=%VCPKG_ROOT%\scripts\buildsystems\vcpkg.cmake ^ + -DVCPKG_APPLOCAL_DEPS=ON ^ + -DPCL_BUILD_WITH_BOOST_DYNAMIC_LINKING_WIN32=ON ^ + -DPCL_BUILD_WITH_FLANN_DYNAMIC_LINKING_WIN32=ON ^ + -DPCL_BUILD_WITH_QHULL_DYNAMIC_LINKING_WIN32=ON ^ + -DBUILD_global_tests=ON ^ + -DBUILD_tools=OFF ^ + -DBUILD_surface_on_nurbs=ON + displayName: 'CMake Configuration' + - script: cd %BUILD_DIR% && cmake --build . --config %CONFIGURATION% + displayName: 'Build Library' + - script: cd %BUILD_DIR% && cmake --build . --target tests --config %CONFIGURATION% + displayName: 'Run Unit Tests' + - task: PublishTestResults@2 + inputs: + testResultsFormat: 'CTest' + testResultsFiles: '**/Test*.xml' + searchFolder: '$(Agent.WorkFolder)\build' + condition: succeededOrFailed() + diff --git a/.ci/azure-pipelines/build-windows.yml b/.ci/azure-pipelines/build-windows.yml deleted file mode 100644 index 7bfaa40a..00000000 --- a/.ci/azure-pipelines/build-windows.yml +++ /dev/null @@ -1,48 +0,0 @@ -jobs: - - job: vs2017 - displayName: Windows VS2017 Build - timeoutInMinutes: 0 - pool: - vmImage: 'vs2017-win2016' - strategy: - matrix: - x86: - PLATFORM: 'x86' - ARCHITECTURE: 'x86' - GENERATOR: 'Visual Studio 15 2017' - x64: - PLATFORM: 'x64' - ARCHITECTURE: 'x86_amd64' - GENERATOR: 'Visual Studio 15 2017 Win64' - variables: - BUILD_DIR: '$(Agent.WorkFolder)\build' - CONFIGURATION: 'Release' - VCPKG_ROOT: 'C:\vcpkg' - steps: - - script: set - displayName: 'Print Environment Variables' - - script: | - echo ##vso[task.prependpath]%BOOST_ROOT%\lib - displayName: 'Update System PATH' - - script: | - vcpkg.exe install eigen3 flann gtest qhull --triplet %PLATFORM%-windows && vcpkg.exe list - displayName: 'Install c++ dependencies via vcpkg' - - script: | - rmdir %VCPKG_ROOT%\downloads /S /Q - rmdir %VCPKG_ROOT%\packages /S /Q - displayName: 'Free Up Space' - - script: | - mkdir %BUILD_DIR% && cd %BUILD_DIR% - cmake $(Build.SourcesDirectory) -G"%GENERATOR%" -DCMAKE_TOOLCHAIN_FILE=%VCPKG_ROOT%\scripts\buildsystems\vcpkg.cmake -DVCPKG_APPLOCAL_DEPS=ON -DPCL_BUILD_WITH_BOOST_DYNAMIC_LINKING_WIN32=ON -DPCL_BUILD_WITH_FLANN_DYNAMIC_LINKING_WIN32=ON -DPCL_BUILD_WITH_QHULL_DYNAMIC_LINKING_WIN32=ON -DBUILD_global_tests=ON -DBUILD_tools=OFF -DBUILD_surface_on_nurbs=ON - displayName: 'CMake Configuration' - - script: cd %BUILD_DIR% && cmake --build . --config %CONFIGURATION% - displayName: 'Build Library' - - script: cd %BUILD_DIR% && cmake --build . --target tests --config %CONFIGURATION% - displayName: 'Run Unit Tests' - - task: PublishTestResults@2 - inputs: - testResultsFormat: 'CTest' - testResultsFiles: '**/Test*.xml' - searchFolder: '$(Agent.WorkFolder)\build' - condition: succeededOrFailed() - diff --git a/.ci/azure-pipelines/documentation.yaml b/.ci/azure-pipelines/documentation.yaml new file mode 100644 index 00000000..f6f93d15 --- /dev/null +++ b/.ci/azure-pipelines/documentation.yaml @@ -0,0 +1,56 @@ +jobs: + - job: documentation + displayName: Generate Documentation + pool: + vmImage: 'Ubuntu 16.04' + container: doc + variables: + BUILD_DIR: '$(Agent.BuildDirectory)/build' + DOC_DIR: '$(Agent.BuildDirectory)/documentation' + CHANGELOG: '$(Agent.TempDirectory)/changelog' + steps: + - checkout: self + # find the commit hash on a quick non-forced update too + fetchDepth: 10 + - task: InstallSSHKey@0 + inputs: + hostName: github.com + sshPublicKey: ssh-ed25519 AAAAC3NzaC1lZDI1NTE5AAAAIBh5Yrau/gguTfoNALxhVX77Pgz6y6UWoJRERMKR68ee documentation@pointclouds.org + sshKeySecureFile: id_ed25519 + condition: eq(variables['Build.Repository.Name'], 'PointCloudLibrary/pcl') + - script: | + $(Build.SourcesDirectory)/.dev/scripts/generate_changelog.py --with-misc > $CHANGELOG.md + grip --export $CHANGELOG.md $CHANGELOG.html + pandoc -f markdown -t plain --wrap=none $CHANGELOG.md + displayName: 'Generate Changelog' + - script: | + mkdir $BUILD_DIR && cd $BUILD_DIR + cmake $(Build.SourcesDirectory) \ + -DDOXYGEN_USE_SHORT_NAMES=OFF \ + -DSPHINX_HTML_FILE_SUFFIX=php \ + -DWITH_DOCS=ON \ + -DWITH_TUTORIALS=ON + displayName: 'CMake Configuration' + - script: | + cd $BUILD_DIR + cmake --build . -- doc tutorials advanced + displayName: 'Build Documentation' + - script: | + cd $BUILD_DIR + sed -i -r -e 's/([0-9]+)\s\.\s([0-9]+)\s/\1.\2/' doc/doxygen/html/deprecated.html + displayName: 'Remove extra spaces in Doxygen''s Deprecated List' + - script: | + git config --global user.email "documentation@pointclouds.org" + git config --global user.name "PointCloudLibrary (via Azure Pipelines)" + echo -e "Host github.com\n\tStrictHostKeyChecking no\n" >> ~/.ssh/config + git clone git@github.com:PointCloudLibrary/documentation.git $DOC_DIR + cd $DOC_DIR + cp -r $BUILD_DIR/doc/tutorials/html/* tutorials + cp -r $BUILD_DIR/doc/advanced/html/* advanced + cp -r $BUILD_DIR/doc/doxygen/html/* . + cp $CHANGELOG.html . + git add --all + git commit --amend --reset-author -m 'Documentation for commit $(Build.SourceVersion)' -q + git push --force + displayName: 'Push Generated Documentation To GitHub' + condition: and(eq(variables['Build.Repository.Name'], 'PointCloudLibrary/pcl'), eq(variables['Build.SourceBranch'], 'refs/heads/master')) diff --git a/.ci/azure-pipelines/documentation.yml b/.ci/azure-pipelines/documentation.yml deleted file mode 100644 index 3ec71b69..00000000 --- a/.ci/azure-pipelines/documentation.yml +++ /dev/null @@ -1,50 +0,0 @@ -resources: - containers: - - container: doc - image: pointcloudlibrary/doc - -jobs: - - job: documentation - displayName: Generate Documentation - pool: - vmImage: 'Ubuntu 16.04' - container: doc - variables: - BUILD_DIR: '$(Agent.BuildDirectory)/build' - DOC_DIR: '$(Agent.BuildDirectory)/documentation' - steps: - - task: InstallSSHKey@0 - inputs: - hostName: github.com - sshPublicKey: ssh-ed25519 AAAAC3NzaC1lZDI1NTE5AAAAIBh5Yrau/gguTfoNALxhVX77Pgz6y6UWoJRERMKR68ee documentation@pointclouds.org - sshKeySecureFile: id_ed25519 - - task: UsePythonVersion@0 - inputs: - versionSpec: '3.6' - addToPath: true - - script: | - mkdir $BUILD_DIR && cd $BUILD_DIR - cmake $(Build.SourcesDirectory) \ - -DDOXYGEN_USE_SHORT_NAMES=OFF \ - -DSPHINX_HTML_FILE_SUFFIX=php \ - -DWITH_DOCS=ON \ - -DWITH_TUTORIALS=ON - displayName: 'CMake Configuration' - - script: | - cd $BUILD_DIR - cmake --build . -- doc tutorials advanced - displayName: 'Build Documentation' - - script: | - git config --global user.email "documentation@pointclouds.org" - git config --global user.name "PointCloudLibrary (via Azure Pipelines)" - echo -e "Host github.com\n\tStrictHostKeyChecking no\n" >> ~/.ssh/config - git clone git@github.com:PointCloudLibrary/documentation.git $DOC_DIR - cd $DOC_DIR - cp -r $BUILD_DIR/doc/tutorials/html/* tutorials - cp -r $BUILD_DIR/doc/advanced/html/* advanced - cp -r $BUILD_DIR/doc/doxygen/html/* . - git add --all - git commit --amend --reset-author -m 'Documentation for commit $(Build.SourceVersion)' -q - git push --force - displayName: 'Push Generated Documentation To GitHub' - condition: eq(variables['Build.SourceBranch'], 'refs/heads/master') diff --git a/.ci/azure-pipelines/env.yml b/.ci/azure-pipelines/env.yml new file mode 100644 index 00000000..d55079df --- /dev/null +++ b/.ci/azure-pipelines/env.yml @@ -0,0 +1,94 @@ +# Docker +# Build a Docker image +# https://docs.microsoft.com/azure/devops/pipelines/languages/docker + +trigger: + branches: + include: + - master + paths: + include: + - .dev/docker/env/Dockerfile + - .ci/azure-pipelines/env.yml + +pr: + paths: + include: + - .dev/docker/env/Dockerfile + - .ci/azure-pipelines/env.yml + +schedules: +- cron: "0 0 * * 0" + displayName: "Sunday midnight build" + branches: + include: + - master + +resources: +- repo: self + +variables: + dockerHub: 'PointCloudLibrary@hub.docker.com' + dockerHubID: "pointcloudlibrary" + +jobs: +- job: BuildAndPush + timeoutInMinutes: 360 + displayName: "Env" + pool: + vmImage: 'ubuntu-latest' + strategy: + matrix: + Ubuntu 16.04: + CUDA_VERSION: 9.2 + UBUNTU_DISTRO: 16.04 + USE_CUDA: true + VTK_VERSION: 6 + tag: 16.04 + Ubuntu 18.04: + CUDA_VERSION: 10.2 + UBUNTU_DISTRO: 18.04 + USE_CUDA: true + VTK_VERSION: 6 + tag: 18.04 + Ubuntu 20.04: + CUDA_VERSION: 11 + UBUNTU_DISTRO: 20.04 + VTK_VERSION: 7 + # nvidia-cuda docker image has not been released for 20.04 yet + USE_CUDA: "" + tag: 20.04 + steps: + - task: Docker@2 + displayName: "Build docker image" + inputs: + command: build + arguments: | + --no-cache + --build-arg CUDA_VERSION=$(CUDA_VERSION) + --build-arg UBUNTU_DISTRO=$(UBUNTU_DISTRO) + --build-arg USE_CUDA=$(USE_CUDA) + --build-arg VTK_VERSION=$(VTK_VERSION) + -t $(dockerHubID)/env:$(tag) + dockerfile: '$(Build.SourcesDirectory)/.dev/docker/env/Dockerfile' + tags: "$(tag)" + - script: | + set -x + docker run --rm -v "$(Build.SourcesDirectory)":/pcl $(dockerHubID)/env:$(tag) bash -c ' \ + mkdir /pcl/build && cd /pcl/build && \ + cmake /pcl \ + -DCMAKE_BUILD_TYPE="Release" \ + -DPCL_ONLY_CORE_POINT_TYPES=ON \ + -DBUILD_io:BOOL=OFF \ + -DBUILD_kdtree:BOOL=OFF && \ + cmake --build . -- -j2' + displayName: 'Verify Dockerimage' + - task: Docker@2 + displayName: "Push docker image" + inputs: + command: push + containerRegistry: $(dockerHub) + repository: $(dockerHubID)/env + tags: "$(tag)" + condition: and(eq(variables['Build.Repository.Name'], 'PointCloudLibrary/pcl'), + eq(variables['Build.SourceBranch'], 'refs/heads/master')) diff --git a/.ci/azure-pipelines/formatting.yaml b/.ci/azure-pipelines/formatting.yaml new file mode 100644 index 00000000..0033e72f --- /dev/null +++ b/.ci/azure-pipelines/formatting.yaml @@ -0,0 +1,29 @@ +jobs: + - job: formatting + displayName: Check code formatting + pool: + vmImage: 'Ubuntu 16.04' + container: fmt + steps: + - checkout: self + # find the commit hash on a quick non-forced update too + fetchDepth: 10 + - script: ./.dev/format.sh $(which clang-format) . + displayName: 'Run clang-format' + - script: git diff > formatting.patch + displayName: 'Compute diff' + - script: cat formatting.patch + displayName: 'Show diff' + - task: CopyFiles@2 + inputs: + sourceFolder: '$(Build.SourcesDirectory)' + contents: 'formatting.patch' + targetFolder: '$(Build.ArtifactStagingDirectory)' + displayName: 'Copy diff to staging directory' + - task: PublishBuildArtifacts@1 + inputs: + pathtoPublish: '$(Build.ArtifactStagingDirectory)' + artifactName: formatting + displayName: 'Publish diff' + - script: "[ ! -s formatting.patch ]" + displayName: 'Set job exit status' diff --git a/.ci/azure-pipelines/formatting.yml b/.ci/azure-pipelines/formatting.yml deleted file mode 100644 index 1bff15b3..00000000 --- a/.ci/azure-pipelines/formatting.yml +++ /dev/null @@ -1,31 +0,0 @@ -resources: - containers: - - container: fmt - image: pointcloudlibrary/fmt - -jobs: - - job: formatting - displayName: Check code formatting - pool: - vmImage: 'Ubuntu 16.04' - container: fmt - steps: - - script: ./.dev/format.sh $(which clang-format-8) . - displayName: 'Run clang-format' - - script: git diff > formatting.patch - displayName: 'Compute diff' - - script: cat formatting.patch - displayName: 'Show diff' - - task: CopyFiles@2 - inputs: - sourceFolder: '$(Build.SourcesDirectory)' - contents: 'formatting.patch' - targetFolder: '$(Build.ArtifactStagingDirectory)' - displayName: 'Copy diff to staging directory' - - task: PublishBuildArtifacts@1 - inputs: - pathtoPublish: '$(Build.ArtifactStagingDirectory)' - artifactName: formatting - displayName: 'Publish diff' - - script: "[ ! -s formatting.patch ]" - displayName: 'Set job exit status' diff --git a/.ci/azure-pipelines/tutorials.yaml b/.ci/azure-pipelines/tutorials.yaml new file mode 100644 index 00000000..05922c47 --- /dev/null +++ b/.ci/azure-pipelines/tutorials.yaml @@ -0,0 +1,53 @@ +jobs: + - job: tutorials + displayName: Building Tutorials + pool: + vmImage: 'Ubuntu 16.04' + container: env1604 + timeoutInMinutes: 0 + variables: + BUILD_DIR: '$(Agent.BuildDirectory)/build' + INSTALL_DIR: '$(Agent.BuildDirectory)/install' + CMAKE_CXX_FLAGS: '-Wall -Wextra -Wabi' + EXCLUDE_TUTORIALS: 'davidsdk,ensenso_cameras,gpu' + steps: + - checkout: self + # find the commit hash on a quick non-forced update too + fetchDepth: 10 + - script: | + mkdir $BUILD_DIR && cd $BUILD_DIR + cmake $(Build.SourcesDirectory) \ + -DCMAKE_BUILD_TYPE="Release" \ + -DCMAKE_INSTALL_PREFIX=$INSTALL_DIR \ + -DCMAKE_CXX_FLAGS="$CMAKE_CXX_FLAGS" \ + -DPCL_ONLY_CORE_POINT_TYPES=ON \ + -DPCL_NO_PRECOMPILE=ON \ + -DBUILD_surface_on_nurbs=ON \ + -DBUILD_global_tests=OFF \ + -DBUILD_tools=OFF \ + -DBUILD_examples=OFF \ + -DBUILD_outofcore=OFF \ + -DBUILD_stereo=OFF \ + -DBUILD_simulation=OFF + displayName: 'CMake Configuration' + - script: | + cd $BUILD_DIR + cmake --build . -- -j2 + displayName: 'Build Library' + - script: | + cd $BUILD_DIR + cmake --build . -- install + displayName: 'Install PCL' + - script: | + cd $BUILD_DIR + major=$(find . -type f -exec sed -n -e \ + 's,#define PCL_MAJOR_VERSION \([0-9]\+\),\1,p' {} \;) + minor=$(find . -type f -exec sed -n -e \ + 's,#define PCL_MINOR_VERSION \([0-9]\+\),\1,p' {} \;) + $(Build.SourcesDirectory)/.ci/scripts/build_tutorials.sh \ + -k -s \ + -e $EXCLUDE_TUTORIALS \ + $INSTALL_DIR/share/pcl-${major}.${minor} \ + $(Build.SourcesDirectory) \ + $BUILD_DIR/tutorials + displayName: 'Build Tutorials' diff --git a/.ci/azure-pipelines/tutorials.yml b/.ci/azure-pipelines/tutorials.yml deleted file mode 100644 index 3b675666..00000000 --- a/.ci/azure-pipelines/tutorials.yml +++ /dev/null @@ -1,44 +0,0 @@ -resources: - containers: - - container: env1604 - image: pointcloudlibrary/env:16.04 - -jobs: - - job: tutorials - displayName: Tutorials - timeoutInMinutes: 0 - pool: - vmImage: 'Ubuntu 16.04' - container: env1604 - variables: - BUILD_DIR: '$(Agent.BuildDirectory)/build' - INSTALL_DIR: '$(Agent.BuildDirectory)/install' - CMAKE_CXX_FLAGS: '-Wall -Wextra -Wabi -O2' - EXCLUDE_TUTORIALS: 'davidsdk,ensenso_cameras,gpu' - steps: - - script: | - mkdir $BUILD_DIR && cd $BUILD_DIR - cmake $(Build.SourcesDirectory) \ - -DCMAKE_INSTALL_PREFIX=$INSTALL_DIR \ - -DCMAKE_CXX_FLAGS="$CMAKE_CXX_FLAGS" \ - -DPCL_ONLY_CORE_POINT_TYPES=ON \ - -DPCL_NO_PRECOMPILE=ON \ - -DBUILD_surface_on_nurbs=ON \ - -DBUILD_global_tests=OFF \ - -DBUILD_tools=OFF \ - -DBUILD_examples=OFF \ - -DBUILD_outofcore=OFF \ - -DBUILD_stereo=OFF \ - -DBUILD_simulation=OFF - displayName: 'CMake Configuration' - - script: | - cd $BUILD_DIR - cmake --build . -- -j2 - displayName: 'Build Library' - - script: | - cd $BUILD_DIR - cmake --build . -- install - displayName: 'Install PCL' - - script: | - $(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/.clang-format b/.clang-format index 86162372..3e0ebf2f 100644 --- a/.clang-format +++ b/.clang-format @@ -1,6 +1,6 @@ --- AlwaysBreakAfterReturnType: All -AlwaysBreakTemplateDeclarations: true +AlwaysBreakTemplateDeclarations: Yes BinPackArguments: false BinPackParameters: false BraceWrapping: @@ -17,7 +17,53 @@ ColumnLimit: 88 ConstructorInitializerAllOnOneLineOrOnePerLine: true ConstructorInitializerIndentWidth: 0 Language: Cpp +NamespaceIndentation: None PointerAlignment: Left -Standard: Cpp11 +Standard: c++14 TabWidth: 2 UseTab: Never +IncludeBlocks: Regroup +IncludeCategories: +# Main PCL includes of common module should be sorted at end of PCL includes + - Regex: '^$' + Priority: 100 + SortPriority: 101 +# All other PCL includes, which are grouped into modules + - Regex: '^$' + Priority: 100 + SortPriority: 100 +# Major 3rd-Party components of tests & modules + - Regex: '^$' + Priority: 300 + - Regex: '^$' + Priority: 300 + - Regex: '^$' + Priority: 310 +# Minor 3rd-Party components + - Regex: '^$' + Priority: 900 +# Any unknown include + - Regex: '.*' + Priority: 500 diff --git a/.dev/docker/doc/Dockerfile b/.dev/docker/doc/Dockerfile index 5d4f95e6..43800b49 100644 --- a/.dev/docker/doc/Dockerfile +++ b/.dev/docker/doc/Dockerfile @@ -1,4 +1,4 @@ -FROM pointcloudlibrary/env:19.04 +FROM pointcloudlibrary/env:20.04 ENV DEBIAN_FRONTEND=noninteractive @@ -8,6 +8,7 @@ RUN apt-get update \ dvipng \ git \ python3-pip \ + pandoc \ && rm -rf /var/lib/apt/lists/* -RUN pip3 install Jinja2==2.8.1 sphinx sphinxcontrib-doxylink +RUN pip3 install Jinja2 sphinx sphinxcontrib-doxylink sphinx_rtd_theme requests grip diff --git a/.dev/docker/env/Dockerfile b/.dev/docker/env/Dockerfile index b133937d..9c832048 100644 --- a/.dev/docker/env/Dockerfile +++ b/.dev/docker/env/Dockerfile @@ -18,6 +18,7 @@ RUN apt-get update \ && apt-get install -y \ cmake \ g++ \ + clang \ wget \ libboost-date-time-dev \ libboost-filesystem-dev \ diff --git a/.dev/docker/fmt/Dockerfile b/.dev/docker/fmt/Dockerfile index b3fc3d98..4535cb93 100644 --- a/.dev/docker/fmt/Dockerfile +++ b/.dev/docker/fmt/Dockerfile @@ -1,11 +1,12 @@ -FROM ubuntu:19.04 +# Azure needs node shadow, sudo and the label +FROM node:lts-alpine -ENV DEBIAN_FRONTEND=noninteractive -ARG CLANG_FORMAT_VERSION=8 +# clang-10 needed alpine edge as of 2020-Apr-28 +RUN apk add \ + --no-cache \ + --repository=http://dl-cdn.alpinelinux.org/alpine/edge/main \ + bash clang git shadow sudo -RUN apt-get update \ - && apt-get install -y \ - clang-format-${CLANG_FORMAT_VERSION} \ - bash \ - git \ - && rm -rf /var/lib/apt/lists/* +LABEL "com.azure.dev.pipelines.agent.handler.node.path"="/usr/local/bin/node" + +CMD [ "bash" ] diff --git a/.dev/format.sh b/.dev/format.sh index 7b7c1d13..c3ccba70 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 octree simulation stereo" + local whitelist="apps/modeler 2d ml octree simulation stereo" local PCL_DIR="${2}" local formatter="${1}" diff --git a/.dev/scripts/generate_changelog.py b/.dev/scripts/generate_changelog.py new file mode 100755 index 00000000..11390a32 --- /dev/null +++ b/.dev/scripts/generate_changelog.py @@ -0,0 +1,253 @@ +#! /usr/bin/env python3 + +""" +Software License Agreement (BSD License) + + Point Cloud Library (PCL) - www.pointclouds.org + Copyright (c) 2018-, 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. + +""" + +import json +import argparse +from pathlib import Path + +import requests +import re + + +CATEGORIES = { + "new feature": ("New features", ""), + "deprecation": ( + "Deprecation", + "of public APIs, scheduled to be removed after two minor releases", + ), + "removal": ("Removal", "of the public APIs deprecated in previous releases"), + "behavior change": ("Behavior changes", "in classes, apps, or tools",), + "API break": ( + "API changes", + "that did not go through the proper deprecation and removal cycle", + ), + "ABI break": ("ABI changes", "that are still API compatible",), + "fix": (None, None), + "enhancement": (None, None), +} + + +MODULES = { + "cmake": "CMake", + "2d": "libpcl_2d", + "common": "libpcl_common", + "cuda": "libpcl_cuda", + "features": "libpcl_features", + "filters": "libpcl_filters", + "geometry": "libpcl_geometry", + "gpu": "libpcl_gpu", + "io": "libpcl_io", + "kdtree": "libpcl_kdtree", + "keypoints": "libpcl_keypoints", + "ml": "libpcl_ml", + "octree": "libpcl_octree", + "outofcore": "libpcl_outofcore", + "people": "libpcl_people", + "recognition": "libpcl_recognition", + "registration": "libpcl_registration", + "sample_consensus": "libpcl_sample_consensus", + "search": "libpcl_search", + "segmentation": "libpcl_segmentation", + "simulation": "libpcl_simulation", + "stereo": "libpcl_stereo", + "surface": "libpcl_surface", + "visualization": "libpcl_visualization", + "apps": "PCL Apps", + "docs": "PCL Docs", + "tutorials": "PCL Tutorials", + "examples": "PCL Examples", + "tests": "PCL Tests", + "tools": "PCL Tools", + "ci": "CI", + None: "Uncategorized", +} + + +def fetch_latest_release_date(): + """ + Fetch the date when the latest release was created. + """ + url = "https://api.github.com/repos/PointCloudLibrary/pcl/releases/latest" + response = requests.get(url) + response.raise_for_status() + return response.json()["created_at"] + + +def fetch_pr_since(start_date): + """ + Fetch data of PRs merged after a given start date. + """ + url = f"https://api.github.com/search/issues?q=is:pr+repo:PointCloudLibrary/pcl+merged:>={start_date}" + pr_data = list() + page = 1 + while True: + response = requests.get(f"{url}&page={page}&per_page=100") + response.raise_for_status() + data = response.json() + total_count = data["total_count"] + pr_data.extend(data["items"]) + page += 1 + if len(pr_data) == total_count: + break + return pr_data + + +def filter_labels(labels, prefix): + """ + Filter a given list of PR labels, keeping only those starting with a given prefix. + The prefix is stripped from the labels. + """ + return [ + label["name"][len(prefix) :] + for label in labels + if label["name"].startswith(prefix) + ] + + +def strip_leading_tag(text): + """ + >>> strip_leading_tag("[text] larger text") + 'larger text' + >>> strip_leading_tag("no tag text") + 'no tag text' + """ + if len(text) == 0 or text[0] != '[': + return text + pattern = re.compile('\[.*\]\s*') + match = pattern.match(text) + return text[match.end():] if match else text + + +def make_pr_bullet_point(pr, prefix=None): + ref = "[#{0}](https://github.com/PointCloudLibrary/pcl/pull/{0})".format( + pr["number"] + ) + + tags = "" + if prefix in ("modules", "categories"): + tags = "".join(["[" + k + "]" for k in pr[prefix]]) + if tags: + tags = "**" + tags + "** " + + return f"* {tags}{pr['title']} [{ref}]" + + +def generate_category_section(key, prs): + section = list() + filtered_prs = [pr for pr in prs if key in pr["categories"]] + title, description = CATEGORIES[key] + if filtered_prs and title: + section += [f"\n**{title}** *{description}*\n"] + section += [make_pr_bullet_point(pr, "modules") for pr in filtered_prs] + return section + + +def generate_module_section(key, prs): + section = list() + filtered_prs = [pr for pr in prs if key in pr["modules"]] + title = MODULES[key] + if filtered_prs and title: + section += [f"\n#### {title}:\n"] + section += [make_pr_bullet_point(pr, "categories") for pr in filtered_prs] + return section + + +if __name__ == "__main__": + parser = argparse.ArgumentParser( + description=""" + Generate changelog from pull requests merged after the latest release. + """, + formatter_class=argparse.RawDescriptionHelpFormatter, + ) + cache_grp = parser.add_mutually_exclusive_group() + cache_grp.add_argument("--save", type=Path, help="Save PR data fetched from GitHub") + cache_grp.add_argument("--load", type=Path, help="Load PR data from a file") + parser.add_argument( + "--with-misc", + "-m", + action="store_true", + help=( + "Add section with miscellaneous PRs that are not important enough to be " + "included in the official changelog" + ), + ) + args = parser.parse_args() + + if args.load: + with open(args.load) as fp: + pr_data = json.loads(fp.read()) + else: + date = fetch_latest_release_date() + pr_data = fetch_pr_since(date) + if args.save: + with open(args.save, "w") as fp: + fp.write(json.dumps(pr_data)) + + selected_prs = list() + excluded_prs = list() + for pr in sorted(pr_data, key=lambda d: d["closed_at"]): + categories = filter_labels(pr["labels"], "changelog: ") + title = strip_leading_tag(pr["title"]) + pr_info = { + "number": pr["number"], + "title": title, + "modules": filter_labels(pr["labels"], "module: "), + "categories": [g for g in categories if g not in ["fix", "enhancement"]], + } + if categories: + selected_prs.append(pr_info) + else: # exclude PRs not tagged with any changelog label + excluded_prs.append(pr_info) + + clog = list() + + clog += ["\n### Notable changes"] + for k in CATEGORIES.keys(): + clog.extend(generate_category_section(k, selected_prs)) + + clog += ["\n### Changes grouped by module"] + for k in MODULES.keys(): + clog.extend(generate_module_section(k, selected_prs)) + + if args.with_misc: + clog += ["\n### Miscellaneous PRs excluded from changelog\n"] + for pr in excluded_prs: + clog += [make_pr_bullet_point(pr)] + + print("\n".join(clog)) diff --git a/.github/change_log.py b/.github/change_log.py deleted file mode 100755 index 44453f57..00000000 --- a/.github/change_log.py +++ /dev/null @@ -1,449 +0,0 @@ -#! /usr/bin/env python3 - -""" -Software License Agreement (BSD License) - - Point Cloud Library (PCL) - www.pointclouds.org - Copyright (c) 2018-, 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. - -""" - -import argparse -from argparse import ArgumentParser -from collections import defaultdict -import getpass -import json -import os -import pathlib -import re -import subprocess -import sys - -import requests - - -def find_pcl_folder(): - folder = os.path.dirname(os.path.abspath(__file__)) - folder = pathlib.Path(folder).parent - return str(folder) - - -def find_pr_list(start: str, end: str): - """Returns all PR ids from a certain commit range. Inspired in - http://joey.aghion.com/find-the-github-pull-request-for-a-commit/ - https://stackoverflow.com/questions/36433572/how-does-ancestry-path-work-with-git-log#36437843 - """ - - # Let git generate the proper pr history - cmd = "git log --oneline " + start + ".." + end - cmd = cmd.split() - output = subprocess.run(cmd, cwd=FOLDER, stdout=subprocess.PIPE) - pr_commits = output.stdout.split(b"\n") - - # Fetch ids for all merge requests from PRS - merge_re = re.compile("\S+ Merge pull request #(\d+) from \S+") - squash_re = re.compile("\(#(\d+)\)") - - ids = [] - for pr in pr_commits: - - pr_s = str(pr) - - # Match agains usual pattern - uid = None - match = merge_re.fullmatch(pr_s) - - # Match agains squash pattern - if not match: - match = squash_re.search(pr_s) - - # Abort - if not match: - continue - - # Extract PR uid - uid = int(match.group(1)) - ids.append(uid) - - return ids - - -def fetch_pr_info(pr_ids, auth): - - prs_url = "https://api.github.com/repos/PointCloudLibrary/pcl/pulls/" - pr_info = [] - - sys.stdout.write("Fetching PR Info: {}%".format(0)) - sys.stdout.flush() - - for i, pr_id in enumerate(pr_ids): - - # Fetch GitHub info - response = requests.get(prs_url + str(pr_id), auth=auth) - data = response.json() - - if response.status_code != 200: - print( - "\nError: Failed to fetch PR info. Server reported '" - + data["message"] - + "'", - file=sys.stderr, - ) - exit(code=1) - - d = {"id": pr_id, "title": data["title"], "labels": data["labels"]} - pr_info.append(d) - - # import pdb; pdb.set_trace() - sys.stdout.write( - "\rFetching PR Info: {:0.2f}%".format(100 * (i + 1) / len(pr_ids)) - ) - sys.stdout.flush() - - print("") - return pr_info - - -def extract_version(tag): - """Finds the corresponding version from a provided tag. - If the tag does not correspond to a suitable version tag, the original tag - is returned - """ - version_re = re.compile("pcl-\S+") - res = version_re.fullmatch(tag) - - # Not a usual version tag - if not res: - return tag - - return tag[4:] - - -def generate_text_content(tag, pr_info): - - module_order = ( - None, - "cmake", - "2d", - "common", - "cuda", - "features", - "filters", - "geometry", - "gpu", - "io", - "kdtree", - "keypoints", - "ml", - "octree", - "outofcore", - "people", - "recognition", - "registration", - "sample_consensus", - "search", - "segmentation", - "simulation", - "stereo", - "surface", - "visualization", - "apps", - "docs", - "tutorials", - "examples", - "tests", - "tools", - "ci", - ) - - module_titles = { - None: "Uncategorized", - "2d": "libpcl_2d", - "apps": "PCL Apps", - "cmake": "CMake", - "ci": "CI", - "common": "libpcl_common", - "cuda": "libpcl_cuda", - "docs": "PCL Docs", - "examples": "PCL Examples", - "features": "libpcl_features", - "filters": "libpcl_filters", - "gpu": "libpcl_gpu", - "io": "libpcl_io", - "kdtree": "libpcl_kdtree", - "keypoints": "libpcl_keypoints", - "ml": "libpcl_ml", - "octree": "libpcl_octree", - "outofcore": "libpcl_outofcore", - "people": "libpcl_people", - "recognition": "libpcl_recognition", - "registration": "libpcl_registration", - "sample_consensus": "libpcl_sample_consensus", - "search": "libpcl_search", - "segmentation": "libpcl_segmentation", - "simulation": "libpcl_simulation", - "stereo": "libpcl_stereo", - "surface": "libpcl_surface", - "tests": "PCL Tests", - "tools": "PCL Tools", - "tutorials": "PCL Tutorials", - "visualization": "libpcl_visualization", - } - - changes_order = ("new-feature", "deprecation", "removal", "behavior", "api", "abi") - - changes_titles = { - "new-feature": "New Features", - "deprecation": "Deprecated", - "removal": "Removed", - "behavior": "Behavioral changes", - "api": "API changes", - "abi": "ABI changes", - } - - changes_description = { - "new-feature": "Newly added functionalities.", - "deprecation": "Deprecated code scheduled to be removed after two minor releases.", - "removal": "Removal of deprecated code.", - "behavior": "Changes in the expected default behavior.", - "api": "Changes to the API which didn't went through the proper deprecation and removal cycle.", - "abi": "Changes that cause ABI incompatibility but are still API compatible.", - } - - changes_labels = { - "breaks API": "api", - "breaks ABI": "abi", - "behavior": "behavior", - "deprecation": "deprecation", - "removal": "removal", - } - - # change_log content - clog = [] - - # Infer version from tag - version = extract_version(tag) - - # Find the commit date for writting the Title - cmd = ("git log -1 --format=%ai " + tag).split() - output = subprocess.run(cmd, cwd=FOLDER, stdout=subprocess.PIPE) - date = output.stdout.split()[0].decode() - tokens = date.split("-") - clog += [ - "## *= " - + version - + " (" - + tokens[2] - + "." - + tokens[1] - + "." - + tokens[0] - + ") =*" - ] - - # Map each PR into the approriate module and changes - modules = defaultdict(list) - changes = defaultdict(list) - module_re = re.compile("module: \S+") - changes_re = re.compile("changes: ") - feature_re = re.compile("new feature") - - for pr in pr_info: - - pr["modules"] = [] - pr["changes"] = [] - - for label in pr["labels"]: - if module_re.fullmatch(label["name"]): - module = label["name"][8:] - pr["modules"].append(module) - modules[module].append(pr) - - elif changes_re.match(label["name"]): - key = changes_labels[label["name"][9:]] - pr["changes"].append(key) - changes[key].append(pr) - - elif feature_re.fullmatch(label["name"]): - pr["changes"].append("new-feature") - changes["new-feature"].append(pr) - - # No labels defaults to section None - if not pr["modules"]: - modules[None].append(pr) - continue - - # Generate Changes Summary - for key in changes_order: - - # Skip empty sections - if not changes[key]: - continue - - clog += ["\n### `" + changes_titles[key] + ":`\n"] - - clog += ["*" + changes_description[key] + "*\n"] - - for pr in changes[key]: - prefix = "".join(["[" + k + "]" for k in pr["modules"]]) - if prefix: - prefix = "**" + prefix + "** " - clog += [ - "* " - + prefix - + pr["title"] - + " [[#" - + str(pr["id"]) - + "]]" - + "(https://github.com/PointCloudLibrary/pcl/pull/" - + str(pr["id"]) - + ")" - ] - - # Traverse Modules and generate each section's content - clog += ["\n### `Modules:`"] - for key in module_order: - - # Skip empty sections - if not modules[key]: - continue - - # if key: - clog += ["\n#### `" + module_titles[key] + ":`\n"] - - for pr in modules[key]: - prefix = "".join(["[" + k + "]" for k in pr["changes"]]) - if prefix: - prefix = "**" + prefix + "** " - clog += [ - "* " - + prefix - + pr["title"] - + " [[#" - + str(pr["id"]) - + "]]" - + "(https://github.com/PointCloudLibrary/pcl/pull/" - + str(pr["id"]) - + ")" - ] - - return clog - - -def parse_arguments(): - - parser = ArgumentParser( - description="Generate a change log between two " - "revisions.\n\nCheck https://github.com/PointCloudLibrary/pcl/wiki/Preparing-Releases#creating-the-change-log " - "for some additional examples on how to use the tool." - ) - parser.add_argument( - "start", - help="The start (exclusive) " "revision/commit/tag to generate the log.", - ) - parser.add_argument( - "end", - nargs="?", - default="HEAD", - help="The end " - "(inclusive) revision/commit/tag to generate the log. " - "(Defaults to HEAD)", - ) - parser.add_argument( - "--username", - help="GitHub Account user name. If " - "specified it will perform requests with the provided credentials. " - "This is often required in order to overcome GitHub API's request " - "limits.", - ) - meg = parser.add_mutually_exclusive_group() - meg.add_argument( - "--cache", - nargs="?", - const="pr_info.json", - metavar="FILE", - help="Caches the PR info extracted from GitHub into a JSON file. " - "(Defaults to 'pr_info.json')", - ) - meg.add_argument( - "--from-cache", - nargs="?", - const="pr_info.json", - metavar="FILE", - help="Uses a previously generated PR info JSON cache " - "file to generate the change log. (Defaults to 'pr_info.json')", - ) - - # Parse arguments - args = parser.parse_args() - args.auth = None - - if args.username: - password = getpass.getpass(prompt="Password for " + args.username + ": ") - args.auth = (args.username, password) - - return args - - -## -## 'main' -## - -FOLDER = find_pcl_folder() - -# Parse arguments -args = parse_arguments() - -pr_info = None -if not args.from_cache: - - # Find all PRs since tag - prs = find_pr_list(start=args.start, end=args.end) - - # Generate pr objects with title, labels from ids - pr_info = fetch_pr_info(prs, auth=args.auth) - if args.cache: - with open(args.cache, "w") as fp: - d = {"start": args.start, "end": args.end, "pr_info": pr_info} - fp.write(json.dumps(d)) -else: - # Load previously cached info - with open(args.from_cache) as fp: - d = json.loads(fp.read()) - pr_info = d["pr_info"] - args.start = d["start"] - args.end = d["start"] - - -# Generate text content based on changes -clog = generate_text_content(tag=args.end, pr_info=pr_info) -print("\n".join(clog)) diff --git a/2d/include/pcl/2d/convolution.h b/2d/include/pcl/2d/convolution.h index 3ee3c8e1..9b5f827f 100644 --- a/2d/include/pcl/2d/convolution.h +++ b/2d/include/pcl/2d/convolution.h @@ -38,6 +38,7 @@ #pragma once #include +#include #include #include #include diff --git a/2d/include/pcl/2d/edge.h b/2d/include/pcl/2d/edge.h index 504e31e9..d606d912 100644 --- a/2d/include/pcl/2d/edge.h +++ b/2d/include/pcl/2d/edge.h @@ -39,6 +39,7 @@ #include #include +#include #include #include diff --git a/2d/include/pcl/2d/impl/convolution.hpp b/2d/include/pcl/2d/impl/convolution.hpp index 33198406..22794b33 100644 --- a/2d/include/pcl/2d/impl/convolution.hpp +++ b/2d/include/pcl/2d/impl/convolution.hpp @@ -35,13 +35,15 @@ * */ -#ifndef PCL_2D_CONVOLUTION_IMPL_HPP -#define PCL_2D_CONVOLUTION_IMPL_HPP +#pragma once + +#include + +namespace pcl { -////////////////////////////////////////////////////////////////////////////// template void -pcl::Convolution::filter(pcl::PointCloud& output) +Convolution::filter(pcl::PointCloud& output) { int input_row = 0; int input_col = 0; @@ -133,5 +135,4 @@ pcl::Convolution::filter(pcl::PointCloud& output) } } // switch } - -#endif +} // namespace pcl diff --git a/2d/include/pcl/2d/impl/edge.hpp b/2d/include/pcl/2d/impl/edge.hpp index ded9f698..6bbe9ef6 100644 --- a/2d/include/pcl/2d/impl/edge.hpp +++ b/2d/include/pcl/2d/impl/edge.hpp @@ -35,16 +35,17 @@ * */ -#ifndef PCL_2D_EDGE_IMPL_HPP -#define PCL_2D_EDGE_IMPL_HPP +#pragma once #include +#include #include // rad2deg() -////////////////////////////////////////////////////////////////////////////// +namespace pcl { + template void -pcl::Edge::detectEdgeSobel(pcl::PointCloud& output) +Edge::detectEdgeSobel(pcl::PointCloud& output) { convolution_.setInputCloud(input_); pcl::PointCloud::Ptr kernel_x(new pcl::PointCloud); @@ -79,10 +80,9 @@ pcl::Edge::detectEdgeSobel(pcl::PointCloud& outp } } -////////////////////////////////////////////////////////////////////////////// template void -pcl::Edge::sobelMagnitudeDirection( +Edge::sobelMagnitudeDirection( const pcl::PointCloud& input_x, const pcl::PointCloud& input_y, pcl::PointCloud& output) @@ -121,10 +121,9 @@ pcl::Edge::sobelMagnitudeDirection( } } -////////////////////////////////////////////////////////////////////////////// template void -pcl::Edge::detectEdgePrewitt(pcl::PointCloud& output) +Edge::detectEdgePrewitt(pcl::PointCloud& output) { convolution_.setInputCloud(input_); @@ -160,10 +159,9 @@ pcl::Edge::detectEdgePrewitt(pcl::PointCloud& ou } } -////////////////////////////////////////////////////////////////////////////// template void -pcl::Edge::detectEdgeRoberts(pcl::PointCloud& output) +Edge::detectEdgeRoberts(pcl::PointCloud& output) { convolution_.setInputCloud(input_); @@ -199,10 +197,9 @@ pcl::Edge::detectEdgeRoberts(pcl::PointCloud& ou } } -////////////////////////////////////////////////////////////////////////////// template void -pcl::Edge::cannyTraceEdge( +Edge::cannyTraceEdge( int rowOffset, int colOffset, int row, int col, pcl::PointCloud& maxima) { int newRow = row + rowOffset; @@ -226,10 +223,9 @@ pcl::Edge::cannyTraceEdge( } } -////////////////////////////////////////////////////////////////////////////// template void -pcl::Edge::discretizeAngles(pcl::PointCloud& thet) +Edge::discretizeAngles(pcl::PointCloud& thet) { const int height = thet.height; const int width = thet.width; @@ -253,10 +249,9 @@ pcl::Edge::discretizeAngles(pcl::PointCloud& the } } -////////////////////////////////////////////////////////////////////////////// template void -pcl::Edge::suppressNonMaxima( +Edge::suppressNonMaxima( const pcl::PointCloud& edges, pcl::PointCloud& maxima, float tLow) @@ -312,10 +307,9 @@ pcl::Edge::suppressNonMaxima( } } -////////////////////////////////////////////////////////////////////////////// template void -pcl::Edge::detectEdgeCanny(pcl::PointCloud& output) +Edge::detectEdgeCanny(pcl::PointCloud& output) { float tHigh = hysteresis_threshold_high_; float tLow = hysteresis_threshold_low_; @@ -377,12 +371,11 @@ pcl::Edge::detectEdgeCanny(pcl::PointCloud& outp } } -////////////////////////////////////////////////////////////////////////////// template void -pcl::Edge::canny(const pcl::PointCloud& input_x, - const pcl::PointCloud& input_y, - pcl::PointCloud& output) +Edge::canny(const pcl::PointCloud& input_x, + const pcl::PointCloud& input_y, + pcl::PointCloud& output) { float tHigh = hysteresis_threshold_high_; float tLow = hysteresis_threshold_low_; @@ -452,12 +445,11 @@ pcl::Edge::canny(const pcl::PointCloud& input_x, } } -////////////////////////////////////////////////////////////////////////////// template void -pcl::Edge::detectEdgeLoG(const float kernel_sigma, - const float kernel_size, - pcl::PointCloud& output) +Edge::detectEdgeLoG(const float kernel_sigma, + const float kernel_size, + pcl::PointCloud& output) { convolution_.setInputCloud(input_); @@ -470,4 +462,4 @@ pcl::Edge::detectEdgeLoG(const float kernel_sigma, convolution_.filter(output); } -#endif +} // namespace pcl diff --git a/2d/include/pcl/2d/impl/kernel.hpp b/2d/include/pcl/2d/impl/kernel.hpp index d85de7e6..0be57ff4 100644 --- a/2d/include/pcl/2d/impl/kernel.hpp +++ b/2d/include/pcl/2d/impl/kernel.hpp @@ -35,13 +35,15 @@ * */ -#ifndef PCL_2D_KERNEL_IMPL_HPP -#define PCL_2D_KERNEL_IMPL_HPP +#pragma once + +#include + +namespace pcl { -////////////////////////////////////////////////////////////////////////////// template void -pcl::kernel::fetchKernel(pcl::PointCloud& kernel) +kernel::fetchKernel(pcl::PointCloud& kernel) { switch (kernel_type_) { case SOBEL_X: @@ -89,10 +91,9 @@ pcl::kernel::fetchKernel(pcl::PointCloud& kernel) } } -////////////////////////////////////////////////////////////////////////////// template void -pcl::kernel::gaussianKernel(pcl::PointCloud& kernel) +kernel::gaussianKernel(pcl::PointCloud& kernel) { float sum = 0; kernel.resize(kernel_size_ * kernel_size_); @@ -116,13 +117,11 @@ pcl::kernel::gaussianKernel(pcl::PointCloud& kernel) kernel[i].intensity /= sum; } -////////////////////////////////////////////////////////////////////////////// template void -pcl::kernel::loGKernel(pcl::PointCloud& kernel) +kernel::loGKernel(pcl::PointCloud& kernel) { float sum = 0; - float temp = 0; kernel.resize(kernel_size_ * kernel_size_); kernel.height = kernel_size_; kernel.width = kernel_size_; @@ -133,7 +132,7 @@ pcl::kernel::loGKernel(pcl::PointCloud& kernel) for (int j = 0; j < kernel_size_; j++) { int iks = (i - kernel_size_ / 2); int jks = (j - kernel_size_ / 2); - temp = float(double(iks * iks + jks * jks) / sigma_sqr); + float temp = float(double(iks * iks + jks * jks) / sigma_sqr); kernel(j, i).intensity = (1.0f - temp) * std::exp(-temp); sum += kernel(j, i).intensity; } @@ -144,10 +143,9 @@ pcl::kernel::loGKernel(pcl::PointCloud& kernel) kernel[i].intensity /= sum; } -////////////////////////////////////////////////////////////////////////////// template void -pcl::kernel::sobelKernelX(pcl::PointCloud& kernel) +kernel::sobelKernelX(pcl::PointCloud& kernel) { kernel.resize(9); kernel.height = 3; @@ -163,10 +161,9 @@ pcl::kernel::sobelKernelX(pcl::PointCloud& kernel) kernel(2, 2).intensity = 1; } -////////////////////////////////////////////////////////////////////////////// template void -pcl::kernel::prewittKernelX(pcl::PointCloud& kernel) +kernel::prewittKernelX(pcl::PointCloud& kernel) { kernel.resize(9); kernel.height = 3; @@ -182,10 +179,9 @@ pcl::kernel::prewittKernelX(pcl::PointCloud& kernel) kernel(2, 2).intensity = 1; } -////////////////////////////////////////////////////////////////////////////// template void -pcl::kernel::robertsKernelX(pcl::PointCloud& kernel) +kernel::robertsKernelX(pcl::PointCloud& kernel) { kernel.resize(4); kernel.height = 2; @@ -196,10 +192,9 @@ pcl::kernel::robertsKernelX(pcl::PointCloud& kernel) kernel(1, 1).intensity = -1; } -////////////////////////////////////////////////////////////////////////////// template void -pcl::kernel::sobelKernelY(pcl::PointCloud& kernel) +kernel::sobelKernelY(pcl::PointCloud& kernel) { kernel.resize(9); kernel.height = 3; @@ -215,10 +210,9 @@ pcl::kernel::sobelKernelY(pcl::PointCloud& kernel) kernel(2, 2).intensity = 1; } -////////////////////////////////////////////////////////////////////////////// template void -pcl::kernel::prewittKernelY(pcl::PointCloud& kernel) +kernel::prewittKernelY(pcl::PointCloud& kernel) { kernel.resize(9); kernel.height = 3; @@ -236,7 +230,7 @@ pcl::kernel::prewittKernelY(pcl::PointCloud& kernel) template void -pcl::kernel::robertsKernelY(pcl::PointCloud& kernel) +kernel::robertsKernelY(pcl::PointCloud& kernel) { kernel.resize(4); kernel.height = 2; @@ -247,10 +241,9 @@ pcl::kernel::robertsKernelY(pcl::PointCloud& kernel) kernel(1, 1).intensity = 0; } -////////////////////////////////////////////////////////////////////////////// template void -pcl::kernel::derivativeXCentralKernel(pcl::PointCloud& kernel) +kernel::derivativeXCentralKernel(pcl::PointCloud& kernel) { kernel.resize(3); kernel.height = 1; @@ -260,10 +253,9 @@ pcl::kernel::derivativeXCentralKernel(pcl::PointCloud& kernel) kernel(2, 0).intensity = 1; } -////////////////////////////////////////////////////////////////////////////// template void -pcl::kernel::derivativeXForwardKernel(pcl::PointCloud& kernel) +kernel::derivativeXForwardKernel(pcl::PointCloud& kernel) { kernel.resize(3); kernel.height = 1; @@ -273,10 +265,9 @@ pcl::kernel::derivativeXForwardKernel(pcl::PointCloud& kernel) kernel(2, 0).intensity = 1; } -////////////////////////////////////////////////////////////////////////////// template void -pcl::kernel::derivativeXBackwardKernel(pcl::PointCloud& kernel) +kernel::derivativeXBackwardKernel(pcl::PointCloud& kernel) { kernel.resize(3); kernel.height = 1; @@ -286,10 +277,9 @@ pcl::kernel::derivativeXBackwardKernel(pcl::PointCloud& kernel) kernel(2, 0).intensity = 0; } -////////////////////////////////////////////////////////////////////////////// template void -pcl::kernel::derivativeYCentralKernel(pcl::PointCloud& kernel) +kernel::derivativeYCentralKernel(pcl::PointCloud& kernel) { kernel.resize(3); kernel.height = 3; @@ -299,10 +289,9 @@ pcl::kernel::derivativeYCentralKernel(pcl::PointCloud& kernel) kernel(0, 2).intensity = 1; } -////////////////////////////////////////////////////////////////////////////// template void -pcl::kernel::derivativeYForwardKernel(pcl::PointCloud& kernel) +kernel::derivativeYForwardKernel(pcl::PointCloud& kernel) { kernel.resize(3); kernel.height = 3; @@ -312,10 +301,9 @@ pcl::kernel::derivativeYForwardKernel(pcl::PointCloud& kernel) kernel(0, 2).intensity = 1; } -////////////////////////////////////////////////////////////////////////////// template void -pcl::kernel::derivativeYBackwardKernel(pcl::PointCloud& kernel) +kernel::derivativeYBackwardKernel(pcl::PointCloud& kernel) { kernel.resize(3); kernel.height = 3; @@ -325,29 +313,25 @@ pcl::kernel::derivativeYBackwardKernel(pcl::PointCloud& kernel) kernel(0, 2).intensity = 0; } -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// template void -pcl::kernel::setKernelType(KERNEL_ENUM kernel_type) +kernel::setKernelType(KERNEL_ENUM kernel_type) { kernel_type_ = kernel_type; } -////////////////////////////////////////////////////////////////////////////// template void -pcl::kernel::setKernelSize(int kernel_size) +kernel::setKernelSize(int kernel_size) { kernel_size_ = kernel_size; } -////////////////////////////////////////////////////////////////////////////// template void -pcl::kernel::setKernelSigma(float kernel_sigma) +kernel::setKernelSigma(float kernel_sigma) { sigma_ = kernel_sigma; } -#endif +} // namespace pcl diff --git a/2d/include/pcl/2d/impl/keypoint.hpp b/2d/include/pcl/2d/impl/keypoint.hpp index fd8235d7..a8959676 100644 --- a/2d/include/pcl/2d/impl/keypoint.hpp +++ b/2d/include/pcl/2d/impl/keypoint.hpp @@ -39,23 +39,25 @@ * Author: somani */ -#ifndef PCL_2D_KEYPOINT_HPP_ -#define PCL_2D_KEYPOINT_HPP_ +#pragma once -#include #include #include +#include // for pcl::Keypoint + +#include -////////////////////////////////////////////////////////////////////////////// +namespace pcl { + +template void -pcl::keypoint::harrisCorner(ImageType& output, - ImageType& input, - const float sigma_d, - const float sigma_i, - const float alpha, - const float thresh) +Keypoint::harrisCorner(ImageType& output, + ImageType& input, + const float sigma_d, + const float sigma_i, + const float alpha, + const float thresh) { - /*creating the gaussian kernels*/ ImageType kernel_d; ImageType kernel_i; @@ -115,12 +117,12 @@ pcl::keypoint::harrisCorner(ImageType& output, } } -////////////////////////////////////////////////////////////////////////////// +template void -pcl::keypoint::hessianBlob(ImageType& output, - ImageType& input, - const float sigma, - bool SCALED) +Keypoint::hessianBlob(ImageType& output, + ImageType& input, + const float sigma, + bool SCALED) { /*creating the gaussian kernels*/ ImageType kernel, cornerness; @@ -175,13 +177,13 @@ pcl::keypoint::hessianBlob(ImageType& output, } } -////////////////////////////////////////////////////////////////////////////// +template void -pcl::keypoint::hessianBlob(ImageType& output, - ImageType& input, - const float start_scale, - const float scaling_factor, - const int num_scales) +Keypoint::hessianBlob(ImageType& output, + ImageType& input, + const float start_scale, + const float scaling_factor, + const int num_scales) { const std::size_t height = input.size(); const std::size_t width = input[0].size(); @@ -193,17 +195,15 @@ pcl::keypoint::hessianBlob(ImageType& output, hessianBlob(cornerness[i], input, scale, false); scale *= scaling_factor; } - bool non_max_flag = false; - float scale_max, local_max; for (std::size_t i = 0; i < height; i++) { for (std::size_t j = 0; j < width; j++) { - scale_max = std::numeric_limits::min(); + float scale_max = std::numeric_limits::min(); /*default output in case of no blob at the current point is 0*/ output[i][j] = 0; for (int k = 0; k < num_scales; k++) { /*check if the current point (k,i,j) is a maximum in the defined search radius*/ - non_max_flag = false; - local_max = cornerness[k][i][j]; + bool non_max_flag = false; + const float local_max = cornerness[k][i][j]; for (int n = -local_search_radius; n <= local_search_radius; n++) { if (n + k < 0 || n + k >= num_scales) continue; @@ -231,7 +231,7 @@ pcl::keypoint::hessianBlob(ImageType& output, scale_max = cornerness[k][i][j]; /*output indicates the scale at which the blob is found at the current * location in the image*/ - output[i][i] = start_scale * pow(scaling_factor, k); + output[i][j] = start_scale * pow(scaling_factor, k); } } } @@ -239,11 +239,11 @@ pcl::keypoint::hessianBlob(ImageType& output, } } -////////////////////////////////////////////////////////////////////////////// +template void -pcl::keypoint::imageElementMultiply(ImageType& output, - ImageType& input1, - ImageType& input2) +Keypoint::imageElementMultiply(ImageType& output, + ImageType& input1, + ImageType& input2) { const std::size_t height = input1.size(); const std::size_t width = input1[0].size(); @@ -255,5 +255,4 @@ pcl::keypoint::imageElementMultiply(ImageType& output, } } } - -#endif // PCL_2D_KEYPOINT_HPP_ +} // namespace pcl diff --git a/2d/include/pcl/2d/impl/morphology.hpp b/2d/include/pcl/2d/impl/morphology.hpp index 4def02de..126c9329 100644 --- a/2d/include/pcl/2d/impl/morphology.hpp +++ b/2d/include/pcl/2d/impl/morphology.hpp @@ -35,14 +35,16 @@ * */ -#ifndef PCL_2D_MORPHOLOGY_HPP_ -#define PCL_2D_MORPHOLOGY_HPP_ +#pragma once + +#include + +namespace pcl { -////////////////////////////////////////////////////////////////////////////// // Assumes input, kernel and output images have 0's and 1's only template void -pcl::Morphology::erosionBinary(pcl::PointCloud& output) +Morphology::erosionBinary(pcl::PointCloud& output) { const int height = input_->height; const int width = input_->width; @@ -90,11 +92,10 @@ pcl::Morphology::erosionBinary(pcl::PointCloud& output) } } -////////////////////////////////////////////////////////////////////////////// // Assumes input, kernel and output images have 0's and 1's only template void -pcl::Morphology::dilationBinary(pcl::PointCloud& output) +Morphology::dilationBinary(pcl::PointCloud& output) { const int height = input_->height; const int width = input_->width; @@ -136,11 +137,10 @@ pcl::Morphology::dilationBinary(pcl::PointCloud& output) } } -////////////////////////////////////////////////////////////////////////////// // Assumes input, kernel and output images have 0's and 1's only template void -pcl::Morphology::openingBinary(pcl::PointCloud& output) +Morphology::openingBinary(pcl::PointCloud& output) { PointCloudInPtr intermediate_output(new PointCloudIn); erosionBinary(*intermediate_output); @@ -148,11 +148,10 @@ pcl::Morphology::openingBinary(pcl::PointCloud& output) dilationBinary(output); } -////////////////////////////////////////////////////////////////////////////// // Assumes input, kernel and output images have 0's and 1's only template void -pcl::Morphology::closingBinary(pcl::PointCloud& output) +Morphology::closingBinary(pcl::PointCloud& output) { PointCloudInPtr intermediate_output(new PointCloudIn); dilationBinary(*intermediate_output); @@ -160,10 +159,9 @@ pcl::Morphology::closingBinary(pcl::PointCloud& output) erosionBinary(output); } -////////////////////////////////////////////////////////////////////////////// template void -pcl::Morphology::erosionGray(pcl::PointCloud& output) +Morphology::erosionGray(pcl::PointCloud& output) { const int height = input_->height; const int width = input_->width; @@ -203,10 +201,9 @@ pcl::Morphology::erosionGray(pcl::PointCloud& output) } } -////////////////////////////////////////////////////////////////////////////// template void -pcl::Morphology::dilationGray(pcl::PointCloud& output) +Morphology::dilationGray(pcl::PointCloud& output) { const int height = input_->height; const int width = input_->width; @@ -247,10 +244,9 @@ pcl::Morphology::dilationGray(pcl::PointCloud& output) } } -////////////////////////////////////////////////////////////////////////////// template void -pcl::Morphology::openingGray(pcl::PointCloud& output) +Morphology::openingGray(pcl::PointCloud& output) { PointCloudInPtr intermediate_output(new PointCloudIn); erosionGray(*intermediate_output); @@ -258,10 +254,9 @@ pcl::Morphology::openingGray(pcl::PointCloud& output) dilationGray(output); } -////////////////////////////////////////////////////////////////////////////// template void -pcl::Morphology::closingGray(pcl::PointCloud& output) +Morphology::closingGray(pcl::PointCloud& output) { PointCloudInPtr intermediate_output(new PointCloudIn); dilationGray(*intermediate_output); @@ -269,12 +264,11 @@ pcl::Morphology::closingGray(pcl::PointCloud& output) erosionGray(output); } -////////////////////////////////////////////////////////////////////////////// template void -pcl::Morphology::subtractionBinary(pcl::PointCloud& output, - const pcl::PointCloud& input1, - const pcl::PointCloud& input2) +Morphology::subtractionBinary(pcl::PointCloud& output, + const pcl::PointCloud& input1, + const pcl::PointCloud& input2) { const int height = (input1.height < input2.height) ? input1.height : input2.height; const int width = (input1.width < input2.width) ? input1.width : input2.width; @@ -290,12 +284,11 @@ pcl::Morphology::subtractionBinary(pcl::PointCloud& output, } } -////////////////////////////////////////////////////////////////////////////// template void -pcl::Morphology::unionBinary(pcl::PointCloud& output, - const pcl::PointCloud& input1, - const pcl::PointCloud& input2) +Morphology::unionBinary(pcl::PointCloud& output, + const pcl::PointCloud& input1, + const pcl::PointCloud& input2) { const int height = (input1.height < input2.height) ? input1.height : input2.height; const int width = (input1.width < input2.width) ? input1.width : input2.width; @@ -311,12 +304,11 @@ pcl::Morphology::unionBinary(pcl::PointCloud& output, } } -////////////////////////////////////////////////////////////////////////////// template void -pcl::Morphology::intersectionBinary(pcl::PointCloud& output, - const pcl::PointCloud& input1, - const pcl::PointCloud& input2) +Morphology::intersectionBinary(pcl::PointCloud& output, + const pcl::PointCloud& input1, + const pcl::PointCloud& input2) { const int height = (input1.height < input2.height) ? input1.height : input2.height; const int width = (input1.width < input2.width) ? input1.width : input2.width; @@ -332,11 +324,10 @@ pcl::Morphology::intersectionBinary(pcl::PointCloud& output, } } -////////////////////////////////////////////////////////////////////////////// template void -pcl::Morphology::structuringElementCircular(pcl::PointCloud& kernel, - const int radius) +Morphology::structuringElementCircular(pcl::PointCloud& kernel, + const int radius) { const int dim = 2 * radius; kernel.height = dim; @@ -353,12 +344,11 @@ pcl::Morphology::structuringElementCircular(pcl::PointCloud& ker } } -////////////////////////////////////////////////////////////////////////////// template void -pcl::Morphology::structuringElementRectangle(pcl::PointCloud& kernel, - const int height, - const int width) +Morphology::structuringElementRectangle(pcl::PointCloud& kernel, + const int height, + const int width) { kernel.height = height; kernel.width = width; @@ -367,13 +357,11 @@ pcl::Morphology::structuringElementRectangle(pcl::PointCloud& ke kernel[i].intensity = 1; } -////////////////////////////////////////////////////////////////////////////// template void -pcl::Morphology::setStructuringElement( - const PointCloudInPtr& structuring_element) +Morphology::setStructuringElement(const PointCloudInPtr& structuring_element) { structuring_element_ = structuring_element; } -#endif // PCL_2D_MORPHOLOGY_HPP_ +} // namespace pcl diff --git a/2d/include/pcl/2d/keypoint.h b/2d/include/pcl/2d/keypoint.h index a7c454d9..ca889e06 100644 --- a/2d/include/pcl/2d/keypoint.h +++ b/2d/include/pcl/2d/keypoint.h @@ -45,10 +45,11 @@ namespace pcl { +template class Keypoint { private: - Edge edge_detection; - Convolution conv_2d; + Edge edge_detection; + Convolution conv_2d; public: Keypoint() {} diff --git a/2d/src/examples.cpp b/2d/src/examples.cpp index 8135ba58..c5851b4e 100644 --- a/2d/src/examples.cpp +++ b/2d/src/examples.cpp @@ -36,13 +36,12 @@ * $Id$ */ -#include - #include #include #include #include #include +#include using namespace pcl; diff --git a/AUTHORS.txt b/AUTHORS.txt index bed9a7b5..d8dfde37 100644 --- a/AUTHORS.txt +++ b/AUTHORS.txt @@ -1,3 +1,2 @@ PCL is a large collaborative effort, and it would not exist without the contributions of several people. Please see https://github.com/PointCloudLibrary/pcl/graphs/contributors for a complete list of developers. - diff --git a/CHANGES.md b/CHANGES.md index af5ec2aa..01a7ad91 100644 --- a/CHANGES.md +++ b/CHANGES.md @@ -1,5 +1,187 @@ # ChangeList +## = 1.11.0 (11.05.2020) = + +Starting with PCL 1.11, PCL uses `std::shared_ptr` and `std::weak_ptr` instead of the +boost smart pointers. The change leverages type aliases included with the 1.10.0 +release. PCL 1.11 also introduces `pcl::index_t` which should be used for the size +of point types instead of `int`, `std::size_t`, etc. EOL for deprecated features +is also explicitly mentioned in the deprecation compile time warnings + +### Notable changes + +**New features** *added to PCL* + +* **[common]** Provide dynamic and static pointer casts in namespace pcl to allow easy migration in future [[#3770](https://github.com/PointCloudLibrary/pcl/pull/3770)] +* **[common]** Add `ignore` function to remove Doxygen warnings for unused arguments [[#3942](https://github.com/PointCloudLibrary/pcl/pull/3942)] +* **[docs]** Generate TODO list [[#3937](https://github.com/PointCloudLibrary/pcl/pull/3937)] +* Force include order via clang-format [[#3909](https://github.com/PointCloudLibrary/pcl/pull/3909)] +* Change PCL smart pointers from `boost` to `std` [[#3750](https://github.com/PointCloudLibrary/pcl/pull/3750)] +* **[ci]** Add pipeline for building PCL's environment docker image [[#3843](https://github.com/PointCloudLibrary/pcl/pull/3843)] + +**Deprecation** *of public APIs, scheduled to be removed after two minor releases* + +* **[common]** Remove `#undef Success` in pcl_macros.h by extracting `PCL_MAKE_ALIGNED_OPERATOR_NEW` into memory.h [[#3654](https://github.com/PointCloudLibrary/pcl/pull/3654)] +* **[common]** Rename `point_traits.h` into `type_traits.h` [[#3698](https://github.com/PointCloudLibrary/pcl/pull/3698)] +* **[filters]** Deprecating functions in non-speclialized Passthrough filter [[#3888](https://github.com/PointCloudLibrary/pcl/pull/3888)] +* **[outofcore][registration]** Homogenize deprecation with PCL_DEPRECATED [[#3925](https://github.com/PointCloudLibrary/pcl/pull/3925)] +* **[cmake][visualization]** Deprecate legacy OpenGL backend of VTK [[#4065](https://github.com/PointCloudLibrary/pcl/pull/4065)] + +**Removal** *of the public APIs deprecated in previous releases* + +* **[io][recognition][tools]** Remove very old deprecated headers [[#3906](https://github.com/PointCloudLibrary/pcl/pull/3906)] +* **[docs]** Remove backup (and defunct) `CMakeLists.txt` [[#3915](https://github.com/PointCloudLibrary/pcl/pull/3915)] +* Remove use of VTK_EXCLUDE_STRSTREAM_HEADERS (unavailable since VTK 6.0.0) [[#3939](https://github.com/PointCloudLibrary/pcl/pull/3939)] + +**Behavior changes** *in classes, apps, or tools* + +* **[io]** Make grabbers move-only using `unique_ptr` [[#3626](https://github.com/PointCloudLibrary/pcl/pull/3626)] + +**API changes** *that did not go through the proper deprecation and removal cycle* + +* **[io]** Make grabbers move-only using `unique_ptr` [[#3626](https://github.com/PointCloudLibrary/pcl/pull/3626)] +* **[common]** Add `pcl::index_t`; move some type declarations from `pcl/pcl_macros.h` to `pcl/types.h` [[#3651](https://github.com/PointCloudLibrary/pcl/pull/3651)] +* **[filters]** Clean up `Filter` and `FilterIndices`, move `indices_`/`input_` from public to protected section [[#3726](https://github.com/PointCloudLibrary/pcl/pull/3726)] +* **[registration]** Better Generalized ICP optimizer gradient check management [[#3854](https://github.com/PointCloudLibrary/pcl/pull/3854)] +* Change PCL smart pointers from `boost` to `std` [[#3750](https://github.com/PointCloudLibrary/pcl/pull/3750)] +* **[registration]** Removing deprecated method `setInputCloud` from public API [[#4026](https://github.com/PointCloudLibrary/pcl/pull/4026)] + +**ABI changes** *that are still API compatible* + +* **[filters]** NormalSpaceSampling - fix bucket assignment, remove use of raw distribution pointer, unit-test rewriting [[#3862](https://github.com/PointCloudLibrary/pcl/pull/3862)] +* **[io]** Add pcl::weak_ptr to have a single-switch move from boost:: to std:: weak pointers [[#3753](https://github.com/PointCloudLibrary/pcl/pull/3753)] + +### Changes grouped by module + +#### CMake: + +* Add a stamp file to build documentation once [[#3819](https://github.com/PointCloudLibrary/pcl/pull/3819)] +* Fix compilation in OSX Catalina with OMP enabled [[#3721](https://github.com/PointCloudLibrary/pcl/pull/3721)] +* Show proper message in CMake config for default-off modules [[#3927](https://github.com/PointCloudLibrary/pcl/pull/3927)] +* **[deprecation]** Deprecate legacy OpenGL backend of VTK [[#4065](https://github.com/PointCloudLibrary/pcl/pull/4065)] + +#### libpcl_2d: + +* variable assigned a value which is never used [[#3857](https://github.com/PointCloudLibrary/pcl/pull/3857)] +* Fix issue with missing templating of `Keypoint`. Fixes coming from clang-doxy [[#3898](https://github.com/PointCloudLibrary/pcl/pull/3898)] + +#### libpcl_common: + +* **[deprecation]** Remove `#undef Success` in pcl_macros.h by extracting `PCL_MAKE_ALIGNED_OPERATOR_NEW` into memory.h [[#3654](https://github.com/PointCloudLibrary/pcl/pull/3654)] +* Fix issues with math defines on mingw-w64. [[#3756](https://github.com/PointCloudLibrary/pcl/pull/3756)] +* **[API break]** Add `pcl::index_t`; move some type declarations from `pcl/pcl_macros.h` to `pcl/types.h` [[#3651](https://github.com/PointCloudLibrary/pcl/pull/3651)] +* **[deprecation]** Rename `point_traits.h` into `type_traits.h` [[#3698](https://github.com/PointCloudLibrary/pcl/pull/3698)] +* Improve `PCL_DEPRECATED` macro to include scheduled removal version [[#3808](https://github.com/PointCloudLibrary/pcl/pull/3808)] +* Fix erroneous PCL version in deprecated message [[#3824](https://github.com/PointCloudLibrary/pcl/pull/3824)] +* Select OpenMP data sharing mode based on specific GCC versions [[#3823](https://github.com/PointCloudLibrary/pcl/pull/3823)] +* Define `PointIndices` based on the global `Indices` type alias [[#3822](https://github.com/PointCloudLibrary/pcl/pull/3822)] +* Fix warning C4067: unexpected tokens following preprocessor directive- expected a newline [[#3871](https://github.com/PointCloudLibrary/pcl/pull/3871)] +* **[new feature]** Provide dynamic and static pointer casts in namespace pcl to allow easy migration in future [[#3770](https://github.com/PointCloudLibrary/pcl/pull/3770)] +* **[new feature]** Add `ignore` function to remove Doxygen warnings for unused arguments [[#3942](https://github.com/PointCloudLibrary/pcl/pull/3942)] +* Fix excessive warnings on MSVC [[#3964](https://github.com/PointCloudLibrary/pcl/pull/3964)] +* Refactoring `PCL_DEPRECATED` macro [[#3945](https://github.com/PointCloudLibrary/pcl/pull/3945)] +* Correcting type mismatch [[#3967](https://github.com/PointCloudLibrary/pcl/pull/3967)] +* Removed empty file [[#4019](https://github.com/PointCloudLibrary/pcl/pull/4019)] + +#### libpcl_filters: + +* Clean up code duplication in `FilterIndices` derived classes [[#3807](https://github.com/PointCloudLibrary/pcl/pull/3807)] +* **[API break]** Clean up `Filter` and `FilterIndices`, move `indices_`/`input_` from public to protected section [[#3726](https://github.com/PointCloudLibrary/pcl/pull/3726)] +* **[deprecation]** Deprecating functions in non-speclialized Passthrough filter [[#3888](https://github.com/PointCloudLibrary/pcl/pull/3888)] +* **[ABI break]** NormalSpaceSampling - fix bucket assignment, remove use of raw distribution pointer, unit-test rewriting [[#3862](https://github.com/PointCloudLibrary/pcl/pull/3862)] +* Optimize VoxelGrid Filter [[#3853](https://github.com/PointCloudLibrary/pcl/pull/3853)] +* Fix error due to multiple declarations of template member function specializations in convolution [[#3971](https://github.com/PointCloudLibrary/pcl/pull/3971)] + +#### libpcl_io: + +* **[ABI break][API break][behavior change]** Make grabbers move-only using `unique_ptr` [[#3626](https://github.com/PointCloudLibrary/pcl/pull/3626)] +* **[removal]** Remove very old deprecated headers [[#3906](https://github.com/PointCloudLibrary/pcl/pull/3906)] +* **[ABI break]** Add pcl::weak_ptr to have a single-switch move from boost:: to std:: weak pointers [[#3753](https://github.com/PointCloudLibrary/pcl/pull/3753)] +* Use pcl::io::raw_read instead of direct call to POSIX function read [[#4062](https://github.com/PointCloudLibrary/pcl/pull/4062)] + +#### libpcl_octree: + +* Fix a memory leak in `OctreeBase::operator=` [[#3787](https://github.com/PointCloudLibrary/pcl/pull/3787)] + +#### libpcl_outofcore: + +* **[deprecation]** Homogenize deprecation with PCL_DEPRECATED [[#3925](https://github.com/PointCloudLibrary/pcl/pull/3925)] + +#### libpcl_people: + +* Missing include on windows [[#3791](https://github.com/PointCloudLibrary/pcl/pull/3791)] + +#### libpcl_recognition: + +* **[removal]** Remove very old deprecated headers [[#3906](https://github.com/PointCloudLibrary/pcl/pull/3906)] + +#### libpcl_registration: + +* **[API break]** Better Generalized ICP optimizer gradient check management [[#3854](https://github.com/PointCloudLibrary/pcl/pull/3854)] +* **[deprecation]** Homogenize deprecation with PCL_DEPRECATED [[#3925](https://github.com/PointCloudLibrary/pcl/pull/3925)] +* **[API break]** Removing deprecated method `setInputCloud` from public API [[#4026](https://github.com/PointCloudLibrary/pcl/pull/4026)] + +#### libpcl_sample_consensus: + +* Better performance, error handling and other improvements in SAC classes [[#3642](https://github.com/PointCloudLibrary/pcl/pull/3642)] +* Use better types for indices: `int` -> `index_t`, `std::vector` ->`Indices` [[#3835](https://github.com/PointCloudLibrary/pcl/pull/3835)] + +#### libpcl_search: + +* Use better types for indices: `int` -> `index_t`, `std::vector` ->`Indices` [[#3840](https://github.com/PointCloudLibrary/pcl/pull/3840)] +* Add include for `pcl::isFinite` for compilation on VS2019 [[#4056](https://github.com/PointCloudLibrary/pcl/pull/4056)] + +#### libpcl_segmentation: + +* Check and warn user about missing normals in `OrganizedMultiPlaneSegmentation` [[#3861](https://github.com/PointCloudLibrary/pcl/pull/3861)] + +#### libpcl_surface: + +* Fix error due to multiple declarations of template member function specializations in Poisson4 [[#3972](https://github.com/PointCloudLibrary/pcl/pull/3972)] +* Reduce time taken in `TextureMapping::sortFacesByCamera` from `O(faces*points)` to `O(faces)` [[#3981](https://github.com/PointCloudLibrary/pcl/pull/3981)] + +#### libpcl_visualization: + +* Minor fix for converting unorganized PointClouds to VTK data [[#3988](https://github.com/PointCloudLibrary/pcl/pull/3988)] +* Fixes #4001 and #3452. [[#4017](https://github.com/PointCloudLibrary/pcl/pull/4017)] +* **[deprecation]** Deprecate legacy OpenGL backend of VTK [[#4065](https://github.com/PointCloudLibrary/pcl/pull/4065)] +* Fix rendering of points. [[#4067](https://github.com/PointCloudLibrary/pcl/pull/4067)] + +#### PCL Docs: + +* Fix Doxygen warnings unrelated to documentation -- Part 1 [[#3701](https://github.com/PointCloudLibrary/pcl/pull/3701)] +* update automatic code formatting info to clang-format [[#3845](https://github.com/PointCloudLibrary/pcl/pull/3845)] +* replace formula with math [[#3846](https://github.com/PointCloudLibrary/pcl/pull/3846)] +* Fix PCL_DEPRECATED usage in doxygen for a proper Deprecation List in documentation [[#3905](https://github.com/PointCloudLibrary/pcl/pull/3905)] +* **[removal]** Remove backup (and defunct) `CMakeLists.txt` [[#3915](https://github.com/PointCloudLibrary/pcl/pull/3915)] +* **[new feature]** Generate TODO list [[#3937](https://github.com/PointCloudLibrary/pcl/pull/3937)] +* Improve output to match the text in tutorial by adding a newline [[#4029](https://github.com/PointCloudLibrary/pcl/pull/4029)] +* Fix typo in region growing tutorial [[#4052](https://github.com/PointCloudLibrary/pcl/pull/4052)] +* Add information regarding header include order [[#4020](https://github.com/PointCloudLibrary/pcl/pull/4020)] + +#### PCL Tutorials: + +* Add a unit test for ICP and modernize tutorial code [[#3628](https://github.com/PointCloudLibrary/pcl/pull/3628)] + +#### PCL Examples: + +* Remove unnecessary includes in examples [[#4071](https://github.com/PointCloudLibrary/pcl/pull/4071)] + +#### PCL Tools: + +* **[removal]** Remove very old deprecated headers [[#3906](https://github.com/PointCloudLibrary/pcl/pull/3906)] + +#### CI: + +* Temporary fix for skipping of certain tests [[#3789](https://github.com/PointCloudLibrary/pcl/pull/3789)] +* Simplify Ubuntu CI using matrix strategy [[#3783](https://github.com/PointCloudLibrary/pcl/pull/3783)] +* Strip leading tag in commit message from changelog [[#3847](https://github.com/PointCloudLibrary/pcl/pull/3847)] +* Shift to clang-format-10 to resolve bug in clang-format-{7-9} [[#3895](https://github.com/PointCloudLibrary/pcl/pull/3895)] +* **[new feature]** Add pipeline for building PCL's environment docker image [[#3843](https://github.com/PointCloudLibrary/pcl/pull/3843)] +* Improve docker ci for other PRs [[#4051](https://github.com/PointCloudLibrary/pcl/pull/4051)] +* Remove unused variables from exceptions [[#4064](https://github.com/PointCloudLibrary/pcl/pull/4064)] +* Removing hardcoded version number from tutorial CI [[#4058](https://github.com/PointCloudLibrary/pcl/pull/4058)] + ## *= 1.10.1 (18.03.2020) =* ### Notable changes diff --git a/CMakeLists.txt b/CMakeLists.txt index f0bc2396..aa59b760 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.1) +project(PCL VERSION 1.11.0) string(TOLOWER ${PROJECT_NAME} PROJECT_NAME_LOWER) ### ---[ Find universal dependencies @@ -42,29 +42,9 @@ if(WIN32 AND NOT MINGW) set(CMAKE_MINSIZEREL_POSTFIX "s" CACHE STRING "Add postfix to target for MinSizeRel build") endif() -# ---[ special maintainer mode -set(CMAKE_CXX_FLAGS_MAINTAINER "-pedantic -Wno-variadic-macros -Weffc++ -Wno-long-long" CACHE STRING - "Flags used by the C++ compiler during maintainer builds." - FORCE) -set(CMAKE_C_FLAGS_MAINTAINER "-pedantic -Wno-variadic-macros -Weffc++ -Wno-long-long" CACHE STRING - "Flags used by the C compiler during maintainer builds." - FORCE) -set(CMAKE_EXE_LINKER_FLAGS_MAINTAINER - "-Wl,--warn-unresolved-symbols,--warn-once" CACHE STRING - "Flags used for linking binaries during maintainer builds." - FORCE) -set(CMAKE_SHARED_LINKER_FLAGS_MAINTAINER - "-Wl,--warn-unresolved-symbols,--warn-once" CACHE STRING - "Flags used by the shared libraries linker during maintainer builds." - FORCE) -mark_as_advanced( - CMAKE_CXX_FLAGS_MAINTAINER - CMAKE_C_FLAGS_MAINTAINER - CMAKE_EXE_LINKER_FLAGS_MAINTAINER - CMAKE_SHARED_LINKER_FLAGS_MAINTAINER) # Update the documentation string of CMAKE_BUILD_TYPE for GUIs set(CMAKE_BUILD_TYPE "${CMAKE_BUILD_TYPE}" CACHE STRING - "Choose the type of build, options are: None Debug Release RelWithDebInfo MinSizeRel Maintainer." + "Choose the type of build, options are: None Debug Release RelWithDebInfo MinSizeRel." FORCE) # Compiler identification @@ -274,11 +254,19 @@ set(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/${LIB_INSTALL_DIR}") endif() ### ---[ Find universal dependencies -find_package(OpenMP) -if(OPENMP_FOUND) + +# OpenMP (optional) +find_package(OpenMP COMPONENTS C CXX) +if(OpenMP_FOUND) set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") - message(STATUS "Found OpenMP") + if(${CMAKE_VERSION} VERSION_LESS "3.7") + message(STATUS "Found OpenMP") + else() + # We could use OpenMP_CXX_VERSION starting from CMake 3.9, but this value is only available on first run of CMake (see https://gitlab.kitware.com/cmake/cmake/issues/19150), + # so we use always OpenMP_CXX_SPEC_DATE, which is available since CMake 3.7. + message(STATUS "Found OpenMP, spec date ${OpenMP_CXX_SPEC_DATE}") + endif() if(MSVC) if(MSVC_VERSION EQUAL 1900) set(OPENMP_DLL VCOMP140) @@ -436,6 +424,9 @@ if(WITH_VTK AND NOT ANDROID) endif() if(${VTK_RENDERING_BACKEND} STREQUAL "OpenGL") set(VTK_RENDERING_BACKEND_OPENGL_VERSION "1") + message(DEPRECATION "The rendering backend OpenGL is deprecated and not available anymore since VTK 8.2." + "Please switch to the OpenGL2 backend instead, which is available since VTK 6.2." + "Support of the deprecated backend will be dropped with PCL 1.13.") elseif(${VTK_RENDERING_BACKEND} STREQUAL "OpenGL2") set(VTK_RENDERING_BACKEND_OPENGL_VERSION "2") endif() @@ -472,9 +463,6 @@ configure_file("${pcl_config_h_in}" "${pcl_config_h}") PCL_ADD_INCLUDES(common "" "${pcl_config_h}") include_directories("${CMAKE_CURRENT_BINARY_DIR}/include") -### ---[ Add the libraries subdirectories -include("${PCL_SOURCE_DIR}/cmake/pcl_targets.cmake") - collect_subproject_directory_names("${PCL_SOURCE_DIR}" "CMakeLists.txt" PCL_MODULES_NAMES PCL_MODULES_DIRS doc) set(PCL_MODULES_NAMES_UNSORTED ${PCL_MODULES_NAMES}) topological_sort(PCL_MODULES_NAMES PCL_ _DEPENDS) diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index 0004aa5f..16e64b7a 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -21,7 +21,7 @@ restrictions: * [Stack Overflow](https://stackoverflow.com/questions/tagged/point-cloud-library) for Q&A as well as support for troubleshooting, installation and debugging. Do remember to tag your questions with the tag `point-cloud-library`. - * [Gitter channel](https://gitter.im/PointCloudLibrary/pcl) for live chat with + * [Discord Server](https://discord.gg/JFFMAXS) for live chat with other members of the PCL community and casual discussions - -getChunk("site-footer"); -echo $chunkOutput; -?> -{% endblock %} - - diff --git a/doc/advanced/content/conf.py b/doc/advanced/content/conf.py index 22a3803d..2753d7ee 100644 --- a/doc/advanced/content/conf.py +++ b/doc/advanced/content/conf.py @@ -1,15 +1,13 @@ # All configuration values have a default; values that are commented out # serve to show the default. -import sys, os - # -- General configuration ----------------------------------------------------- # Add any Sphinx extension module names here, as strings. They can be extensions # coming with Sphinx (named 'sphinx.ext.*') or your custom ones. extensions = ['sphinx.ext.imgmath', 'sphinxcontrib.doxylink.doxylink'] # Add any paths that contain templates here, relative to this directory. -templates_path = ['_templates'] +# templates_path = ['_templates'] # The suffix of source filenames. source_suffix = '.rst' @@ -18,7 +16,7 @@ source_suffix = '.rst' master_doc = 'index' # General information about the project. -project = u'PCL' +project = u'Point Cloud Library' copyright = '' # The version info for the project you're documenting, acts as replacement for @@ -71,7 +69,7 @@ pygments_style = 'sphinx' # The theme to use for HTML and HTML Help pages. Major themes that come with # Sphinx are currently 'default' and 'sphinxdoc'. -html_theme = 'sphinxdoc' +html_theme = 'sphinx_rtd_theme' # Theme options are theme-specific and customize the look and feel of a theme # further. For a list of options available for each theme, see the @@ -106,7 +104,7 @@ html_static_path = ['_static'] html_use_modindex = False # If false, no index is generated. -html_use_index = False +html_use_index = True # If true, the index is split into individual pages for each letter. html_split_index = False @@ -121,13 +119,8 @@ html_show_sourcelink = False # If nonempty, this is the file name suffix for HTML files (e.g. ".xhtml"). html_file_suffix = '.html' - -html_sidebars = { - '**': [], - 'using/windows': [], -} -html_show_copyright = False -html_show_sphinx = False +html_show_copyright = True +html_show_sphinx = True html_add_permalinks = None needs_sphinx = '1.0' file_insertion_enabled = True diff --git a/doc/advanced/content/pcl_style_guide.rst b/doc/advanced/content/pcl_style_guide.rst index 325fe48a..aafbe486 100644 --- a/doc/advanced/content/pcl_style_guide.rst +++ b/doc/advanced/content/pcl_style_guide.rst @@ -167,27 +167,20 @@ variant of the GNU style formatting. 2.1. Namespaces ^^^^^^^^^^^^^^^ -In a header file, the contets of a namespace should be indented, e.g.: +In both header and implementation files, namespaces are to be explicitly +declared, and their contents should not be indented, like clang-format +enforces in the Formatting CI job, e.g.: .. code-block:: cpp namespace pcl { - class Foo - { - ... - }; - } - -In an implementation file, the namespace must be added to each individual -method or function definition, e.g.: - -.. code-block:: cpp - void - pcl::Foo::bar () + class Foo { ... + }; + } @@ -281,19 +274,6 @@ function/method, e.g.: int exampleMethod (int example_arg); -If multiple namespaces are declared within header files, always use **2 -spaces** to indent them, e.g.: - -.. code-block:: cpp - - namespace foo - { - namespace bar - { - void - method (int my_var); - } - } Class and struct members are indented by **2 spaces**. Access qualifiers (public, private and protected) are put at the indentation level of the class body and members affected by these qualifiers are indented by one more level, i.e. 2 spaces. E.g.: @@ -302,79 +282,98 @@ indentation level of the class body and members affected by these qualifiers are namespace foo { - class Bar - { - int i; - public: - int j; - protected: - void - baz (); - } + + class Bar + { + int i; + public: + int j; + protected: + void + baz (); + }; } 2.6. Automatic code formatting ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -The following set of rules can be automatically used by various different IDEs, -editors, etc. +We currently use clang-format-10 as the tool for auto-formatting our C++ code. +Please note that different versions of clang-format can result in slightly different outputs. -2.6.1. Emacs -"""""""""""" +The style rules mentioned in this document are enforced via `PCL's .clang-format file +`_. +The style files which were previously distributed should now be considered deprecated. -You can use the following `PCL C/C++ style file -`_, -download it to some known location and then: +For the integration of clang-format with various text editors and IDE's, refer to this `page +`_. -* open .emacs -* add the following before any C/C++ custom hooks +Details about the style options used can be found `here +`_. -:: +2.6.1. Script usage +""""""""""""""""""" - (load-file "/location/to/pcl-c-style.el") - (add-hook 'c-mode-common-hook 'pcl-set-c-style) +PCL also creates a build target 'format' to format the whitelisted directories using clang-format. -2.6.2. Uncrustify -""""""""""""""""" +Command line usage: -You can find a semi-finished config for `Uncrustify `_ `here -`_ +.. code-block:: shell -2.6.3 Eclipse -""""""""""""" + $ make format -| You can find a PCL code style file for Eclipse `on GitHub `_. -| To add the new formatting style go to: Windows > Preferences > C/C++ > Code Style > Formatter -| To format portion of codes, select the code and press Ctrl + Shift + F. -| If you want to format the whole code in your project go to the tree and right click on the project: Source > Format. +2.7. Includes +^^^^^^^^^^^^^ -Note that the Eclipse formatter style is configured to wrap all arguments in a function, feel free to re-arange the arguments if you feel the need; for example, -this improves readability: +For consistent usage, headers should be included in the following order with alphabetical grouping ensured: -.. code-block:: cpp +1. PCL headers - int - displayPoint (float x, float y, float z, - float r, float g, float b - ); + i. All modular PCL includes, except main includes of common module. + + Examples: -This eclipse formatter fails to add a space before brackets when using PCL macros: + .. code-block:: cpp -.. code-block:: cpp + #include + #include + #include - PCL_ERROR("Text\n"); + #. The main PCL includes of common module. These are the header files in the ``pcl/common/include/pcl/`` directory. + + Examples: -should be + .. code-block:: cpp -.. code-block:: cpp + #include + #include + #include - PCL_ERROR ("Text\n"); +2. Major 3rd-Party components of tests and modules -.. note:: + i. gtest + #. boost + #. Eigen + #. flann +3. Major 3rd-Party components of apps + + i. Qt + #. ui-files + #. vtk +4. Minor 3rd-Party components + + i. librealsense + #. ros/message_filters + #. opencv/opencv2 + #. tide + #. thrust + #. OpenGL, GL & GLUT +5. C++ standard library headers (alphabetical) +6. Others + +This style can also be enforced via clang-format. For usage instructions, refer `2.6. Automatic code formatting`_. - This style sheet is not perfect, please mention errors on the user mailing list and feel free to patch! 3. Structuring ============== @@ -410,3 +409,18 @@ For the compute, filter, segment, etc. type methods the following rules apply: size. * The output arguments will always be passed by reference. +3.3. Object declaration +^^^^^^^^^^^^^^^^^^^^^^^ + +3.3.1 Use of auto +""""""""""""""""" +* For Iterators auto must be used as much as possible +* In all the other cases auto can be used at the author's discretion +* Use const auto references by default in range loops. Drop the const if the item needs to be modified. + +3.3.2 Type qualifiers of variables +"""""""""""""""""""""""""""""""""" +* Declare variables const when they don't need to be modified. +* Use const references whenever you don't need a copy of the variable. +* Use of unsigned variables if the value is sure to not go negative by + use and by definition of the variable diff --git a/doc/doxygen/.gitignore b/doc/doxygen/.gitignore new file mode 100644 index 00000000..695b5edf --- /dev/null +++ b/doc/doxygen/.gitignore @@ -0,0 +1 @@ +pcl.tag diff --git a/doc/doxygen/CMakeLists.txt b/doc/doxygen/CMakeLists.txt index 831bbe8e..c2eff7b0 100644 --- a/doc/doxygen/CMakeLists.txt +++ b/doc/doxygen/CMakeLists.txt @@ -37,11 +37,28 @@ endif() set(STRIPPED_HEADERS "${PCL_SOURCE_DIR}/${PCL_MODULES_NAMES}/include") string(REPLACE ";" "/include \\\n ${PCL_SOURCE_DIR}/" STRIPPED_HEADERS "${STRIPPED_HEADERS}") -set(DOC_SOURCE_DIR "\"${PCL_SOURCE_DIR}\"\\") +set(DOC_SOURCE_DIR "${PCL_SOURCE_DIR}") file(MAKE_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/html") set(doxyfile "${CMAKE_CURRENT_BINARY_DIR}/doxyfile") configure_file("${CMAKE_CURRENT_SOURCE_DIR}/doxyfile.in" "${doxyfile}") -add_custom_target(doc ALL "${DOXYGEN_EXECUTABLE}" "${doxyfile}") + +# The following process to use a stamp file is customized from the CMake 3.16.5 `doxygen_add_docs` command +set(STAMP_FILE "${CMAKE_CURRENT_BINARY_DIR}/doc.stamp") +add_custom_command( + VERBATIM + OUTPUT ${STAMP_FILE} + COMMAND ${CMAKE_COMMAND} -E make_directory ${CMAKE_CURRENT_BINARY_DIR} + COMMAND "${DOXYGEN_EXECUTABLE}" "${doxyfile}" + COMMAND ${CMAKE_COMMAND} -E touch ${STAMP_FILE} + WORKING_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}" + DEPENDS "${doxyfile}" ${DOC_SOURCE_DIR} + COMMENT "Generate stamp file for target doc" +) +add_custom_target(doc ALL + DEPENDS ${STAMP_FILE} + SOURCES ${DOC_SOURCE_DIR} +) + set_target_properties(doc PROPERTIES FOLDER "Documentation (Doxygen)") if(DOCUMENTATION_HTML_HELP STREQUAL YES) install(DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/html" diff --git a/doc/doxygen/doxyfile.in b/doc/doxygen/doxyfile.in index 1f876f3f..fb8e54bd 100644 --- a/doc/doxygen/doxyfile.in +++ b/doc/doxygen/doxyfile.in @@ -8,6 +8,7 @@ PROJECT_BRIEF = PROJECT_LOGO = OUTPUT_DIRECTORY = "@CMAKE_CURRENT_BINARY_DIR@" CREATE_SUBDIRS = NO +ALLOW_UNICODE_NAMES = NO OUTPUT_LANGUAGE = English BRIEF_MEMBER_DESC = YES REPEAT_BRIEF = YES @@ -42,11 +43,14 @@ OPTIMIZE_FOR_FORTRAN = NO OPTIMIZE_OUTPUT_VHDL = NO EXTENSION_MAPPING = MARKDOWN_SUPPORT = YES +TOC_INCLUDE_HEADINGS = 5 +AUTOLINK_SUPPORT = YES BUILTIN_STL_SUPPORT = NO CPP_CLI_SUPPORT = NO SIP_SUPPORT = NO IDL_PROPERTY_SUPPORT = YES DISTRIBUTE_GROUP_DOC = NO +GROUP_NESTED_COMPOUNDS = NO SUBGROUPING = YES INLINE_GROUPED_CLASSES = NO INLINE_SIMPLE_STRUCTS = NO @@ -70,7 +74,9 @@ HIDE_IN_BODY_DOCS = NO INTERNAL_DOCS = NO CASE_SENSE_NAMES = NO HIDE_SCOPE_NAMES = NO +HIDE_COMPOUND_REFERENCE= NO SHOW_INCLUDE_FILES = YES +SHOW_GROUPED_MEMB_INC = YES FORCE_LOCAL_INCLUDES = NO INLINE_INFO = YES SORT_MEMBER_DOCS = YES @@ -79,10 +85,10 @@ SORT_MEMBERS_CTORS_1ST = NO SORT_GROUP_NAMES = NO SORT_BY_SCOPE_NAME = NO STRICT_PROTO_MATCHING = NO -GENERATE_TODOLIST = NO +GENERATE_TODOLIST = YES GENERATE_TESTLIST = NO GENERATE_BUGLIST = NO -GENERATE_DEPRECATEDLIST= NO +GENERATE_DEPRECATEDLIST= YES ENABLED_SECTIONS = MAX_INITIALIZER_LINES = 30 SHOW_USED_FILES = YES @@ -93,18 +99,19 @@ LAYOUT_FILE = "@PCL_SOURCE_DIR@/doc/doxygen/doxygen_layout.xml" CITE_BIB_FILES = #--------------------------------------------------------------------------- -# configuration options related to warning and progress messages +# Configuration options related to warning and progress messages #--------------------------------------------------------------------------- QUIET = YES WARNINGS = YES WARN_IF_UNDOCUMENTED = YES WARN_IF_DOC_ERROR = YES WARN_NO_PARAMDOC = NO +WARN_AS_ERROR = NO WARN_FORMAT = "$file:$line: $text " WARN_LOGFILE = #--------------------------------------------------------------------------- -# configuration options related to the input files +# Configuration options related to the input files #--------------------------------------------------------------------------- INPUT = "@PCL_SOURCE_DIR@" \ "@PCL_SOURCE_DIR@/doc/doxygen" @@ -137,9 +144,10 @@ INPUT_FILTER = FILTER_PATTERNS = FILTER_SOURCE_FILES = NO FILTER_SOURCE_PATTERNS = +USE_MDFILE_AS_MAINPAGE = #--------------------------------------------------------------------------- -# configuration options related to source browsing +# Configuration options related to source browsing #--------------------------------------------------------------------------- SOURCE_BROWSER = YES INLINE_SOURCES = NO @@ -147,18 +155,19 @@ STRIP_CODE_COMMENTS = NO REFERENCED_BY_RELATION = YES REFERENCES_RELATION = YES REFERENCES_LINK_SOURCE = YES +SOURCE_TOOLTIPS = YES USE_HTAGS = NO VERBATIM_HEADERS = YES #--------------------------------------------------------------------------- -# configuration options related to the alphabetical class index +# Configuration options related to the alphabetical class index #--------------------------------------------------------------------------- ALPHABETICAL_INDEX = YES COLS_IN_ALPHA_INDEX = 5 IGNORE_PREFIX = #--------------------------------------------------------------------------- -# configuration options related to the HTML output +# Configuration options related to the HTML output #--------------------------------------------------------------------------- GENERATE_HTML = YES HTML_OUTPUT = html @@ -166,12 +175,14 @@ HTML_FILE_EXTENSION = .html HTML_HEADER = HTML_FOOTER = @PCL_SOURCE_DIR@/doc/doxygen/footer.html HTML_STYLESHEET = +HTML_EXTRA_STYLESHEET = HTML_EXTRA_FILES = HTML_COLORSTYLE_HUE = 87 HTML_COLORSTYLE_SAT = 46 HTML_COLORSTYLE_GAMMA = 73 HTML_TIMESTAMP = YES HTML_DYNAMIC_SECTIONS = YES +HTML_INDEX_NUM_ENTRIES = 100 GENERATE_DOCSET = YES DOCSET_FEEDNAME = "Doxygen generated docs" DOCSET_BUNDLE_ID = pointclouds.org @@ -202,13 +213,20 @@ EXT_LINKS_IN_WINDOW = NO FORMULA_FONTSIZE = 10 FORMULA_TRANSPARENT = YES USE_MATHJAX = NO +MATHJAX_FORMAT = HTML-CSS MATHJAX_RELPATH = http://cdn.mathjax.org/mathjax/latest MATHJAX_EXTENSIONS = +MATHJAX_CODEFILE = SEARCHENGINE = @DOCUMENTATION_SEARCHENGINE@ SERVER_BASED_SEARCH = NO +EXTERNAL_SEARCH = NO +SEARCHENGINE_URL = +SEARCHDATA_FILE = searchdata.xml +EXTERNAL_SEARCH_ID = +EXTRA_SEARCH_MAPPINGS = #--------------------------------------------------------------------------- -# configuration options related to the LaTeX output +# Configuration options related to the LaTeX output #--------------------------------------------------------------------------- GENERATE_LATEX = YES LATEX_OUTPUT = latex @@ -219,15 +237,18 @@ PAPER_TYPE = a4wide EXTRA_PACKAGES = amsmath amssymb LATEX_HEADER = LATEX_FOOTER = +LATEX_EXTRA_STYLESHEET = +LATEX_EXTRA_FILES = PDF_HYPERLINKS = YES USE_PDFLATEX = YES LATEX_BATCHMODE = NO LATEX_HIDE_INDICES = NO LATEX_SOURCE_CODE = NO LATEX_BIB_STYLE = plain +LATEX_TIMESTAMP = NO #--------------------------------------------------------------------------- -# configuration options related to the RTF output +# Configuration options related to the RTF output #--------------------------------------------------------------------------- GENERATE_RTF = NO RTF_OUTPUT = rtf @@ -235,29 +256,38 @@ COMPACT_RTF = NO RTF_HYPERLINKS = NO RTF_STYLESHEET_FILE = RTF_EXTENSIONS_FILE = +RTF_SOURCE_CODE = NO #--------------------------------------------------------------------------- -# configuration options related to the man page output +# Configuration options related to the man page output #--------------------------------------------------------------------------- GENERATE_MAN = NO MAN_OUTPUT = man MAN_EXTENSION = .3 +MAN_SUBDIR = MAN_LINKS = NO #--------------------------------------------------------------------------- -# configuration options related to the XML output +# Configuration options related to the XML output #--------------------------------------------------------------------------- GENERATE_XML = NO XML_OUTPUT = xml XML_PROGRAMLISTING = YES #--------------------------------------------------------------------------- -# configuration options for the AutoGen Definitions output +# Configuration options related to the DOCBOOK output +#--------------------------------------------------------------------------- +GENERATE_DOCBOOK = NO +DOCBOOK_OUTPUT = docbook +DOCBOOK_PROGRAMLISTING = NO + +#--------------------------------------------------------------------------- +# Configuration options for the AutoGen Definitions output #--------------------------------------------------------------------------- GENERATE_AUTOGEN_DEF = NO #--------------------------------------------------------------------------- -# configuration options related to the Perl module output +# Configuration options related to the Perl module output #--------------------------------------------------------------------------- GENERATE_PERLMOD = NO PERLMOD_LATEX = NO @@ -280,25 +310,27 @@ PREDEFINED = = "HAVE_QHULL=1" \ "HAVE_DAVIDSDK=1" \ "HAVE_DSSDK=1" \ "HAVE_RSSDK=1" \ - "DOXYGEN_ONLY=1" + "DOXYGEN_ONLY=1" \ + "_WIN32=1" \ + "PCL_DEPRECATED(major,minor,message)=/** \deprecated Scheduled for removal in version major.\ minor: message */" EXPAND_AS_DEFINED = SKIP_FUNCTION_MACROS = YES #--------------------------------------------------------------------------- -# Configuration::additions related to external references +# Configuration options related to external references #--------------------------------------------------------------------------- TAGFILES = #TAGFILES = qtools_docs/qtools.tag=../../qtools_docs/html GENERATE_TAGFILE = pcl.tag ALLEXTERNALS = NO EXTERNAL_GROUPS = YES -PERL_PATH = /usr/bin/perl +EXTERNAL_PAGES = YES #--------------------------------------------------------------------------- # Configuration options related to the dot tool #--------------------------------------------------------------------------- CLASS_DIAGRAMS = YES -MSCGEN_PATH = +DIA_PATH = HIDE_UNDOC_RELATIONS = YES HAVE_DOT = @HAVE_DOT@ DOT_NUM_THREADS = 0 @@ -322,7 +354,11 @@ INTERACTIVE_SVG = YES DOT_PATH = DOTFILE_DIRS = MSCFILE_DIRS = -DOT_GRAPH_MAX_NODES = 50 +DIAFILE_DIRS = +PLANTUML_JAR_PATH = +PLANTUML_CFG_FILE = +PLANTUML_INCLUDE_PATH = +DOT_GRAPH_MAX_NODES = 250 MAX_DOT_GRAPH_DEPTH = 0 DOT_TRANSPARENT = YES DOT_MULTI_TARGETS = NO diff --git a/doc/doxygen/pcl.doxy b/doc/doxygen/pcl.doxy index b23a02f3..29a0f91c 100644 --- a/doc/doxygen/pcl.doxy +++ b/doc/doxygen/pcl.doxy @@ -28,9 +28,10 @@ Please visit http://www.pointclouds.org for more information.

Quick Links

    -
  • Main website: http://www.pointclouds.org
    • -
  • Developer Zone: https://github.com/PointCloudLibrary
    • -
  • Build farm: https://travis-ci.org/PointCloudLibrary/pcl
    • +
  • Main Website: http://www.pointclouds.org
    • +
  • GitHub Repository: https://github.com/PointCloudLibrary
    • +
  • Continuous Integration: https://dev.azure.com/PointCloudLibrary/pcl/_build
    • +
  • Changelog: https://pointcloudlibrary.github.io/documentation/changelog.html

References

diff --git a/doc/requirements.txt b/doc/requirements.txt new file mode 100644 index 00000000..03f46b69 --- /dev/null +++ b/doc/requirements.txt @@ -0,0 +1,2 @@ +sphinx +sphinxcontrib-doxylink diff --git a/doc/tutorials/CMakeLists.txt b/doc/tutorials/CMakeLists.txt index 5282c37a..b0c77124 100644 --- a/doc/tutorials/CMakeLists.txt +++ b/doc/tutorials/CMakeLists.txt @@ -3,7 +3,12 @@ if(NOT SPHINX_FOUND) endif() add_custom_target(tutorials ALL - COMMAND "${SPHINX_EXECUTABLE}" -b html -a -d "${SPHINX_CACHE_DIR}" -D html_file_suffix=".${SPHINX_HTML_FILE_SUFFIX}" "${CMAKE_CURRENT_SOURCE_DIR}/content" html) + COMMAND "${SPHINX_EXECUTABLE}" + -b html + -d "${SPHINX_CACHE_DIR}" + -Dversion="${PCL_VERSION_PRETTY}" + -Drelease="${PCL_VERSION_PRETTY}" + "${CMAKE_CURRENT_SOURCE_DIR}/content" html) add_dependencies(tutorials doc) set_target_properties(tutorials PROPERTIES FOLDER "Documentation (Tutorials)") install(DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/html" diff --git a/doc/tutorials/content/_templates/layout.html b/doc/tutorials/content/_templates/layout.html deleted file mode 100644 index 0516be55..00000000 --- a/doc/tutorials/content/_templates/layout.html +++ /dev/null @@ -1,47 +0,0 @@ - - - -Documentation - Point Cloud Library (PCL) - -{% extends "!layout.html" %} - -{% block extrahead %} -initialize('web'); - -$snip = $modx->runSnippet("getSiteNavigation", array('id'=>5, 'phLevels'=>'sitenav.level0,sitenav.level1', 'showPageNav'=>'n')); -$chunkOutput = $modx->getChunk("site-header", array('sitenav'=>$snip)); -$bodytag = str_replace("[[+showSubmenus:notempty=`", "", $chunkOutput); -$bodytag = str_replace("`]]", "", $bodytag); -echo $bodytag; -echo "\n"; -?> -
-

Documentation

- -
-
-{% endblock %} - -{% block relbar1 %}{% endblock %} -{% block relbar2 %}{% endblock %} -{% block rootrellink %}{% endblock %} - -{% block sidebarsearch %}{% endblock %} - -{% block footer %} -
- -getChunk("site-footer"); -echo $chunkOutput; -?> -{% endblock %} - - diff --git a/doc/tutorials/content/adding_custom_ptype.rst b/doc/tutorials/content/adding_custom_ptype.rst index 350e7964..6bf3fa9e 100644 --- a/doc/tutorials/content/adding_custom_ptype.rst +++ b/doc/tutorials/content/adding_custom_ptype.rst @@ -6,7 +6,7 @@ Adding your own custom `PointT` type The current document explains not only how to add your own `PointT` point type, but also what templated point types are in PCL, why do they exist, and how are they exposed. If you're already familiar with this information, feel free to -skip to the last part of the document. +skip to the last part of the document. .. contents:: @@ -72,7 +72,7 @@ to understand why the existing types were created they way they were. In addition, the type that you want, might already be defined for you. * `PointXYZ` - Members: float x, y, z; - + This is one of the most used data types, as it represents 3D xyz information only. The 3 floats are padded with an additional float for SSE alignment. The user can either access `points[i].data[0]` or `points[i].x` for accessing @@ -80,10 +80,10 @@ addition, the type that you want, might already be defined for you. .. code-block:: cpp - union + union { float data[4]; - struct + struct { float x; float y; @@ -100,16 +100,16 @@ addition, the type that you want, might already be defined for you. `intensity` a member of the same structure, as its contents will be overwritten. For example, a dot product between two points will set their 4th component to 0, otherwise the dot product doesn't make sense, etc. - + Therefore for SSE-alignment, we pad intensity with 3 extra floats. Inefficient in terms of storage, but good in terms of memory alignment. .. code-block:: cpp - union + union { float data[4]; - struct + struct { float x; float y; @@ -188,10 +188,10 @@ addition, the type that you want, might already be defined for you. .. code-block:: cpp - union + union { float data[4]; - struct + struct { float x; float y; @@ -223,20 +223,20 @@ addition, the type that you want, might already be defined for you. .. code-block:: cpp - union + union { float data_n[4]; float normal[3]; - struct + struct { float normal_x; float normal_y; float normal_z; }; } - union + union { - struct + struct { float curvature; }; @@ -250,30 +250,30 @@ addition, the type that you want, might already be defined for you. .. code-block:: cpp - union + union { float data[4]; - struct + struct { float x; float y; float z; }; }; - union + union { float data_n[4]; float normal[3]; - struct + struct { float normal_x; float normal_y; float normal_z; }; }; - union + union { - struct + struct { float curvature; }; @@ -292,21 +292,21 @@ addition, the type that you want, might already be defined for you. .. code-block:: cpp - union + union { float data[4]; - struct + struct { float x; float y; float z; }; }; - union + union { float data_n[4]; float normal[3]; - struct + struct { float normal_x; float normal_y; @@ -345,30 +345,30 @@ addition, the type that you want, might already be defined for you. .. code-block:: cpp - union + union { float data[4]; - struct + struct { float x; float y; float z; }; }; - union + union { float data_n[4]; float normal[3]; - struct + struct { float normal_x; float normal_y; float normal_z; }; } - union + union { - struct + struct { float intensity; float curvature; @@ -383,10 +383,10 @@ addition, the type that you want, might already be defined for you. .. code-block:: cpp - union + union { float data[4]; - struct + struct { float x; float y; @@ -410,10 +410,10 @@ addition, the type that you want, might already be defined for you. .. code-block:: cpp - union + union { float data[4]; - struct + struct { float x; float y; @@ -435,7 +435,7 @@ addition, the type that you want, might already be defined for you. * `MomentInvariants` - float j1, j2, j3; Simple point type holding the 3 moment invariants at a surface patch. See - `MomentInvariantsEstimation` for more information. + `MomentInvariantsEstimation` for more information. .. code-block:: cpp @@ -447,7 +447,7 @@ addition, the type that you want, might already be defined for you. * `PrincipalRadiiRSD` - float r_min, r_max; Simple point type holding the 2 RSD radii at a surface patch. See - `RSDEstimation` for more information. + `RSDEstimation` for more information. .. code-block:: cpp @@ -459,7 +459,7 @@ addition, the type that you want, might already be defined for you. * `Boundary` - std::uint8_t boundary_point; Simple point type holding whether the point is lying on a surface boundary or - not. See `BoundaryEstimation` for more information. + not. See `BoundaryEstimation` for more information. .. code-block:: cpp @@ -471,7 +471,7 @@ addition, the type that you want, might already be defined for you. * `PrincipalCurvatures` - float principal_curvature[3], pc1, pc2; Simple point type holding the principal curvatures of a given point. See - `PrincipalCurvaturesEstimation` for more information. + `PrincipalCurvaturesEstimation` for more information. .. code-block:: cpp @@ -494,7 +494,7 @@ addition, the type that you want, might already be defined for you. * `PFHSignature125` - float pfh[125]; Simple point type holding the PFH (Point Feature Histogram) of a given point. - See `PFHEstimation` for more information. + See `PFHEstimation` for more information. .. code-block:: cpp @@ -506,7 +506,7 @@ addition, the type that you want, might already be defined for you. * `FPFHSignature33` - float fpfh[33]; Simple point type holding the FPFH (Fast Point Feature Histogram) of a given - point. See `FPFHEstimation` for more information. + point. See `FPFHEstimation` for more information. .. code-block:: cpp @@ -518,7 +518,7 @@ addition, the type that you want, might already be defined for you. * `VFHSignature308` - float vfh[308]; Simple point type holding the VFH (Viewpoint Feature Histogram) of a given - point. See `VFHEstimation` for more information. + point. See `VFHEstimation` for more information. .. code-block:: cpp @@ -530,7 +530,7 @@ addition, the type that you want, might already be defined for you. * `Narf36` - float x, y, z, roll, pitch, yaw; float descriptor[36]; Simple point type holding the NARF (Normally Aligned Radius Feature) of a given - point. See `NARFEstimation` for more information. + point. See `NARFEstimation` for more information. .. code-block:: cpp @@ -543,7 +543,7 @@ addition, the type that you want, might already be defined for you. * `BorderDescription` - int x, y; BorderTraits traits; Simple point type holding the border type of a given point. See - `BorderEstimation` for more information. + `BorderEstimation` for more information. .. code-block:: cpp @@ -556,12 +556,12 @@ addition, the type that you want, might already be defined for you. * `IntensityGradient` - float gradient[3]; Simple point type holding the intensity gradient of a given point. See - `IntensityGradientEstimation` for more information. + `IntensityGradientEstimation` for more information. .. code-block:: cpp struct - { + { union { float gradient[3]; @@ -571,8 +571,8 @@ addition, the type that you want, might already be defined for you. float gradient_y; float gradient_z; }; - }; - }; + }; + }; * `Histogram` - float histogram[N]; @@ -596,10 +596,10 @@ addition, the type that you want, might already be defined for you. struct { - union + union { float data[4]; - struct + struct { float x; float y; @@ -616,21 +616,21 @@ addition, the type that you want, might already be defined for you. .. code-block:: cpp - union + union { float data[4]; - struct + struct { float x; float y; float z; }; }; - union + union { float data_n[4]; float normal[3]; - struct + struct { float normal_x; float normal_y; @@ -681,7 +681,7 @@ fictitious example: { public: void - compute (const pcl::PointCloud &input, + compute (const pcl::PointCloud &input, pcl::PointCloud &output); } @@ -702,7 +702,7 @@ anything yet. #include "foo.h" template void - Foo::compute (const pcl::PointCloud &input, + Foo::compute (const pcl::PointCloud &input, pcl::PointCloud &output) { output = input; @@ -715,7 +715,7 @@ The above defines the actual template implementation of the method .. code-block:: cpp :linenos: - + // foo.cpp #include "pcl/point_types.h" @@ -765,7 +765,7 @@ following would do: .. code-block:: cpp :linenos: - + // foo.cpp #include "pcl/point_types.h" @@ -786,7 +786,7 @@ To add a new point type, you first have to define it. For example: .. code-block:: cpp :linenos: - + struct MyPointType { float test; @@ -824,6 +824,7 @@ data (SSE padded), together with a test float. :linenos: #define PCL_NO_PRECOMPILE + #include #include #include #include diff --git a/doc/tutorials/content/cluster_extraction.rst b/doc/tutorials/content/cluster_extraction.rst index 21888cdf..d8c71ea4 100644 --- a/doc/tutorials/content/cluster_extraction.rst +++ b/doc/tutorials/content/cluster_extraction.rst @@ -16,7 +16,7 @@ A clustering method needs to divide an unorganized point cloud model :math:`P` into smaller parts so that the overall processing time for :math:`P` is significantly reduced. A simple data clustering approach in an Euclidean sense can be implemented by making use of a 3D grid subdivision of the space using -fixed width boxes, or more generally, an octree data structure. This particular +fixed-width boxes, or more generally, an octree data structure. This particular representation is very fast to build and is useful for situations where either a volumetric representation of the occupied space is needed, or the data in each resultant 3D box (or octree leaf) can be approximated with a different @@ -24,7 +24,7 @@ structure. In a more general sense however, we can make use of nearest neighbors and implement a clustering technique that is essentially similar to a flood fill algorithm. -Let's assume we have given a point cloud with a table and objects on top of it. +Let's assume we are given a point cloud with a table and objects on top of it. We want to find and segment the individual object point clusters lying on the plane. Assuming that we use a Kd-tree structure for finding the nearest neighbors, the algorithmic steps for that would be (from [RusuDissertation]_): @@ -41,7 +41,7 @@ neighbors, the algorithmic steps for that would be (from [RusuDissertation]_): * *for every point* :math:`\boldsymbol{p}_i \in Q` *do:* - * *search for the set* :math:`P^i_k` *of point neighbors of* :math:`\boldsymbol{p}_i` *in a sphere with radius* :math:`r < d_{th}`; + * *search for the set* :math:`P^k_i` *of point neighbors of* :math:`\boldsymbol{p}_i` *in a sphere with radius* :math:`r < d_{th}`; * *for every neighbor* :math:`\boldsymbol{p}^k_i \in P^k_i`, *check if the point has already been processed, and if not add it to* :math:`Q`; diff --git a/doc/tutorials/content/conf.py b/doc/tutorials/content/conf.py index 9190ca76..6eaf69bc 100644 --- a/doc/tutorials/content/conf.py +++ b/doc/tutorials/content/conf.py @@ -1,8 +1,6 @@ # All configuration values have a default; values that are commented out # serve to show the default. -import sys, os - # -- General configuration ----------------------------------------------------- # Add any Sphinx extension module names here, as strings. They can be extensions # coming with Sphinx (named 'sphinx.ext.*') or your custom ones. @@ -10,7 +8,7 @@ extensions = ['sphinx.ext.imgmath', 'sphinxcontrib.doxylink.doxylink'] imgmath_dvipng_args = ['-gamma', '1.5', '-D', '110', '-bg', 'Transparent'] # Add any paths that contain templates here, relative to this directory. -templates_path = ['_templates'] +# templates_path = ['_templates'] # The suffix of source filenames. source_suffix = '.rst' @@ -19,7 +17,7 @@ source_suffix = '.rst' master_doc = 'index' # General information about the project. -project = u'PCL' +project = u'Point Cloud Library' copyright = '' # The version info for the project you're documenting, acts as replacement for @@ -72,7 +70,7 @@ pygments_style = 'sphinx' # The theme to use for HTML and HTML Help pages. Major themes that come with # Sphinx are currently 'default' and 'sphinxdoc'. -html_theme = 'sphinxdoc' +html_theme = 'sphinx_rtd_theme' # Theme options are theme-specific and customize the look and feel of a theme # further. For a list of options available for each theme, see the @@ -107,7 +105,7 @@ html_static_path = ['_static'] html_use_modindex = False # If false, no index is generated. -html_use_index = False +html_use_index = True # If true, the index is split into individual pages for each letter. html_split_index = False @@ -122,13 +120,8 @@ html_show_sourcelink = False # If nonempty, this is the file name suffix for HTML files (e.g. ".xhtml"). html_file_suffix = '.html' - -html_sidebars = { - '**': [], - 'using/windows': [], -} -html_show_copyright = False -html_show_sphinx = False +html_show_copyright = True +html_show_sphinx = True html_add_permalinks = u'' needs_sphinx = u'1.1' file_insertion_enabled = True diff --git a/doc/tutorials/content/convex_hull_2d.rst b/doc/tutorials/content/convex_hull_2d.rst index 956a500a..3fa12db8 100644 --- a/doc/tutorials/content/convex_hull_2d.rst +++ b/doc/tutorials/content/convex_hull_2d.rst @@ -37,7 +37,7 @@ gets created and the reconstruction is performed: .. literalinclude:: sources/convex_hull_2d/convex_hull_2d.cpp :language: cpp - :lines: 48-51 + :lines: 49-52 Compiling and running the program diff --git a/doc/tutorials/content/how_features_work.rst b/doc/tutorials/content/how_features_work.rst index 7098d80d..f05d717d 100644 --- a/doc/tutorials/content/how_features_work.rst +++ b/doc/tutorials/content/how_features_work.rst @@ -45,7 +45,7 @@ In general, PCL features use approximate methods to compute the nearest neighbor Terminology ----------- -For the reminder of this article, we will make certain abbreviations and +For the remainder of this article, we will make certain abbreviations and introduce certain notations, to simplify the in-text explanations. Please see the table below for a reference on each of the terms used. @@ -71,7 +71,7 @@ the `pcl::Feature` class accepts input data in two different ways: 1. an entire point cloud dataset, given via **setInputCloud (PointCloudConstPtr &)** - **mandatory** - Any feature estimation class with attempt to estimate a feature at **every** point in the given input cloud. + Any feature estimation class will attempt to estimate a feature at **every** point in the given input cloud. 2. a subset of a point cloud dataset, given via **setInputCloud (PointCloudConstPtr &)** and **setIndices (IndicesConstPtr &)** - **optional** diff --git a/doc/tutorials/content/hull_2d.rst b/doc/tutorials/content/hull_2d.rst index 563e1c2d..255a57d8 100644 --- a/doc/tutorials/content/hull_2d.rst +++ b/doc/tutorials/content/hull_2d.rst @@ -15,7 +15,7 @@ First, download the dataset `table_scene_mug_stereo_textured.pcd and save it somewhere to disk. Then, create a file, let's say, ``concave_hull_2d.cpp`` or -``convex_hull2d.cpp`` in your favorite editor and place the following inside: +``convex_hull_2d.cpp`` in your favorite editor and place the following inside: .. literalinclude:: sources/concave_hull_2d/concave_hull_2d.cpp :language: cpp diff --git a/doc/tutorials/content/images/form_0.png b/doc/tutorials/content/images/form_0.png deleted file mode 100644 index 8312220e..00000000 Binary files a/doc/tutorials/content/images/form_0.png and /dev/null differ diff --git a/doc/tutorials/content/images/form_1.png b/doc/tutorials/content/images/form_1.png deleted file mode 100644 index fb127a64..00000000 Binary files a/doc/tutorials/content/images/form_1.png and /dev/null differ diff --git a/doc/tutorials/content/index.rst b/doc/tutorials/content/index.rst index ca487209..f286330a 100644 --- a/doc/tutorials/content/index.rst +++ b/doc/tutorials/content/index.rst @@ -1,5 +1,9 @@ .. toctree:: - + + +Introduction +----------- + The following links describe a set of basic PCL tutorials. Please note that their source codes may already be provided as part of the PCL regular releases, so check there before you start copy & pasting the code. The list of tutorials @@ -12,27 +16,6 @@ below is automatically generated from reST files located in our git repository. As always, we would be happy to hear your comments and receive your contributions on any tutorial. -Table of contents ------------------ - - * :ref:`basic_usage` - * :ref:`advanced_usage` - * :ref:`applications_tutorial` - * :ref:`features_tutorial` - * :ref:`filtering_tutorial` - * :ref:`i_o` - * :ref:`keypoints_tutorial` - * :ref:`kdtree_tutorial` - * :ref:`octree_tutorial` - * :ref:`range_images` - * :ref:`recognition_tutorial` - * :ref:`registration_tutorial` - * :ref:`sample_consensus` - * :ref:`segmentation_tutorial` - * :ref:`surface_tutorial` - * :ref:`visualization_tutorial` - * :ref:`gpu` - .. _basic_usage: Basic Usage diff --git a/doc/tutorials/content/kdtree_search.rst b/doc/tutorials/content/kdtree_search.rst index ef387dde..c6cf9db8 100644 --- a/doc/tutorials/content/kdtree_search.rst +++ b/doc/tutorials/content/kdtree_search.rst @@ -8,7 +8,7 @@ In this tutorial we will go over how to use a KdTree for finding the K nearest n Theoretical primer ------------------ -A k-d tree, or k-dimensional tree, is a data structure used in computer science for organizing some number of points in a space with k dimensions. It is a binary search tree with other constraints imposed on it. K-d trees are very useful for range and nearest neighbor searches. For our purposes we will generally only be dealing with point clouds in three dimensions, so all of our k-d trees will be three-dimensional. Each level of a k-d tree splits all children along a specific dimension, using a hyperplane that is perpendicular to the corresponding axis. At the root of the tree all children will be split based on the first dimension (i.e. if the first dimension coordinate is less than the root it will be in the left-sub tree and if it is greater than the root it will obviously be in the right sub-tree). Each level down in the tree divides on the next dimension, returning to the first dimension once all others have been exhausted. They most efficient way to build a k-d tree is to use a partition method like the one Quick Sort uses to place the median point at the root and everything with a smaller one dimensional value to the left and larger to the right. You then repeat this procedure on both the left and right sub-trees until the last trees that you are to partition are only composed of one element. +A k-d tree, or k-dimensional tree, is a data structure used in computer science for organizing some number of points in a space with k dimensions. It is a binary search tree with other constraints imposed on it. K-d trees are very useful for range and nearest neighbor searches. For our purposes we will generally only be dealing with point clouds in three dimensions, so all of our k-d trees will be three-dimensional. Each level of a k-d tree splits all children along a specific dimension, using a hyperplane that is perpendicular to the corresponding axis. At the root of the tree all children will be split based on the first dimension (i.e. if the first dimension coordinate is less than the root it will be in the left-sub tree and if it is greater than the root it will obviously be in the right sub-tree). Each level down in the tree divides on the next dimension, returning to the first dimension once all others have been exhausted. The most efficient way to build a k-d tree is to use a partition method like the one Quick Sort uses to place the median point at the root and everything with a smaller one-dimensional value to the left and larger to the right. You then repeat this procedure on both the left and right sub-trees until the last trees that you are to partition are only composed of one element. From [Wikipedia]_: @@ -22,7 +22,7 @@ From [Wikipedia]_: :align: center :alt: - This is a demonstration of hour the Nearest-Neighbor search works. + This is a demonstration of how the Nearest-Neighbor search works. The code -------- diff --git a/doc/tutorials/content/normal_estimation.rst b/doc/tutorials/content/normal_estimation.rst index 96d572c0..ec7ccd85 100644 --- a/doc/tutorials/content/normal_estimation.rst +++ b/doc/tutorials/content/normal_estimation.rst @@ -9,7 +9,7 @@ correct light sources that generate shadings and other visual effects. Given a geometric surface, it's usually trivial to infer the direction of the normal at a certain point on the surface as the vector perpendicular to the -surface in that point. However, since the point cloud datasets that we acquire +surface at that point. However, since the point cloud datasets that we acquire represent a set of point samples on the real surface, there are two possibilities: @@ -133,7 +133,7 @@ the surrounding point neighborhood support of the point (also called **k-neighborhood**). The specifics of the nearest-neighbor estimation problem raise the question of -the *right scale factor*: given a sampled point cloud dataset , what are the +the *right scale factor*: given a sampled point cloud dataset, what are the correct **k** (given via **pcl::Feature::setKSearch**) or **r** (given via **pcl::Feature::setRadiusSearch**) values that should be used in determining the set of nearest neighbors of a point? @@ -143,7 +143,7 @@ automatic estimation (i.e., without user given thresholds) of a point feature representation. To better illustrate this issue, the figure below presents the effects of selecting a smaller scale (i.e., small **r** or **k**) versus a larger scale (i.e., large **r** or **k**). The left part of the figures depicts -a reasonable well chosen scale factor, with estimated surface normals +a reasonable well-chosen scale factor, with estimated surface normals approximately perpendicular for the two planar surfaces and small edges visible all across the table. If the scale factor however is too big (right part), and thus the set of neighbors is larger covering points from adjacent diff --git a/doc/tutorials/content/passthrough.rst b/doc/tutorials/content/passthrough.rst index b352d953..bb970370 100644 --- a/doc/tutorials/content/passthrough.rst +++ b/doc/tutorials/content/passthrough.rst @@ -75,7 +75,7 @@ A graphical display of the filtering process is shown below. .. image:: images/passthrough_2.png -Note that the coordinate axis are represented as red (x), green (y), and blue +Note that the coordinate axes are represented as red (x), green (y), and blue (z). The five points are represented with green as the points remaining after filtering and red as the points that have been removed by the filter. diff --git a/doc/tutorials/content/planar_segmentation.rst b/doc/tutorials/content/planar_segmentation.rst index 3c397599..be1b1bef 100644 --- a/doc/tutorials/content/planar_segmentation.rst +++ b/doc/tutorials/content/planar_segmentation.rst @@ -3,8 +3,8 @@ Plane model segmentation ------------------------- -In this tutorial we will learn how do a simple plane segmentation of a set of -points, that is find all the points within a point cloud that support a plane +In this tutorial we will learn how to do a simple plane segmentation of a set of +points, that is to find all the points within a point cloud that support a plane model. This tutorial supports the :ref:`extract_indices` tutorial, presented in the **filtering** section. @@ -58,8 +58,7 @@ Then, lines: create the :pcl:`SACSegmentation ` object and set the model and method type. -This is also where we specify the "distance threshold", which determines how close a point must be to the model -in order to be considered an inlier. +This is also where we specify the "distance threshold", which determines how close a point must be to the model in order to be considered an inlier. In this tutorial, we will use the RANSAC method (pcl::SAC_RANSAC) as the robust estimator of choice. Our decision is motivated by RANSAC's simplicity (other robust estimators use it as a base and add additional, more complicated concepts). For more information @@ -127,7 +126,7 @@ A graphical display of the segmentation process is shown below. .. image:: images/planar_segmentation_2.png -Note that the coordinate axis are represented as red (x), green (y), and blue +Note that the coordinate axes are represented as red (x), green (y), and blue (z). The points are represented with red as the outliers, and green as the inliers of the plane model found. diff --git a/doc/tutorials/content/random_sample_consensus.rst b/doc/tutorials/content/random_sample_consensus.rst index d86328dc..d627f8b2 100644 --- a/doc/tutorials/content/random_sample_consensus.rst +++ b/doc/tutorials/content/random_sample_consensus.rst @@ -101,13 +101,13 @@ the program will display only the indices of the original PointCloud which satis :align: center :height: 400px -Again hit 'r' to scale and center the view and then click and drag with the mouse to rotate around the cloud. You can see there are no longer any points that do not lie with in the plane model in this PointCloud. Hit 'q' to exit the viewer and program. +Again hit 'r' to scale and center the view and then click and drag with the mouse to rotate around the cloud. You can see there are no longer any points that do not lie within the plane model in this PointCloud. Hit 'q' to exit the viewer and program. There is also an example using a sphere in this program. If you run it with:: $ ./random_sample_consensus -s -It will generate and display a sphereical cloud and some outliers as well. +It will generate and display a spherical cloud and some outliers as well. .. image:: images/ransac_outliers_sphere.png :align: center diff --git a/doc/tutorials/content/range_image_creation.rst b/doc/tutorials/content/range_image_creation.rst index 0c4bc599..e81bf593 100644 --- a/doc/tutorials/content/range_image_creation.rst +++ b/doc/tutorials/content/range_image_creation.rst @@ -44,11 +44,11 @@ sensorPose defines the 6DOF position of the virtual sensor as the origin with ro coordinate_frame=CAMERA_FRAME tells the system that x is facing right, y downwards and the z axis is forward. An alternative would be LASER_FRAME, with x facing forward, y to the left and z upwards. -For noiseLevel=0 the range image is created using a normal z-buffer. Yet if you want to average over points falling in the same cell you can use a higher value. 0.05 would mean, that all point with a maximum distance of 5cm to the closest point are used to calculate the range. +For noiseLevel=0 the range image is created using a normal z-buffer. Yet if you want to average over points falling in the same cell you can use a higher value. 0.05 would mean, that all points with a maximum distance of 5cm to the closest point are used to calculate the range. -If minRange is greater 0 all points that are closer will be ignored. +If minRange is greater than 0, all points that are closer will be ignored. -borderSize greater 0 will leave a border of unobserved points around the image when cropping it. +borderSize greater than 0 will leave a border of unobserved points around the image when cropping it. .. literalinclude:: sources/range_image_creation/range_image_creation.cpp @@ -57,7 +57,7 @@ borderSize greater 0 will leave a border of unobserved points around the image w The remaining code creates the range image from the point cloud with the given parameters and outputs some information on the terminal. -The range image is derived from the PointCloud class and its points have the members x,y,z and range. There are three kinds of points. Valid points have a real range greater zero. Unobserved points have x=y=z=NAN and range=-INFINITY. Far range points have x=y=z=NAN and range=INFINITY. +The range image is derived from the PointCloud class and its points have the members x,y,z and range. There are three kinds of points. Valid points have a real range greater than zero. Unobserved points have x=y=z=NAN and range=-INFINITY. Far range points have x=y=z=NAN and range=INFINITY. Compiling and running the program --------------------------------- diff --git a/doc/tutorials/content/region_growing_segmentation.rst b/doc/tutorials/content/region_growing_segmentation.rst index 1bd5dd31..65193dc0 100644 --- a/doc/tutorials/content/region_growing_segmentation.rst +++ b/doc/tutorials/content/region_growing_segmentation.rst @@ -6,27 +6,27 @@ Region growing segmentation In this tutorial we will learn how to use the region growing algorithm implemented in the ``pcl::RegionGrowing`` class. The purpose of the said algorithm is to merge the points that are close enough in terms of the smoothness constraint. Thereby, the output of this algorithm is the set of clusters, -were each cluster is a set of points that are considered to be a part of the same smooth surface. +where each cluster is a set of points that are considered to be a part of the same smooth surface. The work of this algorithm is based on the comparison of the angles between the points normals. Theoretical Primer --------------------------- -Let's take a look on how the algorithm works. +Let's take a look at how the algorithm works. First of all it sorts the points by their curvature value. It needs to be done because the region begins its growth from the point that has the minimum curvature value. The reason for this is that the point with the minimum curvature is located in the flat area (growth from the flattest area allows to reduce the total number of segments). -So we have the sorted cloud. Until there are unlabeled points in the cloud, algorithm picks up the point with minimum curvature value and starts the growth of the region. This process occurs as follows: +So we have the sorted cloud. Until there are no unlabeled points in the cloud, the algorithm picks up the point with minimum curvature value and starts the growth of the region. This process occurs as follows: * The picked point is added to the set called seeds. - * For every seed point algorithm finds neighbouring points. + * For every seed point, the algorithm finds its neighbouring points. - * Every neighbour is tested for the angle between its normal and normal of the current seed point. If the angle is less than threshold value + * Every neighbour is tested for the angle between its normal and normal of the current seed point. If the angle is less than the threshold value then current point is added to the current region. - * After that every neighbour is tested for the curvature value. If the curvature is less than threshold value then this point is added to the seeds. + * After that every neighbour is tested for the curvature value. If the curvature is less than the threshold value then this point is added to the seeds. * Current seed is removed from the seeds. If the seeds set becomes empty this means that the algorithm has grown the region and the process is repeated from the beginning. @@ -119,20 +119,19 @@ To learn more about how it is done you should take a look at the :ref:`normal_es :lines: 30-35 These lines are given only for example. You can safely comment this part. Insofar as ``pcl::RegionGrowing`` is derived from ``pcl::PCLBase``, -it can work with indices. It means you can point that you need to segment only -those points that are listed in the indices array instead of the whole point cloud. +it can work with indices. It means you can instruct that you only segment those points that are listed in the indices array instead of the whole point cloud. .. literalinclude:: sources/region_growing_segmentation/region_growing_segmentation.cpp :language: cpp :lines: 37-39 -You have finally reached the part where ``pcl::RegionGrowing`` is instantiated. It is a template class that have two parameters: +You have finally reached the part where ``pcl::RegionGrowing`` is instantiated. It is a template class that has two parameters: * PointT - type of points to use(in the given example it is ``pcl::PointXYZ``) * NormalT - type of normals to use(in the given example it is ``pcl::Normal``) After that minimum and maximum cluster sizes are set. It means that -after the segmentation is done all clusters that have less points then was set as minimum(or have more than maximum) will be discarded. +after the segmentation is done all clusters that have less points than minimum(or have more than maximum) will be discarded. The default values for minimum and maximum are 1 and 'as much as possible' respectively. .. literalinclude:: sources/region_growing_segmentation/region_growing_segmentation.cpp @@ -146,12 +145,12 @@ and number of neighbours is set. After that it receives the cloud that must be s :language: cpp :lines: 45-46 -This two lines are most important part in the algorithm initialization, because they are responsible for the mentioned smoothness constraint. +These two lines are the most important part in the algorithm initialization, because they are responsible for the mentioned smoothness constraint. First method sets the angle in radians that will be used as the allowable range for the normals deviation. -If the deviation between points normals is less than smoothness threshold then they are suggested to be in the same cluster +If the deviation between points normals is less than the smoothness threshold then they are suggested to be in the same cluster (new point - the tested one - will be added to the cluster). -The second one is responsible for curvature threshold. If two points have a small normals deviation then the disparity between their curvatures is tested. -And if this value is less than curvature threshold then the algorithm will continue the growth of the cluster using new added point. +The second one is responsible for the curvature threshold. If two points have a small normals deviation then the disparity between their curvatures is tested. +And if this value is less than the curvature threshold then the algorithm will continue the growth of the cluster using the newly added point. .. literalinclude:: sources/region_growing_segmentation/region_growing_segmentation.cpp :language: cpp @@ -195,7 +194,7 @@ After the segmentation the cloud viewer window will be opened and you will see s .. image:: images/region_growing_segmentation_2.jpg :height: 200px -On the last image you can see that the colored cloud has many red points. This means that these points belong to the clusters +In the last image you can see that the colored cloud has many red points. This means that these points belong to the clusters that were rejected, because they had too much/little points. .. image:: images/region_growing_segmentation_3.jpg diff --git a/doc/tutorials/content/remove_outliers.rst b/doc/tutorials/content/remove_outliers.rst index 5405fbeb..7ee6bb0e 100644 --- a/doc/tutorials/content/remove_outliers.rst +++ b/doc/tutorials/content/remove_outliers.rst @@ -3,7 +3,7 @@ Removing outliers using a Conditional or RadiusOutlier removal -------------------------------------------------------------- -This document demonstrates how to remove outliers from a PointCloud using several different methods in the filter module. First we will look at how to use a ConditionalRemoval filter which removes all indices in the given input cloud that do not satisfy one or more given conditions. Then we will learn how to us a RadiusOutlierRemoval filter which removes all indices in it's input cloud that don't have at least some number of neighbors within a certain range. +This document demonstrates how to remove outliers from a PointCloud using several different methods in the filter module. First we will look at how to use a ConditionalRemoval filter which removes all indices in the given input cloud that do not satisfy one or more given conditions. Then we will learn how to us a RadiusOutlierRemoval filter which removes all indices in its input cloud that don't have at least some number of neighbors within a certain range. The code -------- diff --git a/doc/tutorials/content/resampling.rst b/doc/tutorials/content/resampling.rst index 7e095af0..66455a5a 100644 --- a/doc/tutorials/content/resampling.rst +++ b/doc/tutorials/content/resampling.rst @@ -36,7 +36,7 @@ we obtain: To approximate the surface defined by a local neighborhood of points **p1**, **p2** ... **pk** at a point **q** we use a bivariate polynomial height function -defined on a on a robustly computed reference plane. +defined on a robustly computed reference plane. .. raw:: html @@ -45,7 +45,11 @@ defined on a on a robustly computed reference plane. The code -------- -First, create a file, let's say, ``resampling.cpp`` in your favorite +First, download the dataset `bun0.pcd +`_ +and save it somewhere to disk. + +Then, create a file, let's say, ``resampling.cpp`` in your favorite editor, and place the following inside it: diff --git a/doc/tutorials/content/sources/bspline_fitting/CMakeLists.txt b/doc/tutorials/content/sources/bspline_fitting/CMakeLists.txt index 33251588..d64245b4 100644 --- a/doc/tutorials/content/sources/bspline_fitting/CMakeLists.txt +++ b/doc/tutorials/content/sources/bspline_fitting/CMakeLists.txt @@ -10,4 +10,3 @@ add_definitions(${PCL_DEFINITIONS}) add_executable (bspline_fitting bspline_fitting.cpp) target_link_libraries (bspline_fitting ${PCL_LIBRARIES}) - diff --git a/doc/tutorials/content/sources/concave_hull_2d/concave_hull_2d.cpp b/doc/tutorials/content/sources/concave_hull_2d/concave_hull_2d.cpp index a21b5264..76137580 100644 --- a/doc/tutorials/content/sources/concave_hull_2d/concave_hull_2d.cpp +++ b/doc/tutorials/content/sources/concave_hull_2d/concave_hull_2d.cpp @@ -45,7 +45,7 @@ main (int argc, char** argv) // Project the model inliers pcl::ProjectInliers proj; proj.setModelType (pcl::SACMODEL_PLANE); - proj.setIndices (inliers); + // proj.setIndices (inliers); proj.setInputCloud (cloud_filtered); proj.setModelCoefficients (coefficients); proj.filter (*cloud_projected); diff --git a/doc/tutorials/content/sources/conditional_euclidean_clustering/CMakeLists.txt b/doc/tutorials/content/sources/conditional_euclidean_clustering/CMakeLists.txt index 453fc946..870d0de8 100644 --- a/doc/tutorials/content/sources/conditional_euclidean_clustering/CMakeLists.txt +++ b/doc/tutorials/content/sources/conditional_euclidean_clustering/CMakeLists.txt @@ -10,4 +10,3 @@ add_definitions(${PCL_DEFINITIONS}) add_executable (conditional_euclidean_clustering conditional_euclidean_clustering.cpp) target_link_libraries (conditional_euclidean_clustering ${PCL_LIBRARIES}) - diff --git a/doc/tutorials/content/sources/correspondence_grouping/CMakeLists.txt b/doc/tutorials/content/sources/correspondence_grouping/CMakeLists.txt index d48f2497..b7dcfb8c 100644 --- a/doc/tutorials/content/sources/correspondence_grouping/CMakeLists.txt +++ b/doc/tutorials/content/sources/correspondence_grouping/CMakeLists.txt @@ -9,4 +9,4 @@ link_directories(${PCL_LIBRARY_DIRS}) add_definitions(${PCL_DEFINITIONS}) add_executable (correspondence_grouping correspondence_grouping.cpp) -target_link_libraries (correspondence_grouping ${PCL_LIBRARIES}) \ No newline at end of file +target_link_libraries (correspondence_grouping ${PCL_LIBRARIES}) diff --git a/doc/tutorials/content/sources/cylinder_segmentation/CMakeLists.txt b/doc/tutorials/content/sources/cylinder_segmentation/CMakeLists.txt index 8eb2b12c..4ef87ab1 100644 --- a/doc/tutorials/content/sources/cylinder_segmentation/CMakeLists.txt +++ b/doc/tutorials/content/sources/cylinder_segmentation/CMakeLists.txt @@ -10,5 +10,3 @@ add_definitions(${PCL_DEFINITIONS}) add_executable (cylinder_segmentation cylinder_segmentation.cpp) target_link_libraries (cylinder_segmentation ${PCL_LIBRARIES}) - - diff --git a/doc/tutorials/content/sources/don_segmentation/CMakeLists.txt b/doc/tutorials/content/sources/don_segmentation/CMakeLists.txt index 4a356689..e2f1310b 100644 --- a/doc/tutorials/content/sources/don_segmentation/CMakeLists.txt +++ b/doc/tutorials/content/sources/don_segmentation/CMakeLists.txt @@ -10,5 +10,3 @@ add_definitions(${PCL_DEFINITIONS}) add_executable (don_segmentation don_segmentation.cpp) target_link_libraries (don_segmentation ${PCL_LIBRARIES}) - - diff --git a/doc/tutorials/content/sources/ensenso_cameras/ensenso_cloud_images_viewer.cpp b/doc/tutorials/content/sources/ensenso_cameras/ensenso_cloud_images_viewer.cpp index 83fd61e8..6d4d10a6 100644 --- a/doc/tutorials/content/sources/ensenso_cameras/ensenso_cloud_images_viewer.cpp +++ b/doc/tutorials/content/sources/ensenso_cameras/ensenso_cloud_images_viewer.cpp @@ -11,7 +11,7 @@ #include #include -#include +#include #include #include #include diff --git a/doc/tutorials/content/sources/extract_indices/CMakeLists.txt b/doc/tutorials/content/sources/extract_indices/CMakeLists.txt index 7c75f4ee..6a406a91 100644 --- a/doc/tutorials/content/sources/extract_indices/CMakeLists.txt +++ b/doc/tutorials/content/sources/extract_indices/CMakeLists.txt @@ -10,5 +10,3 @@ add_definitions(${PCL_DEFINITIONS}) add_executable (extract_indices extract_indices.cpp) target_link_libraries (extract_indices ${PCL_LIBRARIES}) - - diff --git a/doc/tutorials/content/sources/global_hypothesis_verification/CMakeLists.txt b/doc/tutorials/content/sources/global_hypothesis_verification/CMakeLists.txt index 10d5b006..30db6a5a 100644 --- a/doc/tutorials/content/sources/global_hypothesis_verification/CMakeLists.txt +++ b/doc/tutorials/content/sources/global_hypothesis_verification/CMakeLists.txt @@ -11,4 +11,3 @@ add_definitions(${PCL_DEFINITIONS}) add_executable (global_hypothesis_verification global_hypothesis_verification.cpp) target_link_libraries (global_hypothesis_verification ${PCL_LIBRARIES}) - diff --git a/doc/tutorials/content/sources/gpu/people_detect/CMakeLists.txt.backup b/doc/tutorials/content/sources/gpu/people_detect/CMakeLists.txt.backup deleted file mode 100644 index 0d96a2a9..00000000 --- a/doc/tutorials/content/sources/gpu/people_detect/CMakeLists.txt.backup +++ /dev/null @@ -1,18 +0,0 @@ -if(NOT VTK_FOUND) - set(DEFAULT FALSE) - set(REASON "VTK was not found.") -else(NOT VTK_FOUND) - set(DEFAULT TRUE) - set(REASON) - set(VTK_USE_FILE ${VTK_USE_FILE} CACHE INTERNAL "VTK_USE_FILE") - include (${VTK_USE_FILE}) - include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include) -endif(NOT VTK_FOUND) - -set(the_target people_tracking) - -include_directories(${VTK_INCLUDE_DIRS}) - -PCL_ADD_EXECUTABLE(people_detect ${SUBSYS_NAME} src/people_detect.cpp) -target_link_libraries (people_detect pcl_common pcl_filters pcl_kdtree pcl_segmentation pcl_kdtree pcl_gpu_people pcl_filters pcl_io pcl_visualization ${Boost_LIBRARIES}) - diff --git a/doc/tutorials/content/sources/iccv2011/include/registration.h b/doc/tutorials/content/sources/iccv2011/include/registration.h index 15af15a2..b209ed8e 100644 --- a/doc/tutorials/content/sources/iccv2011/include/registration.h +++ b/doc/tutorials/content/sources/iccv2011/include/registration.h @@ -34,7 +34,7 @@ computeInitialAlignment (const PointCloudPtr & source_points, const LocalDescrip sac_ia.setMaxCorrespondenceDistance (max_correspondence_distance); sac_ia.setMaximumIterations (nr_iterations); - sac_ia.setInputCloud (source_points); + sac_ia.setInputSource (source_points); sac_ia.setSourceFeatures (source_descriptors); sac_ia.setInputTarget (target_points); @@ -81,7 +81,7 @@ refineAlignment (const PointCloudPtr & source_points, const PointCloudPtr & targ PointCloudPtr source_points_transformed (new PointCloud); pcl::transformPointCloud (*source_points, *source_points_transformed, initial_alignment); - icp.setInputCloud (source_points_transformed); + icp.setInputSource (source_points_transformed); icp.setInputTarget (target_points); PointCloud registration_output; diff --git a/doc/tutorials/content/sources/iccv2011/include/surface.h b/doc/tutorials/content/sources/iccv2011/include/surface.h index 92bccbf0..c7bf1c03 100644 --- a/doc/tutorials/content/sources/iccv2011/include/surface.h +++ b/doc/tutorials/content/sources/iccv2011/include/surface.h @@ -18,7 +18,7 @@ class Mesh std::vector faces; }; -typedef boost::shared_ptr MeshPtr; +using MeshPtr = std::shared_ptr; PointCloudPtr smoothPointCloud (const PointCloudPtr & input, float radius, int polynomial_order) diff --git a/doc/tutorials/content/sources/iccv2011/src/openni_capture.cpp b/doc/tutorials/content/sources/iccv2011/src/openni_capture.cpp index ad76242a..1f1c5073 100644 --- a/doc/tutorials/content/sources/iccv2011/src/openni_capture.cpp +++ b/doc/tutorials/content/sources/iccv2011/src/openni_capture.cpp @@ -1,6 +1,8 @@ #include "openni_capture.h" + #include -#include +#include // for pcl::make_shared + #include OpenNICapture::OpenNICapture (const std::string& device_id) @@ -67,7 +69,7 @@ OpenNICapture::onNewFrame (const PointCloudConstPtr &cloud) { mutex_.lock (); ++frame_counter_; - most_recent_frame_ = boost::make_shared (*cloud); // Make a copy of the frame + most_recent_frame_ = pcl::make_shared (*cloud); // Make a copy of the frame mutex_.unlock (); } diff --git a/doc/tutorials/content/sources/iccv2011/src/tutorial.cpp b/doc/tutorials/content/sources/iccv2011/src/tutorial.cpp index 0a9718a9..bfae39a3 100644 --- a/doc/tutorials/content/sources/iccv2011/src/tutorial.cpp +++ b/doc/tutorials/content/sources/iccv2011/src/tutorial.cpp @@ -1,6 +1,7 @@ #include #include #include + #include #include #include @@ -13,6 +14,7 @@ # include #endif +#include // for pcl::dynamic_pointer_cast #include #include #include @@ -43,7 +45,7 @@ class ICCVTutorial pcl::PCLSurfaceBase::Ptr surface_reconstructor, typename pcl::PointCloud::ConstPtr source, typename pcl::PointCloud::ConstPtr target); - + /** * @brief starts the event loop for the visualizer */ @@ -55,14 +57,14 @@ class ICCVTutorial * @param segmented the resulting segmented point cloud containing only points of the largest cluster */ void segmentation (typename pcl::PointCloud::ConstPtr input, typename pcl::PointCloud::Ptr segmented) const; - + /** * @brief Detects key points in the input point cloud * @param input the input point cloud * @param keypoints the resulting key points. Note that they are not necessarily a subset of the input cloud */ void detectKeypoints (typename pcl::PointCloud::ConstPtr input, pcl::PointCloud::Ptr keypoints) const; - + /** * @brief extract descriptors for given key points * @param input point cloud to be used for descriptor extraction @@ -70,25 +72,25 @@ class ICCVTutorial * @param features resulting descriptors */ void extractDescriptors (typename pcl::PointCloud::ConstPtr input, typename pcl::PointCloud::Ptr keypoints, typename pcl::PointCloud::Ptr features); - + /** * @brief find corresponding features based on some metric * @param source source feature descriptors - * @param target target feature descriptors + * @param target target feature descriptors * @param correspondences indices out of the target descriptors that correspond (nearest neighbor) to the source descriptors - */ + */ void findCorrespondences (typename pcl::PointCloud::Ptr source, typename pcl::PointCloud::Ptr target, std::vector& correspondences) const; - + /** * @brief remove non-consistent correspondences */ void filterCorrespondences (); - + /** * @brief calculate the initial rigid transformation from filtered corresponding keypoints */ void determineInitialTransformation (); - + /** * @brief calculate the final rigid transformation using ICP over all points */ @@ -105,7 +107,7 @@ class ICCVTutorial * @param cookie user defined data passed during registration of the callback */ void keyboard_callback (const pcl::visualization::KeyboardEvent& event, void* cookie); - + private: pcl::visualization::PCLVisualizer visualizer_; pcl::PointCloud::Ptr source_keypoints_; @@ -157,24 +159,24 @@ ICCVTutorial::ICCVTutorial(pcl::Keypoint::segmentation (typename pcl::PointCloud extract; extract.setInputCloud (source); extract.setIndices (inliers); @@ -206,7 +208,7 @@ void ICCVTutorial::segmentation (typename pcl::PointCloud indices; pcl::removeNaNFromPointCloud(*segmented, *segmented, indices); std::cout << "OK" << std::endl; - + std::cout << "clustering..." << std::flush; // euclidean clustering typename pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); @@ -220,7 +222,7 @@ void ICCVTutorial::segmentation (typename pcl::PointCloud 0)//use largest cluster { std::cout << cluster_indices.size() << " clusters found"; @@ -252,16 +254,16 @@ void ICCVTutorial::extractDescriptors (typename pcl::PointCloud::Ptr kpts(new pcl::PointCloud); kpts->points.resize(keypoints->points.size()); - + pcl::copyPointCloud(*keypoints, *kpts); - - typename pcl::FeatureFromNormals::Ptr feature_from_normals = boost::dynamic_pointer_cast > (feature_extractor_); - + + typename pcl::FeatureFromNormals::Ptr feature_from_normals = pcl::dynamic_pointer_cast > (feature_extractor_); + feature_extractor_->setSearchSurface(input); feature_extractor_->setInputCloud(kpts); - + if (feature_from_normals) - //if (boost::dynamic_pointer_cast > (feature_extractor_)) + //if (pcl::dynamic_pointer_cast > (feature_extractor_)) { std::cout << "normal estimation..." << std::flush; typename pcl::PointCloud::Ptr normals (new pcl::PointCloud); @@ -309,14 +311,14 @@ void ICCVTutorial::filterCorrespondences () for (unsigned cIdx = 0; cIdx < source2target_.size (); ++cIdx) if (target2source_[source2target_[cIdx]] == cIdx) correspondences.push_back(std::make_pair(cIdx, source2target_[cIdx])); - + correspondences_->resize (correspondences.size()); for (unsigned cIdx = 0; cIdx < correspondences.size(); ++cIdx) { (*correspondences_)[cIdx].index_query = correspondences[cIdx].first; (*correspondences_)[cIdx].index_match = correspondences[cIdx].second; } - + pcl::registration::CorrespondenceRejectorSampleConsensus rejector; rejector.setInputSource(source_keypoints_); rejector.setInputTarget(target_keypoints_); @@ -330,9 +332,9 @@ void ICCVTutorial::determineInitialTransformation () { std::cout << "initial alignment..." << std::flush; pcl::registration::TransformationEstimation::Ptr transformation_estimation (new pcl::registration::TransformationEstimationSVD); - + transformation_estimation->estimateRigidTransformation (*source_keypoints_, *target_keypoints_, *correspondences_, initial_transformation_matrix_); - + pcl::transformPointCloud(*source_segmented_, *source_transformed_, initial_transformation_matrix_); std::cout << "OK" << std::endl; } @@ -342,7 +344,7 @@ void ICCVTutorial::determineFinalTransformation () { std::cout << "final registration..." << std::flush; pcl::Registration::Ptr registration (new pcl::IterativeClosestPoint); - registration->setInputCloud(source_transformed_); + registration->setInputSource(source_transformed_); //registration->setInputCloud(source_segmented_); registration->setInputTarget (target_segmented_); registration->setMaxCorrespondenceDistance(0.05); @@ -362,7 +364,7 @@ void ICCVTutorial::reconstructSurface () pcl::PointCloud::Ptr merged (new pcl::PointCloud); *merged = *source_transformed_; *merged += *target_segmented_; - + // apply grid filtering to reduce amount of points as well as to make them uniform distributed pcl::VoxelGrid voxel_grid; voxel_grid.setInputCloud(merged); @@ -378,7 +380,7 @@ void ICCVTutorial::reconstructSurface () normal_estimation.setRadiusSearch (0.01); normal_estimation.setInputCloud (merged); normal_estimation.compute (*vertices); - + pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); tree->setInputCloud (vertices); @@ -407,28 +409,28 @@ void ICCVTutorial::keyboard_callback (const pcl::visualization::Key visualizer_.addPointCloud(source_, "source_points"); } break; - + case '2': if (!visualizer_.removePointCloud("target_points")) { visualizer_.addPointCloud(target_, "target_points"); } break; - + case '3': if (!visualizer_.removePointCloud("source_segmented")) { visualizer_.addPointCloud(source_segmented_, "source_segmented"); } break; - + case '4': if (!visualizer_.removePointCloud("target_segmented")) { visualizer_.addPointCloud(target_segmented_, "target_segmented"); } break; - + case '5': if (!visualizer_.removePointCloud("source_keypoints")) { @@ -437,7 +439,7 @@ void ICCVTutorial::keyboard_callback (const pcl::visualization::Key visualizer_.addPointCloud(source_keypoints_, keypoint_color, "source_keypoints"); } break; - + case '6': if (!visualizer_.removePointCloud("target_keypoints")) { @@ -452,7 +454,7 @@ void ICCVTutorial::keyboard_callback (const pcl::visualization::Key visualizer_.addCorrespondences(source_keypoints_, target_keypoints_, source2target_, "source2target"); else visualizer_.removeCorrespondences("source2target"); - + show_source2target_ = !show_source2target_; break; @@ -464,7 +466,7 @@ void ICCVTutorial::keyboard_callback (const pcl::visualization::Key show_target2source_ = !show_target2source_; break; - + case '9': if (!show_correspondences) visualizer_.addCorrespondences(source_keypoints_, target_keypoints_, *correspondences_, "correspondences"); @@ -472,7 +474,7 @@ void ICCVTutorial::keyboard_callback (const pcl::visualization::Key visualizer_.removeCorrespondences("correspondences"); show_correspondences = !show_correspondences; break; - + case 'i': case 'I': if (!visualizer_.removePointCloud("transformed")) @@ -484,7 +486,7 @@ void ICCVTutorial::keyboard_callback (const pcl::visualization::Key if (!visualizer_.removePointCloud("registered")) visualizer_.addPointCloud(source_registered_, "registered"); break; - + case 't': case 'T': visualizer_.addPolygonMesh(surface_, "surface"); @@ -493,10 +495,10 @@ void ICCVTutorial::keyboard_callback (const pcl::visualization::Key } } -int +int main (int argc, char ** argv) { - if (argc < 6) + if (argc < 6) { pcl::console::print_info ("Syntax is: %s \n", argv[0]); pcl::console::print_info ("available : 1 = Sift3D\n"); @@ -511,7 +513,7 @@ main (int argc, char ** argv) pcl::console::print_info (" 4 = PFHRGB\n\n"); pcl::console::print_info ("available : 1 = Greedy Projection\n"); pcl::console::print_info (" 2 = Marching Cubes\n"); - + return (1); } pcl::console::print_info ("== MENU ==\n"); @@ -529,19 +531,19 @@ main (int argc, char ** argv) pcl::console::print_info ("t - show/hide triangulation (surface reconstruction)\n"); pcl::console::print_info ("h - show visualizer options\n"); pcl::console::print_info ("q - quit\n"); - + pcl::PointCloud::Ptr source (new pcl::PointCloud); pcl::io::loadPCDFile (argv[1], *source); - + pcl::PointCloud::Ptr target (new pcl::PointCloud); pcl::io::loadPCDFile (argv[2], *target); - + int keypoint_type = atoi (argv[3]); int descriptor_type = atoi (argv[4]); int surface_type = atoi (argv[5]); - + pcl::Keypoint::Ptr keypoint_detector; - + if (keypoint_type == 1) { pcl::SIFTKeypoint* sift3D = new pcl::SIFTKeypoint; @@ -569,7 +571,7 @@ main (int argc, char ** argv) case 4: harris3D->setMethod(pcl::HarrisKeypoint3D::NOBLE); break; - + case 5: harris3D->setMethod(pcl::HarrisKeypoint3D::LOWE); break; @@ -582,11 +584,10 @@ main (int argc, char ** argv) exit (1); break; } - } - + pcl::PCLSurfaceBase::Ptr surface_reconstruction; - + if (surface_type == 1) { pcl::GreedyProjectionTriangulation* gp3 = new pcl::GreedyProjectionTriangulation; @@ -615,7 +616,7 @@ main (int argc, char ** argv) pcl::console::print_error("unknown surface reconstruction method %d\n expecting values between 1 and 2", surface_type); exit (1); } - + switch (descriptor_type) { case 1: @@ -627,7 +628,7 @@ main (int argc, char ** argv) tutorial.run (); } break; - + case 2: { pcl::SHOTColorEstimationOMP* shot = new pcl::SHOTColorEstimationOMP; @@ -637,7 +638,7 @@ main (int argc, char ** argv) tutorial.run (); } break; - + case 3: { pcl::Feature::Ptr feature_extractor (new pcl::PFHEstimation); @@ -646,7 +647,7 @@ main (int argc, char ** argv) tutorial.run (); } break; - + case 4: { pcl::Feature::Ptr feature_extractor (new pcl::PFHRGBEstimation); @@ -655,12 +656,12 @@ main (int argc, char ** argv) tutorial.run (); } break; - + default: pcl::console::print_error("unknown descriptor type %d\n expecting values between 1 and 4", descriptor_type); exit (1); break; - } - + } + return (0); } diff --git a/doc/tutorials/content/sources/implicit_shape_model/CMakeLists.txt b/doc/tutorials/content/sources/implicit_shape_model/CMakeLists.txt index b19cf301..9f85b0a1 100644 --- a/doc/tutorials/content/sources/implicit_shape_model/CMakeLists.txt +++ b/doc/tutorials/content/sources/implicit_shape_model/CMakeLists.txt @@ -11,5 +11,3 @@ add_definitions(${PCL_DEFINITIONS}) add_executable (implicit_shape_model implicit_shape_model.cpp) target_link_libraries (implicit_shape_model ${PCL_LIBRARIES}) - - diff --git a/doc/tutorials/content/sources/iros2011/include/solution/registration.h b/doc/tutorials/content/sources/iros2011/include/solution/registration.h index 0c307044..45c48d48 100644 --- a/doc/tutorials/content/sources/iros2011/include/solution/registration.h +++ b/doc/tutorials/content/sources/iros2011/include/solution/registration.h @@ -34,7 +34,7 @@ computeInitialAlignment (const PointCloudPtr & source_points, const LocalDescrip sac_ia.setMaxCorrespondenceDistance (max_correspondence_distance); sac_ia.setMaximumIterations (nr_iterations); - sac_ia.setInputCloud (source_points); + sac_ia.setInputSource (source_points); sac_ia.setSourceFeatures (source_descriptors); sac_ia.setInputTarget (target_points); @@ -81,7 +81,7 @@ refineAlignment (const PointCloudPtr & source_points, const PointCloudPtr & targ PointCloudPtr source_points_transformed (new PointCloud); pcl::transformPointCloud (*source_points, *source_points_transformed, initial_alignment); - icp.setInputCloud (source_points_transformed); + icp.setInputSource (source_points_transformed); icp.setInputTarget (target_points); PointCloud registration_output; diff --git a/doc/tutorials/content/sources/iros2011/include/solution/surface.h b/doc/tutorials/content/sources/iros2011/include/solution/surface.h index 92bccbf0..c7bf1c03 100644 --- a/doc/tutorials/content/sources/iros2011/include/solution/surface.h +++ b/doc/tutorials/content/sources/iros2011/include/solution/surface.h @@ -18,7 +18,7 @@ class Mesh std::vector faces; }; -typedef boost::shared_ptr MeshPtr; +using MeshPtr = std::shared_ptr; PointCloudPtr smoothPointCloud (const PointCloudPtr & input, float radius, int polynomial_order) diff --git a/doc/tutorials/content/sources/iros2011/include/surface.h b/doc/tutorials/content/sources/iros2011/include/surface.h index 731a52ab..7749faed 100644 --- a/doc/tutorials/content/sources/iros2011/include/surface.h +++ b/doc/tutorials/content/sources/iros2011/include/surface.h @@ -18,7 +18,7 @@ public: std::vector faces; }; -typedef boost::shared_ptr MeshPtr; +using MeshPtr = std::shared_ptr; PointCloudPtr smoothPointCloud (const PointCloudPtr & input, float radius, int polynomial_order) diff --git a/doc/tutorials/content/sources/iros2011/src/openni_capture.cpp b/doc/tutorials/content/sources/iros2011/src/openni_capture.cpp index 4f4f3989..29ae70d5 100644 --- a/doc/tutorials/content/sources/iros2011/src/openni_capture.cpp +++ b/doc/tutorials/content/sources/iros2011/src/openni_capture.cpp @@ -1,7 +1,9 @@ #include "openni_capture.h" + #include +#include // for pcl::make_shared + #include -#include OpenNICapture::OpenNICapture (const std::string& device_id) : grabber_ (device_id) @@ -22,7 +24,6 @@ OpenNICapture::~OpenNICapture () { // Stop the grabber when shutting down grabber_.stop (); - if (preview_) delete preview_; } @@ -70,7 +71,7 @@ OpenNICapture::onNewFrame (const PointCloudConstPtr &cloud) { mutex_.lock (); ++frame_counter_; - most_recent_frame_ = boost::make_shared (*cloud); // Make a copy of the frame + most_recent_frame_ = pcl::make_shared (*cloud); // Make a copy of the frame mutex_.unlock (); } diff --git a/doc/tutorials/content/sources/iterative_closest_point/iterative_closest_point.cpp b/doc/tutorials/content/sources/iterative_closest_point/iterative_closest_point.cpp index 2dff353e..ee4e3f74 100644 --- a/doc/tutorials/content/sources/iterative_closest_point/iterative_closest_point.cpp +++ b/doc/tutorials/content/sources/iterative_closest_point/iterative_closest_point.cpp @@ -6,39 +6,40 @@ int main (int argc, char** argv) { - pcl::PointCloud::Ptr cloud_in (new pcl::PointCloud); + pcl::PointCloud::Ptr cloud_in (new pcl::PointCloud(5,1)); pcl::PointCloud::Ptr cloud_out (new pcl::PointCloud); // Fill in the CloudIn data - cloud_in->width = 5; - cloud_in->height = 1; - cloud_in->is_dense = false; - cloud_in->points.resize (cloud_in->width * cloud_in->height); - for (std::size_t i = 0; i < cloud_in->points.size (); ++i) + for (auto& point : *cloud_in) { - cloud_in->points[i].x = 1024 * rand () / (RAND_MAX + 1.0f); - cloud_in->points[i].y = 1024 * rand () / (RAND_MAX + 1.0f); - cloud_in->points[i].z = 1024 * rand () / (RAND_MAX + 1.0f); + point.x = 1024 * rand() / (RAND_MAX + 1.0f); + point.y = 1024 * rand() / (RAND_MAX + 1.0f); + point.z = 1024 * rand() / (RAND_MAX + 1.0f); } - std::cout << "Saved " << cloud_in->points.size () << " data points to input:" - << std::endl; - for (std::size_t i = 0; i < cloud_in->points.size (); ++i) std::cout << " " << - cloud_in->points[i].x << " " << cloud_in->points[i].y << " " << - cloud_in->points[i].z << std::endl; + + std::cout << "Saved " << cloud_in->points.size () << " data points to input:" << std::endl; + + for (auto& point : *cloud_in) + std::cout << point << std::endl; + *cloud_out = *cloud_in; + std::cout << "size:" << cloud_out->points.size() << std::endl; - for (std::size_t i = 0; i < cloud_in->points.size (); ++i) - cloud_out->points[i].x = cloud_in->points[i].x + 0.7f; - std::cout << "Transformed " << cloud_in->points.size () << " data points:" - << std::endl; - for (std::size_t i = 0; i < cloud_out->points.size (); ++i) - std::cout << " " << cloud_out->points[i].x << " " << - cloud_out->points[i].y << " " << cloud_out->points[i].z << std::endl; + for (auto& point : *cloud_out) + point.x += 0.7f; + + std::cout << "Transformed " << cloud_in->points.size () << " data points:" << std::endl; + + for (auto& point : *cloud_out) + std::cout << point << std::endl; + pcl::IterativeClosestPoint icp; icp.setInputSource(cloud_in); icp.setInputTarget(cloud_out); + pcl::PointCloud Final; icp.align(Final); + std::cout << "has converged:" << icp.hasConverged() << " score: " << icp.getFitnessScore() << std::endl; std::cout << icp.getFinalTransformation() << std::endl; diff --git a/doc/tutorials/content/sources/min_cut_segmentation/CMakeLists.txt b/doc/tutorials/content/sources/min_cut_segmentation/CMakeLists.txt index e3d26ae2..541dfb27 100644 --- a/doc/tutorials/content/sources/min_cut_segmentation/CMakeLists.txt +++ b/doc/tutorials/content/sources/min_cut_segmentation/CMakeLists.txt @@ -10,5 +10,3 @@ add_definitions(${PCL_DEFINITIONS}) add_executable (min_cut_segmentation min_cut_segmentation.cpp) target_link_libraries (min_cut_segmentation ${PCL_LIBRARIES}) - - diff --git a/doc/tutorials/content/sources/moment_of_inertia/CMakeLists.txt b/doc/tutorials/content/sources/moment_of_inertia/CMakeLists.txt index 817ee201..f64ea547 100644 --- a/doc/tutorials/content/sources/moment_of_inertia/CMakeLists.txt +++ b/doc/tutorials/content/sources/moment_of_inertia/CMakeLists.txt @@ -11,4 +11,3 @@ add_definitions(${PCL_DEFINITIONS}) add_executable (moment_of_inertia moment_of_inertia.cpp) target_link_libraries (moment_of_inertia ${PCL_LIBRARIES}) - diff --git a/doc/tutorials/content/sources/narf_descriptor_visualization/CMakeLists.txt b/doc/tutorials/content/sources/narf_descriptor_visualization/CMakeLists.txt index 69cc07cb..41137397 100644 --- a/doc/tutorials/content/sources/narf_descriptor_visualization/CMakeLists.txt +++ b/doc/tutorials/content/sources/narf_descriptor_visualization/CMakeLists.txt @@ -10,4 +10,3 @@ add_definitions(${PCL_DEFINITIONS}) add_executable (narf_descriptor_visualization narf_descriptor_visualization.cpp) target_link_libraries (narf_descriptor_visualization ${PCL_LIBRARIES}) - diff --git a/doc/tutorials/content/sources/narf_feature_extraction/CMakeLists.txt b/doc/tutorials/content/sources/narf_feature_extraction/CMakeLists.txt index a83a289a..bf26ed21 100644 --- a/doc/tutorials/content/sources/narf_feature_extraction/CMakeLists.txt +++ b/doc/tutorials/content/sources/narf_feature_extraction/CMakeLists.txt @@ -10,4 +10,3 @@ add_definitions(${PCL_DEFINITIONS}) add_executable (narf_feature_extraction narf_feature_extraction.cpp) target_link_libraries (narf_feature_extraction ${PCL_LIBRARIES}) - diff --git a/doc/tutorials/content/sources/narf_keypoint_extraction/CMakeLists.txt b/doc/tutorials/content/sources/narf_keypoint_extraction/CMakeLists.txt index df0c9823..334094af 100644 --- a/doc/tutorials/content/sources/narf_keypoint_extraction/CMakeLists.txt +++ b/doc/tutorials/content/sources/narf_keypoint_extraction/CMakeLists.txt @@ -10,4 +10,3 @@ add_definitions(${PCL_DEFINITIONS}) add_executable (narf_keypoint_extraction narf_keypoint_extraction.cpp) target_link_libraries (narf_keypoint_extraction ${PCL_LIBRARIES}) - diff --git a/doc/tutorials/content/sources/openni_range_image_visualization/CMakeLists.txt b/doc/tutorials/content/sources/openni_range_image_visualization/CMakeLists.txt index ee9d2936..d5d9d397 100644 --- a/doc/tutorials/content/sources/openni_range_image_visualization/CMakeLists.txt +++ b/doc/tutorials/content/sources/openni_range_image_visualization/CMakeLists.txt @@ -10,4 +10,3 @@ add_definitions(${PCL_DEFINITIONS}) add_executable (openni_range_image_visualization openni_range_image_visualization.cpp) target_link_libraries (openni_range_image_visualization ${PCL_LIBRARIES}) - diff --git a/doc/tutorials/content/sources/pairwise_incremental_registration/pairwise_incremental_registration.cpp b/doc/tutorials/content/sources/pairwise_incremental_registration/pairwise_incremental_registration.cpp index d07f9286..e377b6f7 100644 --- a/doc/tutorials/content/sources/pairwise_incremental_registration/pairwise_incremental_registration.cpp +++ b/doc/tutorials/content/sources/pairwise_incremental_registration/pairwise_incremental_registration.cpp @@ -38,7 +38,7 @@ /* \author Radu Bogdan Rusu * adaptation Raphael Favier*/ -#include +#include // for pcl::make_shared #include #include #include @@ -255,7 +255,7 @@ void pairAlign (const PointCloud::Ptr cloud_src, const PointCloud::Ptr cloud_tgt // Note: adjust this based on the size of your datasets reg.setMaxCorrespondenceDistance (0.1); // Set the point representation - reg.setPointRepresentation (boost::make_shared (point_representation)); + reg.setPointRepresentation (pcl::make_shared (point_representation)); reg.setInputSource (points_with_normals_src); reg.setInputTarget (points_with_normals_tgt); diff --git a/doc/tutorials/content/sources/region_growing_segmentation/CMakeLists.txt b/doc/tutorials/content/sources/region_growing_segmentation/CMakeLists.txt index 855ab516..f7e99610 100644 --- a/doc/tutorials/content/sources/region_growing_segmentation/CMakeLists.txt +++ b/doc/tutorials/content/sources/region_growing_segmentation/CMakeLists.txt @@ -11,4 +11,3 @@ add_definitions(${PCL_DEFINITIONS}) add_executable (region_growing_segmentation region_growing_segmentation.cpp) target_link_libraries (region_growing_segmentation ${PCL_LIBRARIES}) - diff --git a/doc/tutorials/content/sources/registration_api/example2.cpp b/doc/tutorials/content/sources/registration_api/example2.cpp index 9023fa22..f4a7f5e3 100644 --- a/doc/tutorials/content/sources/registration_api/example2.cpp +++ b/doc/tutorials/content/sources/registration_api/example2.cpp @@ -120,7 +120,7 @@ rejectBadCorrespondences (const CorrespondencesPtr &all_correspondences, Correspondences &remaining_correspondences) { CorrespondenceRejectorDistance rej; - rej.setInputCloud (keypoints_src); + rej.setInputSource (keypoints_src); rej.setInputTarget (keypoints_tgt); rej.setMaximumDistance (1); // 1m rej.setInputCorrespondences (all_correspondences); diff --git a/doc/tutorials/content/sources/rops_feature/CMakeLists.txt b/doc/tutorials/content/sources/rops_feature/CMakeLists.txt index 47895569..98f3fc80 100644 --- a/doc/tutorials/content/sources/rops_feature/CMakeLists.txt +++ b/doc/tutorials/content/sources/rops_feature/CMakeLists.txt @@ -11,4 +11,3 @@ add_definitions(${PCL_DEFINITIONS}) add_executable (rops_feature rops_feature.cpp) target_link_libraries (rops_feature ${PCL_LIBRARIES}) - diff --git a/doc/tutorials/content/sources/stick_segmentation/stick_segmentation.cpp b/doc/tutorials/content/sources/stick_segmentation/stick_segmentation.cpp index 7d8db5d0..03f85dfa 100644 --- a/doc/tutorials/content/sources/stick_segmentation/stick_segmentation.cpp +++ b/doc/tutorials/content/sources/stick_segmentation/stick_segmentation.cpp @@ -2,7 +2,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/doc/tutorials/content/sources/template_alignment/template_alignment.cpp b/doc/tutorials/content/sources/template_alignment/template_alignment.cpp index 5d998352..46f95e87 100644 --- a/doc/tutorials/content/sources/template_alignment/template_alignment.cpp +++ b/doc/tutorials/content/sources/template_alignment/template_alignment.cpp @@ -2,6 +2,7 @@ #include #include #include +#include #include #include #include @@ -161,7 +162,7 @@ class TemplateAlignment void align (FeatureCloud &template_cloud, TemplateAlignment::Result &result) { - sac_ia_.setInputCloud (template_cloud.getPointCloud ()); + sac_ia_.setInputSource (template_cloud.getPointCloud ()); sac_ia_.setSourceFeatures (template_cloud.getLocalFeatures ()); pcl::PointCloud registration_output; diff --git a/doc/tutorials/content/sources/voxel_grid/voxel_grid.cpp b/doc/tutorials/content/sources/voxel_grid/voxel_grid.cpp index abd31d08..95c52067 100644 --- a/doc/tutorials/content/sources/voxel_grid/voxel_grid.cpp +++ b/doc/tutorials/content/sources/voxel_grid/voxel_grid.cpp @@ -15,7 +15,7 @@ main (int argc, char** argv) reader.read ("table_scene_lms400.pcd", *cloud); // Remember to download the file first! std::cerr << "PointCloud before filtering: " << cloud->width * cloud->height - << " data points (" << pcl::getFieldsList (*cloud) << ")."; + << " data points (" << pcl::getFieldsList (*cloud) << ")." << std::endl; // Create the filtering object pcl::VoxelGrid sor; @@ -24,7 +24,7 @@ main (int argc, char** argv) sor.filter (*cloud_filtered); std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height - << " data points (" << pcl::getFieldsList (*cloud_filtered) << ")."; + << " data points (" << pcl::getFieldsList (*cloud_filtered) << ")." << std::endl; pcl::PCDWriter writer; writer.write ("table_scene_lms400_downsampled.pcd", *cloud_filtered, diff --git a/doc/tutorials/content/statistical_outlier.rst b/doc/tutorials/content/statistical_outlier.rst index 76ee373d..fa8ee28c 100644 --- a/doc/tutorials/content/statistical_outlier.rst +++ b/doc/tutorials/content/statistical_outlier.rst @@ -20,7 +20,7 @@ characteristics such as surface normals or curvature changes, leading to erroneous values, which in turn might cause point cloud registration failures. Some of these irregularities can be solved by performing a statistical analysis on each point's neighborhood, and trimming those which do not meet a certain -criteria. Our sparse outlier removal is based on the computation of the +criterion. Our sparse outlier removal is based on the computation of the distribution of point to neighbors distances in the input dataset. For each point, we compute the mean distance from it to all its neighbors. By assuming that the resulted distribution is Gaussian with a mean and a standard diff --git a/doc/tutorials/content/template_alignment.rst b/doc/tutorials/content/template_alignment.rst index 3f544d3e..f8cd1ce4 100644 --- a/doc/tutorials/content/template_alignment.rst +++ b/doc/tutorials/content/template_alignment.rst @@ -5,11 +5,11 @@ Aligning object templates to a point cloud This tutorial gives an example of how some of the tools covered in the other tutorials can be combined to solve a higher level problem --- aligning a previously captured model of an object to some newly captured data. In this specific example, we'll take a depth image that contains a person and try to fit some previously captured templates of their face; this will allow us to determine the position and orientation of the face in the scene. -.. raw:: html +.. raw:: html -We can use the code below to fit a template of a person's face (the blue points) to a new point cloud (the green points). +We can use the code below to fit a template of a person's face (the blue points) to a new point cloud (the green points). The code @@ -71,11 +71,11 @@ The methods described above serve to encapsulate the work needed to compute feat Now we'll examine the *TemplateAlignment* class, which as the name suggests, will be used to perform template alignment (also referred to as template fitting/matching/registration). A template is typically a small group of pixels or points that represents a known part of a larger object or scene. By registering a template to a new image or point cloud, you can determine the position and orientation of the object that the template represents. -We start by defining a structure to store the alignment results. It contains a floating point value that represents the "fitness" of the alignment (a lower number means a better alignment) and a transformation matrix that describes how template points should be rotated and translated in order to best align with the points in the target cloud. +We start by defining a structure to store the alignment results. It contains a floating point value that represents the "fitness" of the alignment (a lower number means a better alignment) and a transformation matrix that describes how template points should be rotated and translated in order to best align with the points in the target cloud. .. note:: - Because we are including an Eigen::Matrix4f in this struct, we need to include the EIGEN_MAKE_ALIGNED_OPERATOR_NEW macro, which will overload the struct's "operator new" so that it will generate 16-bytes-aligned pointers. If you're curious, you can find more information about this issue `here `_. For convenience, there is a redefinition of the macro in pcl_macros.h, aptly named PCL_MAKE_ALIGNED_OPERATOR_NEW which will let us for example call `pcl::make_shared` to create a `shared_ptr` of over-aligned classes. + Because we are including an Eigen::Matrix4f in this struct, we need to include the EIGEN_MAKE_ALIGNED_OPERATOR_NEW macro, which will overload the struct's "operator new" so that it will generate 16-bytes-aligned pointers. If you're curious, you can find more information about this issue `here `_. For convenience, there is a redefinition of the macro in memory.h, aptly named PCL_MAKE_ALIGNED_OPERATOR_NEW which will let us for example call `pcl::make_shared` to create a `shared_ptr` of over-aligned classes. .. literalinclude:: sources/template_alignment/template_alignment.cpp :language: cpp @@ -201,11 +201,11 @@ After you have made the executable, you can run it like so:: After a few seconds, you will see output similar to:: Best fitness score: 0.000009 - - | 0.834 0.295 0.466 | - R = | -0.336 0.942 0.006 | - | -0.437 -0.162 0.885 | - + + | 0.834 0.295 0.466 | + R = | -0.336 0.942 0.006 | + | -0.437 -0.162 0.885 | + t = < -0.373, -0.097, 0.087 > You can also use the `pcl_viewer `_ utility to visualize the aligned template and overlay it against the target cloud by running the following command:: diff --git a/doc/tutorials/content/walkthrough.rst b/doc/tutorials/content/walkthrough.rst index a1e45602..8f3a9baa 100644 --- a/doc/tutorials/content/walkthrough.rst +++ b/doc/tutorials/content/walkthrough.rst @@ -112,13 +112,7 @@ Features .. image:: images/features_normal.jpg - An example of two of the most widely used geometric point features are the underlying surface's estimated curvature and normal at a query point ``p``. Both of them are considered local features, as they characterize a point using the information provided by its ``k`` closest point neighbors. For determining these neighbors efficiently, the input dataset is usually split into smaller chunks using spatial decomposition techniques such as octrees or kD-trees (see the figure below - left: kD-tree, right: octree), and then closest point searches are performed in that space. Depending on the application one can opt for either determining a fixed number of ``k`` points in the vicinity of ``p``, or all points which are found inside of a sphere of radius ``r`` centered at ``p``. Unarguably, one the easiest methods for estimating the surface normals and curvature changes at a point ``p`` is to perform an eigendecomposition (i.e., compute the eigenvectors and eigenvalues) of the k-neighborhood point surface patch. Thus, the eigenvector corresponding to the smallest eigenvalue will approximate the surface normal ``n`` at point ``p``, while the surface curvature change will be estimated from the eigenvalues as: - - .. image:: images/form_0.png - - .. image:: images/form_1.png - - | + An example of two of the most widely used geometric point features are the underlying surface's estimated curvature and normal at a query point ``p``. Both of them are considered local features, as they characterize a point using the information provided by its ``k`` closest point neighbors. For determining these neighbors efficiently, the input dataset is usually split into smaller chunks using spatial decomposition techniques such as octrees or kD-trees, and then closest point searches are performed in that space. Depending on the application one can opt for either determining a fixed number of ``k`` points in the vicinity of ``p``, or all points which are found inside of a sphere of radius ``r`` centered at ``p``. Unarguably, one the easiest methods for estimating the surface normals and curvature changes at a point ``p`` is to perform an eigendecomposition (i.e., compute the eigenvectors and eigenvalues) of the k-neighborhood point surface patch. Thus, the eigenvector corresponding to the smallest eigenvalue will approximate the surface normal ``n`` at point ``p``, while the surface curvature change will be estimated from the eigenvalues as :math:`\frac{\lambda_0}{\lambda_0+\lambda_1+\lambda_2}` with :math:`\lambda_0<\lambda_1<\lambda_2`. .. image:: images/features_bunny.jpg @@ -789,4 +783,3 @@ This section provides a quick reference for some of the common tools in PCL. __ Octree_ Top_ - diff --git a/examples/common/example_check_if_point_is_valid.cpp b/examples/common/example_check_if_point_is_valid.cpp index 752f47cc..ab42690b 100644 --- a/examples/common/example_check_if_point_is_valid.cpp +++ b/examples/common/example_check_if_point_is_valid.cpp @@ -37,10 +37,8 @@ * */ -#include -#include - #include +#include // for pcl::isFinite int main () diff --git a/examples/common/example_copy_point_cloud.cpp b/examples/common/example_copy_point_cloud.cpp index 6ed6b298..8b95496e 100644 --- a/examples/common/example_copy_point_cloud.cpp +++ b/examples/common/example_copy_point_cloud.cpp @@ -43,12 +43,8 @@ * members of the source PointType. */ -// STL #include -// PCL -#include -#include #include static void diff --git a/examples/common/example_get_max_min_coordinates.cpp b/examples/common/example_get_max_min_coordinates.cpp index 06d7e43f..f097a24a 100644 --- a/examples/common/example_get_max_min_coordinates.cpp +++ b/examples/common/example_get_max_min_coordinates.cpp @@ -1,6 +1,5 @@ #include #include -#include #include int diff --git a/examples/features/example_difference_of_normals.cpp b/examples/features/example_difference_of_normals.cpp index da281911..59c155bd 100644 --- a/examples/features/example_difference_of_normals.cpp +++ b/examples/features/example_difference_of_normals.cpp @@ -5,20 +5,13 @@ * @author Yani Ioannou * @date 2012-03-11 */ + #include -#include #include -#include -#include -#include -#include -#include #include #include -#include #include -#include #include #include diff --git a/examples/features/example_fast_point_feature_histograms.cpp b/examples/features/example_fast_point_feature_histograms.cpp index fbcfde26..fa2a03ec 100644 --- a/examples/features/example_fast_point_feature_histograms.cpp +++ b/examples/features/example_fast_point_feature_histograms.cpp @@ -37,12 +37,9 @@ * */ - #include -#include #include -#include #include #include diff --git a/examples/features/example_normal_estimation.cpp b/examples/features/example_normal_estimation.cpp index 597a8586..a937faf3 100644 --- a/examples/features/example_normal_estimation.cpp +++ b/examples/features/example_normal_estimation.cpp @@ -40,7 +40,6 @@ #include #include -#include #include int diff --git a/examples/features/example_point_feature_histograms.cpp b/examples/features/example_point_feature_histograms.cpp index 878c773c..68d5cf70 100644 --- a/examples/features/example_point_feature_histograms.cpp +++ b/examples/features/example_point_feature_histograms.cpp @@ -38,10 +38,8 @@ */ #include -#include #include -#include #include #include diff --git a/examples/features/example_principal_curvatures_estimation.cpp b/examples/features/example_principal_curvatures_estimation.cpp index 3ebfc106..8e91072f 100644 --- a/examples/features/example_principal_curvatures_estimation.cpp +++ b/examples/features/example_principal_curvatures_estimation.cpp @@ -37,16 +37,12 @@ * */ - #include -#include #include -#include #include #include - int main (int, char** argv) { diff --git a/examples/features/example_rift_estimation.cpp b/examples/features/example_rift_estimation.cpp index 2e97eed2..aaa4c8e2 100644 --- a/examples/features/example_rift_estimation.cpp +++ b/examples/features/example_rift_estimation.cpp @@ -37,13 +37,10 @@ * * */ -// STL + #include -// PCL #include -#include -#include #include #include #include diff --git a/examples/features/example_shape_contexts.cpp b/examples/features/example_shape_contexts.cpp index c20150ec..60ea9477 100644 --- a/examples/features/example_shape_contexts.cpp +++ b/examples/features/example_shape_contexts.cpp @@ -38,12 +38,9 @@ */ #include -#include #include -#include #include -#include #include int diff --git a/examples/features/example_spin_images.cpp b/examples/features/example_spin_images.cpp index f4d8ad40..49ab93ce 100644 --- a/examples/features/example_spin_images.cpp +++ b/examples/features/example_spin_images.cpp @@ -37,13 +37,9 @@ * */ - - #include -#include #include -#include #include #include diff --git a/examples/filters/example_extract_indices.cpp b/examples/filters/example_extract_indices.cpp index c4c769b3..ec47b82a 100644 --- a/examples/filters/example_extract_indices.cpp +++ b/examples/filters/example_extract_indices.cpp @@ -37,11 +37,8 @@ * */ -// STL #include -// PCL -#include #include int @@ -65,7 +62,7 @@ main (int, char**) indices.indices.push_back (2); pcl::ExtractIndices extract_indices; - extract_indices.setIndices (boost::make_shared (indices)); + extract_indices.setIndices (pcl::make_shared (indices)); extract_indices.setInputCloud (cloud); pcl::PointCloud::Ptr output (new pcl::PointCloud); extract_indices.filter (*output); diff --git a/examples/filters/example_remove_nan_from_point_cloud.cpp b/examples/filters/example_remove_nan_from_point_cloud.cpp index 6037061c..ba338bac 100644 --- a/examples/filters/example_remove_nan_from_point_cloud.cpp +++ b/examples/filters/example_remove_nan_from_point_cloud.cpp @@ -37,13 +37,8 @@ * */ -// STL #include -#include -// PCL -#include -#include #include int diff --git a/examples/keypoints/example_get_keypoints_indices.cpp b/examples/keypoints/example_get_keypoints_indices.cpp index d7e726c6..e6822ca1 100644 --- a/examples/keypoints/example_get_keypoints_indices.cpp +++ b/examples/keypoints/example_get_keypoints_indices.cpp @@ -37,10 +37,6 @@ #include -#include -#include -#include -#include #include #include diff --git a/examples/keypoints/example_sift_keypoint_estimation.cpp b/examples/keypoints/example_sift_keypoint_estimation.cpp index 09d9df3f..8e335022 100644 --- a/examples/keypoints/example_sift_keypoint_estimation.cpp +++ b/examples/keypoints/example_sift_keypoint_estimation.cpp @@ -38,18 +38,11 @@ * */ -// STL #include -// PCL #include -#include -#include #include -#include -#include -// #include - + int main(int, char** argv) { diff --git a/examples/keypoints/example_sift_normal_keypoint_estimation.cpp b/examples/keypoints/example_sift_normal_keypoint_estimation.cpp index 7c2a7b48..d15f2054 100644 --- a/examples/keypoints/example_sift_normal_keypoint_estimation.cpp +++ b/examples/keypoints/example_sift_normal_keypoint_estimation.cpp @@ -38,16 +38,11 @@ * */ -// STL #include -// PCL #include -#include -#include #include #include -// #include /* This example shows how to estimate the SIFT points based on the * Normal gradients i.e. curvature than using the Intensity gradient diff --git a/examples/keypoints/example_sift_z_keypoint_estimation.cpp b/examples/keypoints/example_sift_z_keypoint_estimation.cpp index def4a492..df29ca3e 100644 --- a/examples/keypoints/example_sift_z_keypoint_estimation.cpp +++ b/examples/keypoints/example_sift_z_keypoint_estimation.cpp @@ -38,16 +38,10 @@ * */ -// STL #include -// PCL #include -#include -#include #include -#include -// #include /* This examples shows how to estimate the SIFT points based on the * z gradient of the 3D points than using the Intensity gradient as diff --git a/examples/outofcore/example_outofcore.cpp b/examples/outofcore/example_outofcore.cpp index ebf291cf..0e72dec2 100644 --- a/examples/outofcore/example_outofcore.cpp +++ b/examples/outofcore/example_outofcore.cpp @@ -37,14 +37,9 @@ */ #include -#include - -#include -#include #include #include -#include using namespace pcl::outofcore; diff --git a/examples/outofcore/example_outofcore_with_lod.cpp b/examples/outofcore/example_outofcore_with_lod.cpp index 3dafd480..6906fe0d 100644 --- a/examples/outofcore/example_outofcore_with_lod.cpp +++ b/examples/outofcore/example_outofcore_with_lod.cpp @@ -37,16 +37,10 @@ */ #include -#include #include #include -#include - -#include -#include - using namespace pcl::outofcore; using OctreeDisk = OutofcoreOctreeBase, pcl::PointXYZ>; diff --git a/examples/segmentation/example_cpc_segmentation.cpp b/examples/segmentation/example_cpc_segmentation.cpp index bdd29409..354c9285 100644 --- a/examples/segmentation/example_cpc_segmentation.cpp +++ b/examples/segmentation/example_cpc_segmentation.cpp @@ -35,33 +35,17 @@ * */ -// Stdlib #include -#include -#include #include #include -#include -// PCL input/output #include #include #include -#include -//PCL other -#include -#include - -// The segmentation class this example is for #include -// VTK -#include -#include -#include -#include #include using namespace std::chrono_literals; diff --git a/examples/segmentation/example_extract_clusters_normals.cpp b/examples/segmentation/example_extract_clusters_normals.cpp index 684cf2a3..b91477cd 100644 --- a/examples/segmentation/example_extract_clusters_normals.cpp +++ b/examples/segmentation/example_extract_clusters_normals.cpp @@ -38,18 +38,12 @@ * */ -// STL #include -// PCL -#include #include -#include #include -#include #include - int main (int, char **argv) { diff --git a/examples/segmentation/example_lccp_segmentation.cpp b/examples/segmentation/example_lccp_segmentation.cpp index e7e0dcb7..7d5603f7 100644 --- a/examples/segmentation/example_lccp_segmentation.cpp +++ b/examples/segmentation/example_lccp_segmentation.cpp @@ -35,33 +35,17 @@ * */ -// Stdlib #include -#include -#include #include #include - -// PCL input/output #include #include #include -#include - -//PCL other -#include -#include -// The segmentation class this example is for #include -// VTK -#include -#include -#include -#include #include using namespace std::chrono_literals; diff --git a/examples/segmentation/example_region_growing.cpp b/examples/segmentation/example_region_growing.cpp index 12e36836..13389603 100644 --- a/examples/segmentation/example_region_growing.cpp +++ b/examples/segmentation/example_region_growing.cpp @@ -37,17 +37,12 @@ * */ -// STL #include -// PCL #include #include -#include -#include #include #include -#include #include #include diff --git a/examples/segmentation/example_supervoxels.cpp b/examples/segmentation/example_supervoxels.cpp index 97ffa55b..fbfabca9 100644 --- a/examples/segmentation/example_supervoxels.cpp +++ b/examples/segmentation/example_supervoxels.cpp @@ -1,9 +1,6 @@ #include -#include #include -#include -#include #include #include #include diff --git a/examples/surface/example_nurbs_fitting_closed_curve.cpp b/examples/surface/example_nurbs_fitting_closed_curve.cpp index 842037cb..65342497 100644 --- a/examples/surface/example_nurbs_fitting_closed_curve.cpp +++ b/examples/surface/example_nurbs_fitting_closed_curve.cpp @@ -3,8 +3,6 @@ #include #include -#include -#include #include #include diff --git a/examples/surface/example_nurbs_fitting_closed_curve3d.cpp b/examples/surface/example_nurbs_fitting_closed_curve3d.cpp index 7032bbe3..c6da7235 100644 --- a/examples/surface/example_nurbs_fitting_closed_curve3d.cpp +++ b/examples/surface/example_nurbs_fitting_closed_curve3d.cpp @@ -1,8 +1,6 @@ #include #include -#include -#include #include #include diff --git a/examples/surface/example_nurbs_fitting_curve2d.cpp b/examples/surface/example_nurbs_fitting_curve2d.cpp index 9e986784..f66b050c 100644 --- a/examples/surface/example_nurbs_fitting_curve2d.cpp +++ b/examples/surface/example_nurbs_fitting_curve2d.cpp @@ -1,8 +1,6 @@ #include #include -#include -#include #include #include diff --git a/examples/surface/example_nurbs_fitting_surface.cpp b/examples/surface/example_nurbs_fitting_surface.cpp index b018d5bb..0cbda86a 100644 --- a/examples/surface/example_nurbs_fitting_surface.cpp +++ b/examples/surface/example_nurbs_fitting_surface.cpp @@ -1,5 +1,4 @@ #include -#include #include #include diff --git a/examples/surface/example_nurbs_viewer_surface.cpp b/examples/surface/example_nurbs_viewer_surface.cpp index e63019ca..3a71431a 100644 --- a/examples/surface/example_nurbs_viewer_surface.cpp +++ b/examples/surface/example_nurbs_viewer_surface.cpp @@ -1,10 +1,6 @@ #include -#include -#include #include -#include -#include #include using Point = pcl::PointXYZ; diff --git a/examples/surface/test_nurbs_fitting_surface.cpp b/examples/surface/test_nurbs_fitting_surface.cpp index 56c21f51..74a52083 100644 --- a/examples/surface/test_nurbs_fitting_surface.cpp +++ b/examples/surface/test_nurbs_fitting_surface.cpp @@ -1,6 +1,4 @@ #include -#include -#include #include #include diff --git a/features/include/pcl/features/feature.h b/features/include/pcl/features/feature.h index 1d54d609..4a24a658 100644 --- a/features/include/pcl/features/feature.h +++ b/features/include/pcl/features/feature.h @@ -41,10 +41,11 @@ #pragma once #if defined __GNUC__ -# pragma GCC system_header +# pragma GCC system_header #endif // PCL includes +#include #include #include #include @@ -133,7 +134,7 @@ namespace pcl search_parameter_(0), search_radius_(0), k_(0), fake_surface_(false) {} - + /** \brief Empty destructor */ virtual ~Feature () {} @@ -329,7 +330,7 @@ namespace pcl /** \brief Empty constructor. */ FeatureFromNormals () : normals_ () {} - + /** \brief Empty destructor */ virtual ~FeatureFromNormals () {} @@ -392,7 +393,7 @@ namespace pcl { k_ = 1; // Search tree is not always used here. } - + /** \brief Empty destructor */ virtual ~FeatureFromLabels () {} diff --git a/features/include/pcl/features/from_meshes.h b/features/include/pcl/features/from_meshes.h index b7e6baba..8563a0e1 100644 --- a/features/include/pcl/features/from_meshes.h +++ b/features/include/pcl/features/from_meshes.h @@ -57,7 +57,7 @@ namespace pcl * \param[in] cloud Point cloud containing the XYZ coordinates, * \param[in] normals Point cloud containing the corresponding surface normals. * \param[out] covariances Vector of computed covariances. - * \param[in] Optional: Epsilon for the expected noise along the surface normal (default: 0.001) + * \param[in] epsilon Optional: Epsilon for the expected noise along the surface normal (default: 0.001) */ template inline void computeApproximateCovariances(const pcl::PointCloud& cloud, diff --git a/features/include/pcl/features/gasd.h b/features/include/pcl/features/gasd.h index 6936fb13..c0e39748 100644 --- a/features/include/pcl/features/gasd.h +++ b/features/include/pcl/features/gasd.h @@ -139,8 +139,8 @@ namespace pcl shape_interp_ = interp; } - /** \brief Returns the transformation aligning the point cloud to the canonical coordinate system - * \param[out] trans transformation + /** + * \brief Returns the transformation aligning the point cloud to the canonical coordinate system */ const Eigen::Matrix4f& getTransform () const @@ -338,7 +338,7 @@ namespace pcl /** \brief copy computed color histograms to output descriptor point cloud * \param[in] grid_size size of the regular grid used to compute the descriptor * \param[in] hists_size size of the color histograms - * \param[in,out] hists color histograms, which are finalized, since they are circular + * \param[in,out] hists color histograms, which are finalized, since they are circular * \param[out] output output descriptor point cloud * \param[in,out] pos current position of output descriptor point cloud */ diff --git a/features/include/pcl/features/impl/3dsc.hpp b/features/include/pcl/features/impl/3dsc.hpp index 7b9386ff..d8da789e 100644 --- a/features/include/pcl/features/impl/3dsc.hpp +++ b/features/include/pcl/features/impl/3dsc.hpp @@ -36,14 +36,17 @@ * */ -#ifndef PCL_FEATURES_IMPL_3DSC_HPP_ -#define PCL_FEATURES_IMPL_3DSC_HPP_ +#pragma once -#include #include -#include -#include + #include +#include +#include // for pcl::isFinite +#include + +#include + ////////////////////////////////////////////////////////////////////////////////////////////// template bool @@ -272,4 +275,3 @@ pcl::ShapeContext3DEstimation::computeFeature (Poi #define PCL_INSTANTIATE_ShapeContext3DEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::ShapeContext3DEstimation; -#endif diff --git a/features/include/pcl/features/impl/board.hpp b/features/include/pcl/features/impl/board.hpp index fcd1a416..91080cc7 100644 --- a/features/include/pcl/features/impl/board.hpp +++ b/features/include/pcl/features/impl/board.hpp @@ -189,7 +189,7 @@ pcl::BOARDLocalReferenceFrameEstimation::computePo //extract support points for Rz radius std::vector neighbours_indices; std::vector neighbours_distances; - int n_neighbours = this->searchForNeighbors (index, search_parameter_, neighbours_indices, neighbours_distances); + std::size_t n_neighbours = this->searchForNeighbors (index, search_parameter_, neighbours_indices, neighbours_distances); //check if there are enough neighbor points, otherwise compute a random X axis and use normal as Z axis if (n_neighbours < 6) @@ -206,7 +206,7 @@ pcl::BOARDLocalReferenceFrameEstimation::computePo //copy neighbours coordinates into eigen matrix Eigen::Matrix neigh_points_mat (n_neighbours, 3); - for (int i = 0; i < n_neighbours; ++i) + for (std::size_t i = 0; i < n_neighbours; ++i) { neigh_points_mat.row (i) = (*surface_)[neighbours_indices[i]].getVector3fMap (); } @@ -311,7 +311,7 @@ pcl::BOARDLocalReferenceFrameEstimation::computePo if (!margin_point_found) { //find among points with neighDistance <= marginThresh*radius - for (int curr_neigh = 0; curr_neigh < n_neighbours; curr_neigh++) + for (std::size_t curr_neigh = 0; curr_neigh < n_neighbours; curr_neigh++) { const int& curr_neigh_idx = neighbours_indices[curr_neigh]; const float& neigh_distance_sqr = neighbours_distances[curr_neigh]; @@ -448,7 +448,7 @@ pcl::BOARDLocalReferenceFrameEstimation::computePo float max_hole_prob = -std::numeric_limits::max (); //find holes - for (auto ch = first_no_border; ch < check_margin_array_size_; ch++) + for (auto ch = first_no_border; ch < static_cast(check_margin_array_size_); ch++) { if (!check_margin_array_[ch]) { diff --git a/features/include/pcl/features/impl/boundary.hpp b/features/include/pcl/features/impl/boundary.hpp index 9410a5b2..ff551328 100644 --- a/features/include/pcl/features/impl/boundary.hpp +++ b/features/include/pcl/features/impl/boundary.hpp @@ -38,12 +38,14 @@ * */ -#ifndef PCL_FEATURES_IMPL_BOUNDARY_H_ -#define PCL_FEATURES_IMPL_BOUNDARY_H_ +#pragma once #include +#include // for pcl::isFinite + #include + ////////////////////////////////////////////////////////////////////////////////////////////// template bool pcl::BoundaryEstimation::isBoundaryPoint ( @@ -169,5 +171,3 @@ pcl::BoundaryEstimation::computeFeature (PointClou #define PCL_INSTANTIATE_BoundaryEstimation(PointInT,PointNT,PointOutT) template class PCL_EXPORTS pcl::BoundaryEstimation; -#endif // PCL_FEATURES_IMPL_BOUNDARY_H_ - diff --git a/features/include/pcl/features/impl/brisk_2d.hpp b/features/include/pcl/features/impl/brisk_2d.hpp index 0ee753e3..bbf36879 100644 --- a/features/include/pcl/features/impl/brisk_2d.hpp +++ b/features/include/pcl/features/impl/brisk_2d.hpp @@ -37,12 +37,16 @@ * */ + #ifndef PCL_FEATURES_IMPL_BRISK_2D_HPP_ #define PCL_FEATURES_IMPL_BRISK_2D_HPP_ -/////////////////////////////////////////////////////////////////////////////////////////// + +namespace pcl +{ + template -pcl::BRISK2DEstimation::BRISK2DEstimation () +BRISK2DEstimation::BRISK2DEstimation () : rotation_invariance_enabled_ (true) , scale_invariance_enabled_ (true) , pattern_scale_ (1.0f) @@ -81,21 +85,21 @@ pcl::BRISK2DEstimation::BRISK2DEstim generateKernel (r_list, n_list, 5.85f * pattern_scale_, 8.2f * pattern_scale_); } -/////////////////////////////////////////////////////////////////////////////////////////// + template -pcl::BRISK2DEstimation::~BRISK2DEstimation () +BRISK2DEstimation::~BRISK2DEstimation () { - if (pattern_points_) delete [] pattern_points_; - if (short_pairs_) delete [] short_pairs_; - if (long_pairs_) delete [] long_pairs_; - if (scale_list_) delete [] scale_list_; - if (size_list_) delete [] size_list_; + delete [] pattern_points_; + delete [] short_pairs_; + delete [] long_pairs_; + delete [] scale_list_; + delete [] size_list_; } -/////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::BRISK2DEstimation::generateKernel ( - std::vector &radius_list, +BRISK2DEstimation::generateKernel ( + std::vector &radius_list, std::vector &number_list, float d_max, float d_min, std::vector index_change) { @@ -132,15 +136,15 @@ pcl::BRISK2DEstimation::generateKern { // this is the rotation of the feature double theta = double (rot) * 2 * M_PI / double (n_rot_); - for (int ring = 0; ring < rings; ++ring) + for (int ring = 0; ring < static_cast(rings); ++ring) { for (int num = 0; num < number_list[ring]; ++num) { // the actual coordinates on the circle 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)); + pattern_iterator->x = scale_list_[scale] * radius_list[ring] * static_cast (std::cos (alpha + theta)); pattern_iterator->y = scale_list_[scale] * radius_list[ring] * static_cast (sin (alpha + theta)); // and the gaussian kernel sigma if (ring == 0) @@ -210,9 +214,9 @@ pcl::BRISK2DEstimation::generateKern strings_ = int (std::ceil ((float (no_short_pairs_)) / 128.0)) * 4 * 4; } -/////////////////////////////////////////////////////////////////////////////////////////// + template inline int -pcl::BRISK2DEstimation::smoothedIntensity ( +BRISK2DEstimation::smoothedIntensity ( const std::vector &image, int image_width, int, //const Stefan& integral, @@ -243,26 +247,26 @@ pcl::BRISK2DEstimation::smoothedInte const int r_y = static_cast ((yf - float (y)) * 1024); const int r_x_1 = (1024 - r_x); const int r_y_1 = (1024 - r_y); - + //+const unsigned char* ptr = static_cast (&image.points[0].r) + x + y * imagecols; const unsigned char* ptr = static_cast(&image[0]) + x + y * imagecols; - + // just interpolate: ret_val = (r_x_1 * r_y_1 * int (*ptr)); - + //+ptr += sizeof (PointInT); ptr++; ret_val += (r_x * r_y_1 * int (*ptr)); - + //+ptr += (imagecols * sizeof (PointInT)); ptr += imagecols; - + ret_val += (r_x * r_y * int (*ptr)); - + //+ptr -= sizeof (PointInT); ptr--; - + ret_val += (r_x_1 * r_y * int (*ptr)); return (ret_val + 512) / 1024; } @@ -306,32 +310,32 @@ pcl::BRISK2DEstimation::smoothedInte if (dx + dy > 2) { // now the calculation: - + //+const unsigned char* ptr = static_cast (&image.points[0].r) + x_left + imagecols * y_top; const unsigned char* ptr = static_cast(&image[0]) + x_left + imagecols * y_top; // first the corners: ret_val = A * int (*ptr); - + //+ptr += (dx + 1) * sizeof (PointInT); ptr += dx + 1; - + ret_val += B * int (*ptr); - + //+ptr += (dy * imagecols + 1) * sizeof (PointInT); ptr += dy * imagecols + 1; - + ret_val += C * int (*ptr); - + //+ptr -= (dx + 1) * sizeof (PointInT); ptr -= dx + 1; - + ret_val += D * int (*ptr); // next the edges: //+int* ptr_integral;// = static_cast (integral.data) + x_left + integralcols * y_top + 1; const int* ptr_integral = static_cast (&integral_image[0]) + x_left + integralcols * y_top + 1; - + // find a simple path through the different surface corners const int tmp1 = (*ptr_integral); ptr_integral += dx; @@ -368,16 +372,16 @@ pcl::BRISK2DEstimation::smoothedInte } // now the calculation: - + //const unsigned char* ptr = static_cast (&image.points[0].r) + x_left + imagecols * y_top; const unsigned char* ptr = static_cast(&image[0]) + x_left + imagecols * y_top; - + // first row: ret_val = A * int (*ptr); - + //+ptr += sizeof (PointInT); ptr++; - + //+const unsigned char* end1 = ptr + (dx * sizeof (PointInT)); const unsigned char* end1 = ptr + dx; @@ -385,25 +389,25 @@ pcl::BRISK2DEstimation::smoothedInte for (; ptr < end1; ptr++) ret_val += r_y_1_i * int (*ptr); ret_val += B * int (*ptr); - + // middle ones: //+ptr += (imagecols - dx - 1) * sizeof (PointInT); ptr += imagecols - dx - 1; - + //+const unsigned char* end_j = ptr + (dy * imagecols) * sizeof (PointInT); const unsigned char* end_j = ptr + dy * imagecols; - + //+for (; ptr < end_j; ptr += (imagecols - dx - 1) * sizeof (PointInT)) for (; ptr < end_j; ptr += imagecols - dx - 1) { ret_val += r_x_1_i * int (*ptr); - + //+ptr += sizeof (PointInT); ptr++; - + //+const unsigned char* end2 = ptr + (dx * sizeof (PointInT)); const unsigned char* end2 = ptr + dx; - + //+for (; ptr < end2; ptr += sizeof (PointInT)) for (; ptr < end2; ptr++) ret_val += int (*ptr) * scaling; @@ -412,10 +416,10 @@ pcl::BRISK2DEstimation::smoothedInte } // last row: ret_val += D * int (*ptr); - + //+ptr += sizeof (PointInT); ptr++; - + //+const unsigned char* end3 = ptr + (dx * sizeof (PointInT)); const unsigned char* end3 = ptr + dx; @@ -429,29 +433,28 @@ pcl::BRISK2DEstimation::smoothedInte } -////////////////////////////////////////////////////////////////////////////// template bool -pcl::BRISK2DEstimation::RoiPredicate ( +BRISK2DEstimation::RoiPredicate ( const float min_x, const float min_y, const float max_x, const float max_y, const KeypointT& pt) { return ((pt.x < min_x) || (pt.x >= max_x) || (pt.y < min_y) || (pt.y >= max_y)); } -/////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::BRISK2DEstimation::compute ( +BRISK2DEstimation::compute ( PointCloudOutT &output) { if (!input_cloud_->isOrganized ()) - { + { PCL_ERROR ("[pcl::%s::initCompute] %s doesn't support non organized clouds!\n", name_.c_str ()); return; } // image size - const int width = int (input_cloud_->width); - const int height = int (input_cloud_->height); + const index_t width = static_cast(input_cloud_->width); + const index_t height = static_cast(input_cloud_->height); // destination for intensity data; will be forwarded to BRISK std::vector image_data (width*height); @@ -463,13 +466,13 @@ pcl::BRISK2DEstimation::compute ( std::size_t ksize = keypoints_->points.size (); std::vector kscales; // remember the scale per keypoint kscales.resize (ksize); - + // initialize constants static const float lb_scalerange = std::log2 (scalerange_); typename std::vector >::iterator beginning = keypoints_->points.begin (); std::vector::iterator beginningkscales = kscales.begin (); - + static const float basic_size_06 = basic_size_ * 0.6f; unsigned int basicscale = 0; @@ -518,9 +521,9 @@ pcl::BRISK2DEstimation::compute ( // current integral image std::vector integral ((width+1)*(height+1), 0); // the integral image - for (std::size_t row_index = 1; row_index < height; ++row_index) + for (index_t row_index = 1; row_index < height; ++row_index) { - for (std::size_t col_index = 1; col_index < width; ++col_index) + for (index_t col_index = 1; col_index < width; ++col_index) { const std::size_t index = row_index*width+col_index; const std::size_t index2 = (row_index)*(width+1)+(col_index); @@ -635,12 +638,12 @@ pcl::BRISK2DEstimation::compute ( // now iterate through all the pairings UINT32_ALIAS* ptr2 = reinterpret_cast (ptr); const BriskShortPair* max = short_pairs_ + no_short_pairs_; - + for (BriskShortPair* iter = short_pairs_; iter < max; ++iter) { t1 = *(values + iter->i); t2 = *(values + iter->j); - + if (t1 > t2) *ptr2 |= ((1) << shifter); @@ -656,7 +659,7 @@ pcl::BRISK2DEstimation::compute ( } //ptr += strings_; - + //// Account for the scale + orientation; //ptr += sizeof (output.points[0].scale); //ptr += sizeof (output.points[0].orientation); @@ -671,6 +674,7 @@ pcl::BRISK2DEstimation::compute ( delete [] values; } +} // namespace pcl #endif //#ifndef PCL_FEATURES_IMPL_BRISK_2D_HPP_ diff --git a/features/include/pcl/features/impl/cppf.hpp b/features/include/pcl/features/impl/cppf.hpp index 74782d4f..deabc696 100755 --- a/features/include/pcl/features/impl/cppf.hpp +++ b/features/include/pcl/features/impl/cppf.hpp @@ -71,7 +71,8 @@ pcl::CPPFEstimation::computeFeature (PointCloudOut { PointOutT p; // No need to calculate feature for identity pair (i, j) as they aren't used in future calculations - if (i != j) + // @TODO: resolve issue with comparison in a better manner + if (static_cast(i) != j) { if ( pcl::computeCPPFPairFeature (input_->points[i].getVector4fMap (), diff --git a/features/include/pcl/features/impl/feature.hpp b/features/include/pcl/features/impl/feature.hpp index 5e3a4670..c0c94317 100644 --- a/features/include/pcl/features/impl/feature.hpp +++ b/features/include/pcl/features/impl/feature.hpp @@ -43,11 +43,14 @@ #include -////////////////////////////////////////////////////////////////////////////////////////////// + +namespace pcl +{ + inline void -pcl::solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix, - const Eigen::Vector4f &point, - Eigen::Vector4f &plane_parameters, float &curvature) +solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix, + const Eigen::Vector4f &point, + Eigen::Vector4f &plane_parameters, float &curvature) { solvePlaneParameters (covariance_matrix, plane_parameters [0], plane_parameters [1], plane_parameters [2], curvature); @@ -56,10 +59,10 @@ pcl::solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix, plane_parameters[3] = -1 * plane_parameters.dot (point); } -////////////////////////////////////////////////////////////////////////////////////////////// + inline void -pcl::solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix, - float &nx, float &ny, float &nz, float &curvature) +solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix, + float &nx, float &ny, float &nz, float &curvature) { // Avoid getting hung on Eigen's optimizers // for (int i = 0; i < 9; ++i) @@ -86,11 +89,9 @@ pcl::solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix, curvature = 0; } -////////////////////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////////////////////// + template bool -pcl::Feature::initCompute () +Feature::initCompute () { if (!PCLBase::initCompute ()) { @@ -122,9 +123,9 @@ pcl::Feature::initCompute () else tree_.reset (new pcl::search::KdTree (false)); } - + if (tree_->getInputCloud () != surface_) // Make sure the tree searches the surface - tree_->setInputCloud (surface_); + tree_->setInputCloud (surface_); // Do a fast check to see if the search parameters are well defined @@ -174,9 +175,9 @@ pcl::Feature::initCompute () return (true); } -////////////////////////////////////////////////////////////////////////////////////////////// + template bool -pcl::Feature::deinitCompute () +Feature::deinitCompute () { // Reset the surface if (fake_surface_) @@ -187,9 +188,9 @@ pcl::Feature::deinitCompute () return (true); } -////////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::Feature::compute (PointCloudOut &output) +Feature::compute (PointCloudOut &output) { if (!initCompute ()) { @@ -225,11 +226,9 @@ pcl::Feature::compute (PointCloudOut &output) deinitCompute (); } -////////////////////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////////////////////// + template bool -pcl::FeatureFromNormals::initCompute () +FeatureFromNormals::initCompute () { if (!Feature::initCompute ()) { @@ -258,11 +257,9 @@ pcl::FeatureFromNormals::initCompute () return (true); } -////////////////////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////////////////////// + template bool -pcl::FeatureFromLabels::initCompute () +FeatureFromLabels::initCompute () { if (!Feature::initCompute ()) { @@ -289,12 +286,10 @@ pcl::FeatureFromLabels::initCompute () return (true); } -////////////////////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////////////////////// + template bool -pcl::FeatureWithLocalReferenceFrames::initLocalReferenceFrames (const std::size_t& indices_size, - const LRFEstimationPtr& lrf_estimation) +FeatureWithLocalReferenceFrames::initLocalReferenceFrames (const std::size_t& indices_size, + const LRFEstimationPtr& lrf_estimation) { if (frames_never_defined_) frames_.reset (); @@ -334,5 +329,7 @@ pcl::FeatureWithLocalReferenceFrames::initLocalReferenceFram return (true); } +} // namespace pcl + #endif //#ifndef PCL_FEATURES_IMPL_FEATURE_H_ diff --git a/features/include/pcl/features/impl/flare.hpp b/features/include/pcl/features/impl/flare.hpp index fa53eff4..ca58c39c 100644 --- a/features/include/pcl/features/impl/flare.hpp +++ b/features/include/pcl/features/impl/flare.hpp @@ -81,7 +81,7 @@ templategetInputCloud () != sampled_surface_) // Make sure the tree searches the sampled surface - sampled_tree_->setInputCloud (sampled_surface_); + sampled_tree_->setInputCloud (sampled_surface_); return (true); } @@ -121,7 +121,7 @@ templatesearchForNeighbors (index, search_parameter_, neighbours_indices, neighbours_distances); - if (n_normal_neighbours < min_neighbors_for_normal_axis_) + if (n_normal_neighbours < static_cast(min_neighbors_for_normal_axis_)) { if (!pcl::isFinite ((*normals_)[index])) { @@ -158,7 +158,7 @@ templateradiusSearch ((*input_)[index], tangent_radius_, neighbours_indices, neighbours_distances); - if (n_tangent_neighbours < min_neighbors_for_tangent_axis_) + if (n_tangent_neighbours < static_cast(min_neighbors_for_tangent_axis_)) { //set X axis as a random axis x_axis = pcl::geometry::randomOrthogonalAxis (fitted_normal); diff --git a/features/include/pcl/features/impl/fpfh.hpp b/features/include/pcl/features/impl/fpfh.hpp index e2bf64dd..08e8ac8b 100644 --- a/features/include/pcl/features/impl/fpfh.hpp +++ b/features/include/pcl/features/impl/fpfh.hpp @@ -38,12 +38,14 @@ * */ -#ifndef PCL_FEATURES_IMPL_FPFH_H_ -#define PCL_FEATURES_IMPL_FPFH_H_ +#pragma once #include + +#include // for pcl::isFinite #include + ////////////////////////////////////////////////////////////////////////////////////////////// template bool pcl::FPFHEstimation::computePairFeatures ( @@ -57,7 +59,7 @@ pcl::FPFHEstimation::computePairFeatures ( } ////////////////////////////////////////////////////////////////////////////////////////////// -template void +template void pcl::FPFHEstimation::computePointSPFHSignature ( const pcl::PointCloud &cloud, const pcl::PointCloud &normals, int p_idx, int row, const std::vector &indices, @@ -133,21 +135,21 @@ pcl::FPFHEstimation::weightPointSPFHSignature ( weight = 1.0f / dists[idx]; // Weight the SPFH of the query point with the SPFH of its neighbors - for (std::size_t f1_i = 0; f1_i < nr_bins_f1; ++f1_i) + for (Eigen::MatrixXf::Index f1_i = 0; f1_i < nr_bins_f1; ++f1_i) { val_f1 = hist_f1 (indices[idx], f1_i) * weight; sum_f1 += val_f1; fpfh_histogram[f1_i] += val_f1; } - for (std::size_t f2_i = 0; f2_i < nr_bins_f2; ++f2_i) + for (Eigen::MatrixXf::Index f2_i = 0; f2_i < nr_bins_f2; ++f2_i) { val_f2 = hist_f2 (indices[idx], f2_i) * weight; sum_f2 += val_f2; fpfh_histogram[f2_i + nr_bins_f1] += val_f2; } - for (std::size_t f3_i = 0; f3_i < nr_bins_f3; ++f3_i) + for (Eigen::MatrixXf::Index f3_i = 0; f3_i < nr_bins_f3; ++f3_i) { val_f3 = hist_f3 (indices[idx], f3_i) * weight; sum_f3 += val_f3; @@ -258,7 +260,7 @@ pcl::FPFHEstimation::computeFeature (PointCloudOut continue; } - // ... and remap the nn_indices values so that they represent row indices in the spfh_hist_* matrices + // ... and remap the nn_indices values so that they represent row indices in the spfh_hist_* matrices // instead of indices into surface_->points for (auto &nn_index : nn_indices) nn_index = spfh_hist_lookup[nn_index]; @@ -285,7 +287,7 @@ pcl::FPFHEstimation::computeFeature (PointCloudOut continue; } - // ... and remap the nn_indices values so that they represent row indices in the spfh_hist_* matrices + // ... and remap the nn_indices values so that they represent row indices in the spfh_hist_* matrices // instead of indices into surface_->points for (auto &nn_index : nn_indices) nn_index = spfh_hist_lookup[nn_index]; @@ -301,5 +303,3 @@ pcl::FPFHEstimation::computeFeature (PointCloudOut #define PCL_INSTANTIATE_FPFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::FPFHEstimation; -#endif // PCL_FEATURES_IMPL_FPFH_H_ - diff --git a/features/include/pcl/features/impl/fpfh_omp.hpp b/features/include/pcl/features/impl/fpfh_omp.hpp index c0015a79..4001f894 100644 --- a/features/include/pcl/features/impl/fpfh_omp.hpp +++ b/features/include/pcl/features/impl/fpfh_omp.hpp @@ -38,12 +38,14 @@ * */ -#ifndef PCL_FEATURES_IMPL_FPFH_OMP_H_ -#define PCL_FEATURES_IMPL_FPFH_OMP_H_ +#pragma once + +#include + +#include // for pcl::isFinite #include -#include ////////////////////////////////////////////////////////////////////////////////////////////// template void @@ -106,9 +108,11 @@ pcl::FPFHEstimationOMP::computeFeature (PointCloud // Compute SPFH signatures for every point that needs them -#ifdef _OPENMP -#pragma omp parallel for shared (spfh_hist_lookup) private (nn_indices, nn_dists) num_threads(threads_) -#endif +#pragma omp parallel for \ + default(none) \ + shared(spfh_hist_lookup, spfh_indices_vec) \ + private(nn_indices, nn_dists) \ + num_threads(threads_) for (std::ptrdiff_t i = 0; i < static_cast (spfh_indices_vec.size ()); ++i) { // Get the next point index @@ -133,9 +137,11 @@ pcl::FPFHEstimationOMP::computeFeature (PointCloud nn_dists.clear(); // Iterate over the entire index vector -#ifdef _OPENMP -#pragma omp parallel for shared (output) private (nn_indices, nn_dists) num_threads(threads_) -#endif +#pragma omp parallel for \ + default(none) \ + shared(nr_bins, output, spfh_hist_lookup) \ + private(nn_dists, nn_indices) \ + num_threads(threads_) for (std::ptrdiff_t idx = 0; idx < static_cast (indices_->size ()); ++idx) { // Find the indices of point idx's neighbors... @@ -168,5 +174,3 @@ pcl::FPFHEstimationOMP::computeFeature (PointCloud #define PCL_INSTANTIATE_FPFHEstimationOMP(T,NT,OutT) template class PCL_EXPORTS pcl::FPFHEstimationOMP; -#endif // PCL_FEATURES_IMPL_FPFH_OMP_H_ - diff --git a/features/include/pcl/features/impl/integral_image2D.hpp b/features/include/pcl/features/impl/integral_image2D.hpp index abf594b2..7eef7af9 100644 --- a/features/include/pcl/features/impl/integral_image2D.hpp +++ b/features/include/pcl/features/impl/integral_image2D.hpp @@ -37,21 +37,25 @@ * $Id: feature.h 2784 2011-10-15 22:05:38Z aichim $ */ + #ifndef PCL_INTEGRAL_IMAGE2D_IMPL_H_ #define PCL_INTEGRAL_IMAGE2D_IMPL_H_ #include -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +namespace pcl +{ + template void -pcl::IntegralImage2D::setSecondOrderComputation (bool compute_second_order_integral_images) +IntegralImage2D::setSecondOrderComputation (bool compute_second_order_integral_images) { compute_second_order_integral_images_ = compute_second_order_integral_images; } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::IntegralImage2D::setInput (const DataType * data, unsigned width,unsigned height, unsigned element_stride, unsigned row_stride) +IntegralImage2D::setInput (const DataType * data, unsigned width,unsigned height, unsigned element_stride, unsigned row_stride) { if ((width + 1) * (height + 1) > first_order_integral_image_.size () ) { @@ -65,9 +69,9 @@ pcl::IntegralImage2D::setInput (const DataType * data, unsi computeIntegralImages (data, row_stride, element_stride); } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template typename pcl::IntegralImage2D::ElementType -pcl::IntegralImage2D::getFirstOrderSum ( +IntegralImage2D::getFirstOrderSum ( unsigned start_x, unsigned start_y, unsigned width, unsigned height) const { const unsigned upper_left_idx = start_y * (width_ + 1) + start_x; @@ -79,9 +83,9 @@ pcl::IntegralImage2D::getFirstOrderSum ( first_order_integral_image_[upper_right_idx] - first_order_integral_image_[lower_left_idx] ); } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template typename pcl::IntegralImage2D::SecondOrderType -pcl::IntegralImage2D::getSecondOrderSum ( +IntegralImage2D::getSecondOrderSum ( unsigned start_x, unsigned start_y, unsigned width, unsigned height) const { const unsigned upper_left_idx = start_y * (width_ + 1) + start_x; @@ -93,9 +97,9 @@ pcl::IntegralImage2D::getSecondOrderSum ( second_order_integral_image_[upper_right_idx] - second_order_integral_image_[lower_left_idx] ); } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template unsigned -pcl::IntegralImage2D::getFiniteElementsCount ( +IntegralImage2D::getFiniteElementsCount ( unsigned start_x, unsigned start_y, unsigned width, unsigned height) const { const unsigned upper_left_idx = start_y * (width_ + 1) + start_x; @@ -107,9 +111,9 @@ pcl::IntegralImage2D::getFiniteElementsCount ( finite_values_integral_image_[upper_right_idx] - finite_values_integral_image_[lower_left_idx] ); } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template typename pcl::IntegralImage2D::ElementType -pcl::IntegralImage2D::getFirstOrderSumSE ( +IntegralImage2D::getFirstOrderSumSE ( unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const { const unsigned upper_left_idx = start_y * (width_ + 1) + start_x; @@ -121,9 +125,9 @@ pcl::IntegralImage2D::getFirstOrderSumSE ( first_order_integral_image_[upper_right_idx] - first_order_integral_image_[lower_left_idx] ); } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template typename pcl::IntegralImage2D::SecondOrderType -pcl::IntegralImage2D::getSecondOrderSumSE ( +IntegralImage2D::getSecondOrderSumSE ( unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const { const unsigned upper_left_idx = start_y * (width_ + 1) + start_x; @@ -135,9 +139,9 @@ pcl::IntegralImage2D::getSecondOrderSumSE ( second_order_integral_image_[upper_right_idx] - second_order_integral_image_[lower_left_idx] ); } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template unsigned -pcl::IntegralImage2D::getFiniteElementsCountSE ( +IntegralImage2D::getFiniteElementsCountSE ( unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const { const unsigned upper_left_idx = start_y * (width_ + 1) + start_x; @@ -149,9 +153,9 @@ pcl::IntegralImage2D::getFiniteElementsCountSE ( finite_values_integral_image_[upper_right_idx] - finite_values_integral_image_[lower_left_idx] ); } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::IntegralImage2D::computeIntegralImages ( +IntegralImage2D::computeIntegralImages ( const DataType *data, unsigned row_stride, unsigned element_stride) { ElementType* previous_row = &first_order_integral_image_[0]; @@ -218,10 +222,9 @@ pcl::IntegralImage2D::computeIntegralImages ( } } -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template void -pcl::IntegralImage2D::setInput (const DataType * data, unsigned width,unsigned height, unsigned element_stride, unsigned row_stride) +IntegralImage2D::setInput (const DataType * data, unsigned width,unsigned height, unsigned element_stride, unsigned row_stride) { if ((width + 1) * (height + 1) > first_order_integral_image_.size () ) { @@ -235,9 +238,9 @@ pcl::IntegralImage2D::setInput (const DataType * data, unsigned wid computeIntegralImages (data, row_stride, element_stride); } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template typename pcl::IntegralImage2D::ElementType -pcl::IntegralImage2D::getFirstOrderSum ( +IntegralImage2D::getFirstOrderSum ( unsigned start_x, unsigned start_y, unsigned width, unsigned height) const { const unsigned upper_left_idx = start_y * (width_ + 1) + start_x; @@ -249,9 +252,9 @@ pcl::IntegralImage2D::getFirstOrderSum ( first_order_integral_image_[upper_right_idx] - first_order_integral_image_[lower_left_idx] ); } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template typename pcl::IntegralImage2D::SecondOrderType -pcl::IntegralImage2D::getSecondOrderSum ( +IntegralImage2D::getSecondOrderSum ( unsigned start_x, unsigned start_y, unsigned width, unsigned height) const { const unsigned upper_left_idx = start_y * (width_ + 1) + start_x; @@ -263,9 +266,9 @@ pcl::IntegralImage2D::getSecondOrderSum ( second_order_integral_image_[upper_right_idx] - second_order_integral_image_[lower_left_idx] ); } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template unsigned -pcl::IntegralImage2D::getFiniteElementsCount ( +IntegralImage2D::getFiniteElementsCount ( unsigned start_x, unsigned start_y, unsigned width, unsigned height) const { const unsigned upper_left_idx = start_y * (width_ + 1) + start_x; @@ -277,9 +280,9 @@ pcl::IntegralImage2D::getFiniteElementsCount ( finite_values_integral_image_[upper_right_idx] - finite_values_integral_image_[lower_left_idx] ); } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template typename pcl::IntegralImage2D::ElementType -pcl::IntegralImage2D::getFirstOrderSumSE ( +IntegralImage2D::getFirstOrderSumSE ( unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const { const unsigned upper_left_idx = start_y * (width_ + 1) + start_x; @@ -291,9 +294,9 @@ pcl::IntegralImage2D::getFirstOrderSumSE ( first_order_integral_image_[upper_right_idx] - first_order_integral_image_[lower_left_idx] ); } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template typename pcl::IntegralImage2D::SecondOrderType -pcl::IntegralImage2D::getSecondOrderSumSE ( +IntegralImage2D::getSecondOrderSumSE ( unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const { const unsigned upper_left_idx = start_y * (width_ + 1) + start_x; @@ -305,9 +308,9 @@ pcl::IntegralImage2D::getSecondOrderSumSE ( second_order_integral_image_[upper_right_idx] - second_order_integral_image_[lower_left_idx] ); } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template unsigned -pcl::IntegralImage2D::getFiniteElementsCountSE ( +IntegralImage2D::getFiniteElementsCountSE ( unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const { const unsigned upper_left_idx = start_y * (width_ + 1) + start_x; @@ -319,9 +322,9 @@ pcl::IntegralImage2D::getFiniteElementsCountSE ( finite_values_integral_image_[upper_right_idx] - finite_values_integral_image_[lower_left_idx] ); } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::IntegralImage2D::computeIntegralImages ( +IntegralImage2D::computeIntegralImages ( const DataType *data, unsigned row_stride, unsigned element_stride) { ElementType* previous_row = &first_order_integral_image_[0]; @@ -381,5 +384,8 @@ pcl::IntegralImage2D::computeIntegralImages ( } } } + +} // namespace pcl + #endif // PCL_INTEGRAL_IMAGE2D_IMPL_H_ diff --git a/features/include/pcl/features/impl/intensity_gradient.hpp b/features/include/pcl/features/impl/intensity_gradient.hpp index 3afbee42..72e51e57 100644 --- a/features/include/pcl/features/impl/intensity_gradient.hpp +++ b/features/include/pcl/features/impl/intensity_gradient.hpp @@ -38,11 +38,13 @@ * */ -#ifndef PCL_FEATURES_IMPL_INTENSITY_GRADIENT_H_ -#define PCL_FEATURES_IMPL_INTENSITY_GRADIENT_H_ +#pragma once #include +#include // for pcl::isFinite + + ////////////////////////////////////////////////////////////////////////////////////////////// template void pcl::IntensityGradientEstimation ::computePointIntensityGradient ( @@ -90,9 +92,7 @@ pcl::IntensityGradientEstimation is_dense) { -#ifdef _OPENMP -#pragma omp parallel for shared (output) private (nn_indices, nn_dists) num_threads(threads_) -#endif +#pragma omp parallel for \ + default(none) \ + shared(output) \ + private(nn_indices, nn_dists) \ + num_threads(threads_) // Iterating over the entire index vector for (std::ptrdiff_t idx = 0; idx < static_cast (indices_->size ()); ++idx) { @@ -190,9 +191,11 @@ pcl::IntensityGradientEstimation (indices_->size ()); ++idx) { @@ -234,4 +237,3 @@ pcl::IntensityGradientEstimation; -#endif // PCL_FEATURES_IMPL_INTENSITY_GRADIENT_H_ diff --git a/features/include/pcl/features/impl/normal_3d_omp.hpp b/features/include/pcl/features/impl/normal_3d_omp.hpp index a849a385..42abc887 100644 --- a/features/include/pcl/features/impl/normal_3d_omp.hpp +++ b/features/include/pcl/features/impl/normal_3d_omp.hpp @@ -70,9 +70,11 @@ pcl::NormalEstimationOMP::computeFeature (PointCloudOut &ou // Save a few cycles by not checking every point for NaN/Inf values if the cloud is set to dense if (input_->is_dense) { -#ifdef _OPENMP -#pragma omp parallel for shared (output) firstprivate (nn_indices, nn_dists) num_threads(threads_) -#endif +#pragma omp parallel for \ + default(none) \ + shared(output) \ + firstprivate(nn_indices, nn_dists) \ + num_threads(threads_) // Iterating over the entire index vector for (std::ptrdiff_t idx = 0; idx < static_cast (indices_->size ()); ++idx) { @@ -97,9 +99,11 @@ pcl::NormalEstimationOMP::computeFeature (PointCloudOut &ou } else { -#ifdef _OPENMP -#pragma omp parallel for shared (output) firstprivate (nn_indices, nn_dists) num_threads(threads_) -#endif +#pragma omp parallel for \ + default(none) \ + shared(output) \ + firstprivate(nn_indices, nn_dists) \ + num_threads(threads_) // 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/pfh.hpp b/features/include/pcl/features/impl/pfh.hpp index d0b57e84..ada95705 100644 --- a/features/include/pcl/features/impl/pfh.hpp +++ b/features/include/pcl/features/impl/pfh.hpp @@ -36,11 +36,13 @@ * */ -#ifndef PCL_FEATURES_IMPL_PFH_H_ -#define PCL_FEATURES_IMPL_PFH_H_ +#pragma once #include +#include // for pcl::isFinite + + ////////////////////////////////////////////////////////////////////////////////////////////// template bool pcl::PFHEstimation::computePairFeatures ( @@ -226,5 +228,3 @@ pcl::PFHEstimation::computeFeature (PointCloudOut #define PCL_INSTANTIATE_PFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::PFHEstimation; -#endif // PCL_FEATURES_IMPL_PFH_H_ - diff --git a/features/include/pcl/features/impl/principal_curvatures.hpp b/features/include/pcl/features/impl/principal_curvatures.hpp index bfa2499d..e119adec 100644 --- a/features/include/pcl/features/impl/principal_curvatures.hpp +++ b/features/include/pcl/features/impl/principal_curvatures.hpp @@ -38,11 +38,13 @@ * */ -#ifndef PCL_FEATURES_IMPL_PRINCIPAL_CURVATURES_H_ -#define PCL_FEATURES_IMPL_PRINCIPAL_CURVATURES_H_ +#pragma once #include +#include // for pcl::isFinite + + ////////////////////////////////////////////////////////////////////////////////////////////// template void pcl::PrincipalCurvaturesEstimation::computePointPrincipalCurvatures ( @@ -162,4 +164,3 @@ pcl::PrincipalCurvaturesEstimation::computeFeature #define PCL_INSTANTIATE_PrincipalCurvaturesEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::PrincipalCurvaturesEstimation; -#endif // PCL_FEATURES_IMPL_PRINCIPAL_CURVATURES_H_ diff --git a/features/include/pcl/features/impl/shot_lrf_omp.hpp b/features/include/pcl/features/impl/shot_lrf_omp.hpp index 93e31c4b..814bb38e 100644 --- a/features/include/pcl/features/impl/shot_lrf_omp.hpp +++ b/features/include/pcl/features/impl/shot_lrf_omp.hpp @@ -72,9 +72,10 @@ pcl::SHOTLocalReferenceFrameEstimationOMP::computeFeature ( } tree_->setSortedResults (true); -#ifdef _OPENMP -#pragma omp parallel for num_threads(threads_) -#endif +#pragma omp parallel for \ + default(none) \ + shared(output) \ + num_threads(threads_) for (std::ptrdiff_t i = 0; i < static_cast (indices_->size ()); ++i) { // point result diff --git a/features/include/pcl/features/impl/shot_omp.hpp b/features/include/pcl/features/impl/shot_omp.hpp index 5e0f39f3..681f1ae3 100644 --- a/features/include/pcl/features/impl/shot_omp.hpp +++ b/features/include/pcl/features/impl/shot_omp.hpp @@ -37,13 +37,15 @@ * */ -#ifndef PCL_FEATURES_IMPL_SHOT_OMP_H_ -#define PCL_FEATURES_IMPL_SHOT_OMP_H_ +#pragma once #include + +#include // for pcl::isFinite #include #include + template bool pcl::SHOTEstimationOMP::initCompute () { @@ -148,9 +150,10 @@ pcl::SHOTEstimationOMP::computeFeature ( output.is_dense = true; // Iterating over the entire index vector -#ifdef _OPENMP -#pragma omp parallel for num_threads(threads_) -#endif +#pragma omp parallel for \ + default(none) \ + shared(output) \ + num_threads(threads_) for (std::ptrdiff_t idx = 0; idx < static_cast (indices_->size ()); ++idx) { @@ -234,9 +237,10 @@ pcl::SHOTColorEstimationOMP::computeFeat output.is_dense = true; // Iterating over the entire index vector -#ifdef _OPENMP -#pragma omp parallel for num_threads(threads_) -#endif +#pragma omp parallel for \ + default(none) \ + shared(output) \ + num_threads(threads_) for (std::ptrdiff_t idx = 0; idx < static_cast (indices_->size ()); ++idx) { Eigen::VectorXf shot; @@ -290,4 +294,3 @@ pcl::SHOTColorEstimationOMP::computeFeat #define PCL_INSTANTIATE_SHOTEstimationOMP(T,NT,OutT,RFT) template class PCL_EXPORTS pcl::SHOTEstimationOMP; #define PCL_INSTANTIATE_SHOTColorEstimationOMP(T,NT,OutT,RFT) template class PCL_EXPORTS pcl::SHOTColorEstimationOMP; -#endif // PCL_FEATURES_IMPL_SHOT_OMP_H_ diff --git a/features/include/pcl/features/impl/usc.hpp b/features/include/pcl/features/impl/usc.hpp index da6487ac..c2065a52 100644 --- a/features/include/pcl/features/impl/usc.hpp +++ b/features/include/pcl/features/impl/usc.hpp @@ -38,15 +38,16 @@ * */ -#ifndef PCL_FEATURES_IMPL_USC_HPP_ -#define PCL_FEATURES_IMPL_USC_HPP_ +#pragma once #include #include -#include #include +#include +#include // for pcl::isFinite #include + ////////////////////////////////////////////////////////////////////////////////////////////// template bool pcl::UniqueShapeContext::initCompute () @@ -260,4 +261,3 @@ pcl::UniqueShapeContext::computeFeature (PointClo #define PCL_INSTANTIATE_UniqueShapeContext(T,OutT,RFT) template class PCL_EXPORTS pcl::UniqueShapeContext; -#endif diff --git a/features/include/pcl/features/integral_image2D.h b/features/include/pcl/features/integral_image2D.h index a69537d1..864a195f 100644 --- a/features/include/pcl/features/integral_image2D.h +++ b/features/include/pcl/features/integral_image2D.h @@ -39,9 +39,10 @@ #pragma once +#include + #include -#include namespace pcl { diff --git a/features/include/pcl/features/integral_image_normal.h b/features/include/pcl/features/integral_image_normal.h index c84987f4..5219830c 100644 --- a/features/include/pcl/features/integral_image_normal.h +++ b/features/include/pcl/features/integral_image_normal.h @@ -38,6 +38,7 @@ #pragma once +#include #include #include #include diff --git a/features/include/pcl/features/moment_of_inertia_estimation.h b/features/include/pcl/features/moment_of_inertia_estimation.h index b662d861..2526f98c 100644 --- a/features/include/pcl/features/moment_of_inertia_estimation.h +++ b/features/include/pcl/features/moment_of_inertia_estimation.h @@ -42,6 +42,7 @@ #include #include #include +#include #include #include diff --git a/features/include/pcl/features/narf.h b/features/include/pcl/features/narf.h index 10afe013..0405d8e3 100644 --- a/features/include/pcl/features/narf.h +++ b/features/include/pcl/features/narf.h @@ -38,6 +38,7 @@ #pragma once +#include #include #include #include diff --git a/features/include/pcl/features/normal_3d.h b/features/include/pcl/features/normal_3d.h index a83fc0c3..39618043 100644 --- a/features/include/pcl/features/normal_3d.h +++ b/features/include/pcl/features/normal_3d.h @@ -40,6 +40,7 @@ #pragma once +#include #include #include #include diff --git a/features/include/pcl/features/rops_estimation.h b/features/include/pcl/features/rops_estimation.h index d6792550..e2e3d357 100644 --- a/features/include/pcl/features/rops_estimation.h +++ b/features/include/pcl/features/rops_estimation.h @@ -39,6 +39,7 @@ #pragma once +#include #include #include #include diff --git a/features/include/pcl/features/rsd.h b/features/include/pcl/features/rsd.h index edda7a5c..dca37083 100644 --- a/features/include/pcl/features/rsd.h +++ b/features/include/pcl/features/rsd.h @@ -40,6 +40,7 @@ #pragma once +#include #include #include @@ -89,7 +90,7 @@ namespace pcl int nr_subdiv, double plane_radius, PointOutT &radii, bool compute_histogram = false); template - PCL_DEPRECATED("use computeRSD() overload that takes input point clouds by const reference") + PCL_DEPRECATED(1, 12, "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 +116,7 @@ namespace pcl int nr_subdiv, double plane_radius, PointOutT &radii, bool compute_histogram = false); template - PCL_DEPRECATED("use computeRSD() overload that takes input point cloud by const reference") + PCL_DEPRECATED(1, 12, "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/src/narf.cpp b/features/src/narf.cpp index 50258ddf..5a133265 100644 --- a/features/src/narf.cpp +++ b/features/src/narf.cpp @@ -371,7 +371,11 @@ void Narf::extractForInterestPoints (const RangeImage& range_image, const PointCloud& interest_points, int descriptor_size, float support_size, bool rotation_invariant, std::vector& feature_list) { - # pragma omp parallel for num_threads(max_no_of_threads) default(shared) schedule(dynamic, 10) +#pragma omp parallel for \ + default(none) \ + shared(descriptor_size, feature_list, interest_points, range_image, rotation_invariant, support_size) \ + schedule(dynamic, 10) \ + num_threads(max_no_of_threads) //!!! nizar 20110408 : for OpenMP sake on MSVC this must be kept signed for (std::ptrdiff_t idx = 0; idx < static_cast(interest_points.points.size ()); ++idx) { diff --git a/features/src/range_image_border_extractor.cpp b/features/src/range_image_border_extractor.cpp index 6a270c21..2a74d52b 100644 --- a/features/src/range_image_border_extractor.cpp +++ b/features/src/range_image_border_extractor.cpp @@ -49,7 +49,7 @@ using std::cerr; #include #include -namespace pcl +namespace pcl { /////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -67,7 +67,7 @@ RangeImageBorderExtractor::~RangeImageBorderExtractor() } /////////////////////////////////////////////////////////////////////////////////////////////////////////////// -void +void RangeImageBorderExtractor::setRangeImage (const RangeImage* range_image) { clearData (); @@ -75,7 +75,7 @@ RangeImageBorderExtractor::setRangeImage (const RangeImage* range_image) } /////////////////////////////////////////////////////////////////////////////////////////////////////////////// -void +void RangeImageBorderExtractor::clearData () { //std::cout << PVARC(range_image_size_during_extraction_)<width, - height = range_image_->height; + + const auto width = range_image_->width; + const auto height = range_image_->height; range_image_size_during_extraction_ = width*height; const auto array_size = range_image_size_during_extraction_; surface_structure_ = new LocalSurface*[array_size]; @@ -116,10 +116,25 @@ RangeImageBorderExtractor::extractLocalSurfaceStructure () const auto sqrt_neighbors = parameters_.pixel_radius_plane_extraction/step_size + 1; const auto no_of_nearest_neighbors = sqrt_neighbors * sqrt_neighbors; -# pragma omp parallel for num_threads(parameters_.max_no_of_threads) default(shared) schedule(dynamic, 10) - for (int y=0; y; + +#if OPENMP_LEGACY_CONST_DATA_SHARING_RULE +#pragma omp parallel for \ + default(none) \ + schedule(dynamic, 10) \ + num_threads(parameters_.max_no_of_threads) +#else +#pragma omp parallel for \ + default(none) \ + shared(height, no_of_nearest_neighbors, step_size, width) \ + schedule(dynamic, 10) \ + num_threads(parameters_.max_no_of_threads) +#endif + for (iteration_type y=0; ynormal_no_jumps[0]<<","<normal_no_jumps[1]<<","<normal_no_jumps[2]<<")\n"; } } } /////////////////////////////////////////////////////////////////////////////////////////////////////////////// -void +void RangeImageBorderExtractor::extractBorderScoreImages () { if (!border_scores_left_.empty()) return; - + extractLocalSurfaceStructure(); - + //MEASURE_FUNCTION_TIME; - + int width = range_image_->width, height = range_image_->height, size = width*height; @@ -163,20 +178,24 @@ RangeImageBorderExtractor::extractBorderScoreImages () border_scores_right_.resize (size); border_scores_top_.resize (size); border_scores_bottom_ .resize (size); - -# pragma omp parallel for num_threads(parameters_.max_no_of_threads) default(shared) schedule(dynamic, 10) + +#pragma omp parallel for \ + default(none) \ + shared(height, width) \ + schedule(dynamic, 10) \ + num_threads(parameters_.max_no_of_threads) for (int y=0; ywidth*range_image_->height]; float* new_scores_ptr = new_scores; - for (int y=0; y < static_cast (range_image_->height); ++y) - for (int x=0; x < static_cast (range_image_->width); ++x) + for (int y=0; y < static_cast (range_image_->height); ++y) + for (int x=0; x < static_cast (range_image_->width); ++x) *(new_scores_ptr++) = updatedScoreAccordingToNeighborValues(x, y, border_scores); return (new_scores); } @@ -209,45 +228,45 @@ RangeImageBorderExtractor::updatedScoresAccordingToNeighborValues (const std::ve } /////////////////////////////////////////////////////////////////////////////////////////////////////////////// -void +void RangeImageBorderExtractor::updateScoresAccordingToNeighborValues () { extractBorderScoreImages(); - + //MEASURE_FUNCTION_TIME; - + border_scores_left_ = updatedScoresAccordingToNeighborValues(border_scores_left_); border_scores_right_ = updatedScoresAccordingToNeighborValues(border_scores_right_); border_scores_top_ = updatedScoresAccordingToNeighborValues(border_scores_top_); border_scores_bottom_ = updatedScoresAccordingToNeighborValues(border_scores_bottom_); } - + /////////////////////////////////////////////////////////////////////////////////////////////////////////////// -void +void RangeImageBorderExtractor::findAndEvaluateShadowBorders () { if (shadow_border_informations_ != nullptr) return; - + if (border_scores_left_.empty ()) { std::cerr << __PRETTY_FUNCTION__<<": border score images not available!\n"; } - + //MEASURE_FUNCTION_TIME; - + int width = range_image_->width, height = range_image_->height; shadow_border_informations_ = new ShadowBorderIndices*[width*height]; - for (int y = 0; y < static_cast (height); ++y) + for (int y = 0; y < static_cast (height); ++y) { - for (int x = 0; x < static_cast (width); ++x) + for (int x = 0; x < static_cast (width); ++x) { int index = y*width+x; ShadowBorderIndices*& shadow_border_indices = shadow_border_informations_[index]; shadow_border_indices = nullptr; int shadow_border_idx; - + if (changeScoreAccordingToShadowBorderValue(x, y, -1, 0, border_scores_left_.data (), border_scores_right_.data (), shadow_border_idx)) { shadow_border_indices = (shadow_border_indices==nullptr ? new ShadowBorderIndices : shadow_border_indices); @@ -273,16 +292,16 @@ RangeImageBorderExtractor::findAndEvaluateShadowBorders () } /////////////////////////////////////////////////////////////////////////////////////////////////////////////// -float* +float* RangeImageBorderExtractor::getAnglesImageForBorderDirections () { calculateBorderDirections(); - + int width = range_image_->width, height = range_image_->height, array_size = width*height; float* angles_image = new float[array_size]; - + for (int y=0; ygetPoint(index); - + float border_direction_in_image_x, border_direction_in_image_y; float tmp_factor = point.range*range_image_->getAngularResolution(); range_image_->getImagePoint(point.x+tmp_factor*border_direction[0], point.y+tmp_factor*border_direction[1], point.z+tmp_factor*border_direction[2], @@ -308,18 +327,18 @@ RangeImageBorderExtractor::getAnglesImageForBorderDirections () } /////////////////////////////////////////////////////////////////////////////////////////////////////////////// -float* +float* RangeImageBorderExtractor::getAnglesImageForSurfaceChangeDirections () { //MEASURE_FUNCTION_TIME; - + calculateSurfaceChanges(); - + int width = range_image_->width, height = range_image_->height, array_size = width*height; float* angles_image = new float[array_size]; - + for (int y=0; ygetPoint(index); - + float border_direction_in_image_x, border_direction_in_image_y; float tmp_factor = point.range*range_image_->getAngularResolution(); range_image_->getImagePoint(point.x+tmp_factor*direction[0], point.y+tmp_factor*direction[1], point.z+tmp_factor*direction[2], @@ -350,28 +369,28 @@ RangeImageBorderExtractor::getAnglesImageForSurfaceChangeDirections () /////////////////////////////////////////////////////////////////////////////////////////////////////////////// -void +void RangeImageBorderExtractor::classifyBorders () { if (border_descriptions_ != nullptr) return; - + // Get local plane approximations extractLocalSurfaceStructure(); - + // Get scores for every point, describing how probable a border in that direction is extractBorderScoreImages(); - + // Propagate values to neighboring pixels updateScoresAccordingToNeighborValues(); - + // Change border score according to the existence of a shadow border findAndEvaluateShadowBorders(); - + int width = range_image_->width, height = range_image_->height, size = width*height; - + BorderDescription initial_border_description; initial_border_description.traits = 0; border_descriptions_ = new PointCloudOut; @@ -379,21 +398,21 @@ RangeImageBorderExtractor::classifyBorders () border_descriptions_->height = height; border_descriptions_->is_dense = true; border_descriptions_->points.resize(size, initial_border_description); - - for (int y = 0; y < static_cast (height); ++y) + + for (int y = 0; y < static_cast (height); ++y) { - for (int x = 0; x < static_cast (width); ++x) + for (int x = 0; x < static_cast (width); ++x) { int index = y*width+x; BorderDescription& border_description = border_descriptions_->points[index]; border_description.x = x; border_description.y = y; BorderTraits& border_traits = border_description.traits; - + ShadowBorderIndices* shadow_border_indices = shadow_border_informations_[index]; if (shadow_border_indices == nullptr) continue; - + int shadow_border_index = shadow_border_indices->left; if (shadow_border_index >= 0 && checkIfMaximum(x, y, -1, 0, border_scores_left_.data (), shadow_border_index)) { @@ -406,7 +425,7 @@ RangeImageBorderExtractor::classifyBorders () veil_point[BORDER_TRAIT__VEIL_POINT] = veil_point[BORDER_TRAIT__VEIL_POINT_RIGHT] = true; } } - + shadow_border_index = shadow_border_indices->right; if (shadow_border_index >= 0 && checkIfMaximum(x, y, 1, 0, border_scores_right_.data (), shadow_border_index)) { @@ -419,7 +438,7 @@ RangeImageBorderExtractor::classifyBorders () veil_point[BORDER_TRAIT__VEIL_POINT] = veil_point[BORDER_TRAIT__VEIL_POINT_LEFT] = true; } } - + shadow_border_index = shadow_border_indices->top; if (shadow_border_index >= 0 && checkIfMaximum(x, y, 0, -1, border_scores_top_.data (), shadow_border_index)) { @@ -433,7 +452,7 @@ RangeImageBorderExtractor::classifyBorders () veil_point[BORDER_TRAIT__VEIL_POINT] = veil_point[BORDER_TRAIT__VEIL_POINT_BOTTOM] = true; } } - + shadow_border_index = shadow_border_indices->bottom; if (shadow_border_index >= 0 && checkIfMaximum(x, y, 0, 1, border_scores_bottom_.data (), shadow_border_index)) { @@ -447,7 +466,7 @@ RangeImageBorderExtractor::classifyBorders () veil_point[BORDER_TRAIT__VEIL_POINT] = veil_point[BORDER_TRAIT__VEIL_POINT_TOP] = true; } } - + //if (border_traits[BORDER_TRAIT__OBSTACLE_BORDER]) //{ //border_points.push_back(&border_description); @@ -457,15 +476,15 @@ RangeImageBorderExtractor::classifyBorders () } /////////////////////////////////////////////////////////////////////////////////////////////////////////////// -void +void RangeImageBorderExtractor::calculateBorderDirections () { if (border_directions_!=nullptr) return; classifyBorders(); - + //MEASURE_FUNCTION_TIME; - + int width = range_image_->width, height = range_image_->height, size = width*height; @@ -477,7 +496,7 @@ RangeImageBorderExtractor::calculateBorderDirections () calculateBorderDirection(x, y); } } - + Eigen::Vector3f** average_border_directions = new Eigen::Vector3f*[size]; int radius = parameters_.pixel_radius_border_direction; int minimum_weight = radius+1; @@ -502,7 +521,7 @@ RangeImageBorderExtractor::calculateBorderDirections () const Eigen::Vector3f* neighbor_border_direction = border_directions_[index2]; if (neighbor_border_direction==nullptr || index2==index) continue; - + // Opposite directions? float cos_angle = neighbor_border_direction->dot(*border_direction); if (cos_angle= 0.95f*parameters_.minimum_border_probability) continue; - + *average_border_direction += *neighbor_border_direction; weight_sum += 1.0f; } @@ -531,7 +550,7 @@ RangeImageBorderExtractor::calculateBorderDirections () average_border_direction->normalize(); } } - + for (int i=0; iwidth, height = range_image_->height, size = width*height; surface_change_scores_ = new float[size]; surface_change_directions_ = new Eigen::Vector3f[size]; -# pragma omp parallel for num_threads(parameters_.max_no_of_threads) default(shared) schedule(dynamic, 10) +#pragma omp parallel for \ + default(none) \ + shared(height, width) \ + schedule(dynamic, 10) \ + num_threads(parameters_.max_no_of_threads) for (int y=0; ypoints[index].traits; if (border_traits[BORDER_TRAIT__VEIL_POINT] || border_traits[BORDER_TRAIT__SHADOW_BORDER]) continue; @@ -588,13 +611,13 @@ RangeImageBorderExtractor::calculateSurfaceChanges () } /////////////////////////////////////////////////////////////////////////////////////////////////////////////// -void +void RangeImageBorderExtractor::blurSurfaceChanges () { int blur_radius = 1; - + const RangeImage& range_image = *range_image_; - + Eigen::Vector3f* blurred_directions = new Eigen::Vector3f[range_image.width*range_image.height]; float* blurred_scores = new float[range_image.width*range_image.height]; for (int y=0; y #include -#include -#include #include #include #include diff --git a/filters/include/pcl/filters/box_clipper3D.h b/filters/include/pcl/filters/box_clipper3D.h index 0e68a411..baf4a6df 100644 --- a/filters/include/pcl/filters/box_clipper3D.h +++ b/filters/include/pcl/filters/box_clipper3D.h @@ -37,6 +37,7 @@ #pragma once +#include #include #include "clipper3D.h" diff --git a/filters/include/pcl/filters/clipper3D.h b/filters/include/pcl/filters/clipper3D.h index 0196ca08..b741ff7e 100644 --- a/filters/include/pcl/filters/clipper3D.h +++ b/filters/include/pcl/filters/clipper3D.h @@ -37,6 +37,7 @@ #pragma once +#include #include #include #include diff --git a/filters/include/pcl/filters/conditional_removal.h b/filters/include/pcl/filters/conditional_removal.h index 7fba1e9f..df7b58a5 100644 --- a/filters/include/pcl/filters/conditional_removal.h +++ b/filters/include/pcl/filters/conditional_removal.h @@ -37,6 +37,7 @@ #pragma once +#include #include #include #include diff --git a/filters/include/pcl/filters/convolution.h b/filters/include/pcl/filters/convolution.h index 3658a3d5..ff2c61c8 100644 --- a/filters/include/pcl/filters/convolution.h +++ b/filters/include/pcl/filters/convolution.h @@ -40,7 +40,6 @@ #pragma once #include -#include #include #include #include diff --git a/filters/include/pcl/filters/covariance_sampling.h b/filters/include/pcl/filters/covariance_sampling.h index 5bba6dd0..76231a24 100644 --- a/filters/include/pcl/filters/covariance_sampling.h +++ b/filters/include/pcl/filters/covariance_sampling.h @@ -40,6 +40,7 @@ #pragma once +#include #include #include diff --git a/filters/include/pcl/filters/crop_box.h b/filters/include/pcl/filters/crop_box.h index 3ba04e4e..4beee5a9 100644 --- a/filters/include/pcl/filters/crop_box.h +++ b/filters/include/pcl/filters/crop_box.h @@ -70,7 +70,7 @@ namespace pcl * \param[in] extract_removed_indices Set to true if you want to be able to extract the indices of points being removed (default = false). */ CropBox (bool extract_removed_indices = false) : - FilterIndices::FilterIndices (extract_removed_indices), + FilterIndices (extract_removed_indices), min_pt_ (Eigen::Vector4f (-1, -1, -1, 1)), max_pt_ (Eigen::Vector4f (1, 1, 1, 1)), rotation_ (Eigen::Vector3f::Zero ()), @@ -174,12 +174,6 @@ namespace pcl using FilterIndices::extract_removed_indices_; using FilterIndices::removed_indices_; - /** \brief Sample of point indices into a separate PointCloud - * \param[out] output the resultant point cloud - */ - void - applyFilter (PointCloud &output) override; - /** \brief Sample of point indices * \param[out] indices the resultant point cloud indices */ diff --git a/filters/include/pcl/filters/filter.h b/filters/include/pcl/filters/filter.h index 159573cd..2759857e 100644 --- a/filters/include/pcl/filters/filter.h +++ b/filters/include/pcl/filters/filter.h @@ -83,9 +83,6 @@ namespace pcl class Filter : public PCLBase { public: - using PCLBase::indices_; - using PCLBase::input_; - using Ptr = shared_ptr >; using ConstPtr = shared_ptr >; @@ -104,9 +101,6 @@ namespace pcl { } - /** \brief Empty destructor */ - ~Filter () {} - /** \brief Get the point indices being removed */ inline IndicesConstPtr const getRemovedIndices () const @@ -154,6 +148,9 @@ namespace pcl protected: + using PCLBase::indices_; + using PCLBase::input_; + using PCLBase::initCompute; using PCLBase::deinitCompute; @@ -209,9 +206,6 @@ namespace pcl { } - /** \brief Empty destructor */ - ~Filter () {} - /** \brief Get the point indices being removed */ inline IndicesConstPtr const getRemovedIndices () const diff --git a/filters/include/pcl/filters/filter_indices.h b/filters/include/pcl/filters/filter_indices.h index bd87ae1c..12374b35 100644 --- a/filters/include/pcl/filters/filter_indices.h +++ b/filters/include/pcl/filters/filter_indices.h @@ -85,29 +85,19 @@ namespace pcl * \param[in] extract_removed_indices Set to true if you want to be able to extract the indices of points being removed (default = false). */ FilterIndices (bool extract_removed_indices = false) : - negative_ (false), - keep_organized_ (false), + Filter (extract_removed_indices), + negative_ (false), + keep_organized_ (false), user_filter_value_ (std::numeric_limits::quiet_NaN ()) - { - extract_removed_indices_ = extract_removed_indices; - } - - /** \brief Empty virtual destructor. */ - - ~FilterIndices () { } - inline void - filter (PointCloud &output) - { - pcl::Filter::filter (output); - } + using Filter::filter; /** \brief Calls the filtering method and returns the filtered point cloud indices. * \param[out] indices the resultant filtered point cloud indices */ - inline void + void filter (std::vector &indices) { if (!initCompute ()) @@ -171,6 +161,8 @@ namespace pcl using Filter::initCompute; using Filter::deinitCompute; + using Filter::input_; + using Filter::removed_indices_; /** \brief False = normal filter behavior (default), true = inverted behavior. */ bool negative_; @@ -187,7 +179,7 @@ namespace pcl /** \brief Abstract filter method for point cloud. */ void - applyFilter (PointCloud &output) override = 0; + applyFilter (PointCloud &output) override; }; ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -210,24 +202,14 @@ namespace pcl * \param[in] extract_removed_indices Set to true if you want to extract the indices of points being removed (default = false). */ FilterIndices (bool extract_removed_indices = false) : + Filter (extract_removed_indices), negative_ (false), keep_organized_ (false), user_filter_value_ (std::numeric_limits::quiet_NaN ()) - { - extract_removed_indices_ = extract_removed_indices; - } - - /** \brief Empty virtual destructor. */ - - ~FilterIndices () { } - virtual void - filter (PCLPointCloud2 &output) - { - pcl::Filter::filter (output); - } + using Filter::filter; /** \brief Calls the filtering method and returns the filtered point cloud indices. * \param[out] indices the resultant filtered point cloud indices diff --git a/filters/include/pcl/filters/frustum_culling.h b/filters/include/pcl/filters/frustum_culling.h index 18b64a88..1e06be3b 100644 --- a/filters/include/pcl/filters/frustum_culling.h +++ b/filters/include/pcl/filters/frustum_culling.h @@ -37,6 +37,7 @@ #pragma once +#include #include #include #include @@ -89,7 +90,7 @@ namespace pcl using Filter::getClassName; FrustumCulling (bool extract_removed_indices = false) - : FilterIndices::FilterIndices (extract_removed_indices) + : FilterIndices (extract_removed_indices) , camera_pose_ (Eigen::Matrix4f::Identity ()) , hfov_ (60.0f) , vfov_ (60.0f) @@ -204,12 +205,6 @@ namespace pcl using FilterIndices::extract_removed_indices_; using FilterIndices::removed_indices_; - /** \brief Sample of point indices into a separate PointCloud - * \param[out] output the resultant point cloud - */ - void - applyFilter (PointCloud &output) override; - /** \brief Sample of point indices * \param[out] indices the resultant point cloud indices */ diff --git a/filters/include/pcl/filters/impl/conditional_removal.hpp b/filters/include/pcl/filters/impl/conditional_removal.hpp index bce677a9..6f2c9207 100644 --- a/filters/include/pcl/filters/impl/conditional_removal.hpp +++ b/filters/include/pcl/filters/impl/conditional_removal.hpp @@ -90,11 +90,7 @@ pcl::FieldComparison::FieldComparison ( template pcl::FieldComparison::~FieldComparison () { - if (point_data_ != nullptr) - { - delete point_data_; - point_data_ = nullptr; - } + delete point_data_; } ////////////////////////////////////////////////////////////////////////// diff --git a/filters/include/pcl/filters/impl/convolution.hpp b/filters/include/pcl/filters/impl/convolution.hpp index bf2817d0..4a2f56af 100644 --- a/filters/include/pcl/filters/impl/convolution.hpp +++ b/filters/include/pcl/filters/impl/convolution.hpp @@ -3,6 +3,7 @@ * * Point Cloud Library (PCL) - www.pointclouds.org * Copyright (c) 2010-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. * * All rights reserved. * @@ -37,14 +38,20 @@ * */ -#ifndef PCL_FILTERS_CONVOLUTION_IMPL_HPP -#define PCL_FILTERS_CONVOLUTION_IMPL_HPP +#pragma once #include #include +#include // for pcl::isFinite + + +namespace pcl +{ +namespace filters +{ template -pcl::filters::Convolution::Convolution () +Convolution::Convolution () : borders_policy_ (BORDERS_POLICY_IGNORE) , distance_threshold_ (std::numeric_limits::infinity ()) , input_ () @@ -54,7 +61,7 @@ pcl::filters::Convolution::Convolution () {} template void -pcl::filters::Convolution::initCompute (PointCloud& output) +Convolution::initCompute (PointCloud& output) { if (borders_policy_ != BORDERS_POLICY_IGNORE && borders_policy_ != BORDERS_POLICY_MIRROR && @@ -85,7 +92,7 @@ pcl::filters::Convolution::initCompute (PointCloud& } template inline void -pcl::filters::Convolution::convolveRows (PointCloudOut& output) +Convolution::convolveRows (PointCloudOut& output) { try { @@ -105,7 +112,7 @@ pcl::filters::Convolution::convolveRows (PointCloudOut& outpu } template inline void -pcl::filters::Convolution::convolveCols (PointCloudOut& output) +Convolution::convolveCols (PointCloudOut& output) { try { @@ -125,9 +132,9 @@ pcl::filters::Convolution::convolveCols (PointCloudOut& outpu } template inline void -pcl::filters::Convolution::convolve (const Eigen::ArrayXf& h_kernel, - const Eigen::ArrayXf& v_kernel, - PointCloud& output) +Convolution::convolve (const Eigen::ArrayXf& h_kernel, + const Eigen::ArrayXf& v_kernel, + PointCloud& output) { try { @@ -146,7 +153,7 @@ pcl::filters::Convolution::convolve (const Eigen::ArrayXf& h_ } template inline void -pcl::filters::Convolution::convolve (PointCloud& output) +Convolution::convolve (PointCloud& output) { try { @@ -163,7 +170,7 @@ pcl::filters::Convolution::convolve (PointCloud& ou } template inline PointOut -pcl::filters::Convolution::convolveOneRowDense (int i, int j) +Convolution::convolveOneRowDense (int i, int j) { using namespace pcl::common; PointOut result; @@ -173,7 +180,7 @@ pcl::filters::Convolution::convolveOneRowDense (int i, int j) } template inline PointOut -pcl::filters::Convolution::convolveOneColDense (int i, int j) +Convolution::convolveOneColDense (int i, int j) { using namespace pcl::common; PointOut result; @@ -183,7 +190,7 @@ pcl::filters::Convolution::convolveOneColDense (int i, int j) } template inline PointOut -pcl::filters::Convolution::convolveOneRowNonDense (int i, int j) +Convolution::convolveOneRowNonDense (int i, int j) { using namespace pcl::common; PointOut result; @@ -209,7 +216,7 @@ pcl::filters::Convolution::convolveOneRowNonDense (int i, int } template inline PointOut -pcl::filters::Convolution::convolveOneColNonDense (int i, int j) +Convolution::convolveOneColNonDense (int i, int j) { using namespace pcl::common; PointOut result; @@ -234,174 +241,44 @@ pcl::filters::Convolution::convolveOneColNonDense (int i, int return (result); } -namespace pcl -{ - namespace filters - { - template<> pcl::PointXYZRGB - Convolution::convolveOneRowDense (int i, int j) - { - pcl::PointXYZRGB result; - float r = 0, g = 0, b = 0; - for (int k = kernel_width_, l = i - half_width_; k > -1; --k, ++l) - { - result.x += (*input_) (l,j).x * kernel_[k]; - result.y += (*input_) (l,j).y * kernel_[k]; - result.z += (*input_) (l,j).z * kernel_[k]; - r += kernel_[k] * static_cast ((*input_) (l,j).r); - g += kernel_[k] * static_cast ((*input_) (l,j).g); - b += kernel_[k] * static_cast ((*input_) (l,j).b); - } - result.r = static_cast (r); - result.g = static_cast (g); - result.b = static_cast (b); - return (result); - } +template<> pcl::PointXYZRGB +Convolution::convolveOneRowDense (int i, int j); - template<> pcl::PointXYZRGB - Convolution::convolveOneColDense (int i, int j) - { - pcl::PointXYZRGB result; - float r = 0, g = 0, b = 0; - for (int k = kernel_width_, l = j - half_width_; k > -1; --k, ++l) - { - result.x += (*input_) (i,l).x * kernel_[k]; - result.y += (*input_) (i,l).y * kernel_[k]; - result.z += (*input_) (i,l).z * kernel_[k]; - r += kernel_[k] * static_cast ((*input_) (i,l).r); - g += kernel_[k] * static_cast ((*input_) (i,l).g); - b += kernel_[k] * static_cast ((*input_) (i,l).b); - } - result.r = static_cast (r); - result.g = static_cast (g); - result.b = static_cast (b); - return (result); - } +template<> pcl::PointXYZRGB +Convolution::convolveOneColDense (int i, int j); - template<> pcl::PointXYZRGB - Convolution::convolveOneRowNonDense (int i, int j) - { - pcl::PointXYZRGB result; - float weight = 0; - float r = 0, g = 0, b = 0; - for (int k = kernel_width_, l = i - half_width_; k > -1; --k, ++l) - { - if (!isFinite ((*input_) (l,j))) - continue; - if (pcl::squaredEuclideanDistance ((*input_) (i,j), (*input_) (l,j)) < distance_threshold_) - { - result.x += (*input_) (l,j).x * kernel_[k]; result.y += (*input_) (l,j).y * kernel_[k]; result.z += (*input_) (l,j).z * kernel_[k]; - r+= kernel_[k] * static_cast ((*input_) (l,j).r); - g+= kernel_[k] * static_cast ((*input_) (l,j).g); - b+= kernel_[k] * static_cast ((*input_) (l,j).b); - weight += kernel_[k]; - } - } - - if (weight == 0) - result.x = result.y = result.z = std::numeric_limits::quiet_NaN (); - else - { - weight = 1.f/weight; - r*= weight; g*= weight; b*= weight; - result.x*= weight; result.y*= weight; result.z*= weight; - result.r = static_cast (r); - result.g = static_cast (g); - result.b = static_cast (b); - } - return (result); - } +template<> pcl::PointXYZRGB +Convolution::convolveOneRowNonDense (int i, int j); - template<> pcl::PointXYZRGB - Convolution::convolveOneColNonDense (int i, int j) - { - pcl::PointXYZRGB result; - float weight = 0; - float r = 0, g = 0, b = 0; - for (int k = kernel_width_, l = j - half_width_; k > -1; --k, ++l) - { - if (!isFinite ((*input_) (i,l))) - continue; - if (pcl::squaredEuclideanDistance ((*input_) (i,j), (*input_) (i,l)) < distance_threshold_) - { - result.x += (*input_) (i,l).x * kernel_[k]; result.y += (*input_) (i,l).y * kernel_[k]; result.z += (*input_) (i,l).z * kernel_[k]; - r+= kernel_[k] * static_cast ((*input_) (i,l).r); - g+= kernel_[k] * static_cast ((*input_) (i,l).g); - b+= kernel_[k] * static_cast ((*input_) (i,l).b); - weight+= kernel_[k]; - } - } - if (weight == 0) - result.x = result.y = result.z = std::numeric_limits::quiet_NaN (); - else - { - weight = 1.f/weight; - r*= weight; g*= weight; b*= weight; - result.x*= weight; result.y*= weight; result.z*= weight; - result.r = static_cast (r); - result.g = static_cast (g); - result.b = static_cast (b); - } - return (result); - } +template<> pcl::PointXYZRGB +Convolution::convolveOneColNonDense (int i, int j); - /////////////////////////////////////////////////////////////////////////////////////////////// - template<> pcl::RGB - Convolution::convolveOneRowDense (int i, int j) - { - pcl::RGB result; - float r = 0, g = 0, b = 0; - for (int k = kernel_width_, l = i - half_width_; k > -1; --k, ++l) - { - r += kernel_[k] * static_cast ((*input_) (l,j).r); - g += kernel_[k] * static_cast ((*input_) (l,j).g); - b += kernel_[k] * static_cast ((*input_) (l,j).b); - } - result.r = static_cast (r); - result.g = static_cast (g); - result.b = static_cast (b); - return (result); - } +template<> pcl::RGB +Convolution::convolveOneRowDense (int i, int j); - template<> pcl::RGB - Convolution::convolveOneColDense (int i, int j) - { - pcl::RGB result; - float r = 0, g = 0, b = 0; - for (int k = kernel_width_, l = j - half_width_; k > -1; --k, ++l) - { - r += kernel_[k] * static_cast ((*input_) (i,l).r); - g += kernel_[k] * static_cast ((*input_) (i,l).g); - b += kernel_[k] * static_cast ((*input_) (i,l).b); - } - result.r = static_cast (r); - result.g = static_cast (g); - result.b = static_cast (b); - return (result); - } +template<> pcl::RGB +Convolution::convolveOneColDense (int i, int j); - template<> pcl::RGB - Convolution::convolveOneRowNonDense (int i, int j) - { - return (convolveOneRowDense (i,j)); - } +template<> inline pcl::RGB +Convolution::convolveOneRowNonDense (int i, int j) +{ + return (convolveOneRowDense (i,j)); +} - template<> pcl::RGB - Convolution::convolveOneColNonDense (int i, int j) - { - return (convolveOneColDense (i,j)); - } +template<> inline pcl::RGB +Convolution::convolveOneColNonDense (int i, int j) +{ + return (convolveOneColDense (i,j)); +} - template<> void - Convolution::makeInfinite (pcl::RGB& p) - { - p.r = 0; p.g = 0; p.b = 0; - } - } +template<> inline void +Convolution::makeInfinite (pcl::RGB& p) +{ + p.r = 0; p.g = 0; p.b = 0; } template void -pcl::filters::Convolution::convolve_rows (PointCloudOut& output) +Convolution::convolve_rows (PointCloudOut& output) { using namespace pcl::common; @@ -410,9 +287,10 @@ pcl::filters::Convolution::convolve_rows (PointCloudOut& outp int last = input_->width - half_width_; if (input_->is_dense) { -#ifdef _OPENMP -#pragma omp parallel for shared (output) num_threads (threads_) -#endif +#pragma omp parallel for \ + default(none) \ + shared(height, last, output, width) \ + num_threads(threads_) for(int j = 0; j < height; ++j) { for (int i = 0; i < half_width_; ++i) @@ -427,9 +305,10 @@ pcl::filters::Convolution::convolve_rows (PointCloudOut& outp } else { -#ifdef _OPENMP -#pragma omp parallel for shared (output) num_threads (threads_) -#endif +#pragma omp parallel for \ + default(none) \ + shared(height, last, output, width) \ + num_threads(threads_) for(int j = 0; j < height; ++j) { for (int i = 0; i < half_width_; ++i) @@ -445,7 +324,7 @@ pcl::filters::Convolution::convolve_rows (PointCloudOut& outp } template void -pcl::filters::Convolution::convolve_rows_duplicate (PointCloudOut& output) +Convolution::convolve_rows_duplicate (PointCloudOut& output) { using namespace pcl::common; @@ -455,9 +334,10 @@ pcl::filters::Convolution::convolve_rows_duplicate (PointClou int w = last - 1; if (input_->is_dense) { -#ifdef _OPENMP -#pragma omp parallel for shared (output) num_threads (threads_) -#endif +#pragma omp parallel for \ + default(none) \ + shared(height, last, output, w, width) \ + num_threads(threads_) for(int j = 0; j < height; ++j) { for (int i = half_width_; i < last; ++i) @@ -472,9 +352,10 @@ pcl::filters::Convolution::convolve_rows_duplicate (PointClou } else { -#ifdef _OPENMP -#pragma omp parallel for shared (output) num_threads (threads_) -#endif +#pragma omp parallel for \ + default(none) \ + shared(height, last, output, w, width) \ + num_threads(threads_) for(int j = 0; j < height; ++j) { for (int i = half_width_; i < last; ++i) @@ -490,7 +371,7 @@ pcl::filters::Convolution::convolve_rows_duplicate (PointClou } template void -pcl::filters::Convolution::convolve_rows_mirror (PointCloudOut& output) +Convolution::convolve_rows_mirror (PointCloudOut& output) { using namespace pcl::common; @@ -500,9 +381,10 @@ pcl::filters::Convolution::convolve_rows_mirror (PointCloudOu int w = last - 1; if (input_->is_dense) { -#ifdef _OPENMP -#pragma omp parallel for shared (output) num_threads (threads_) -#endif +#pragma omp parallel for \ + default(none) \ + shared(height, last, output, w, width) \ + num_threads(threads_) for(int j = 0; j < height; ++j) { for (int i = half_width_; i < last; ++i) @@ -517,9 +399,10 @@ pcl::filters::Convolution::convolve_rows_mirror (PointCloudOu } else { -#ifdef _OPENMP -#pragma omp parallel for shared (output) num_threads (threads_) -#endif +#pragma omp parallel for \ + default(none) \ + shared(height, last, output, w, width) \ + num_threads(threads_) for(int j = 0; j < height; ++j) { for (int i = half_width_; i < last; ++i) @@ -535,7 +418,7 @@ pcl::filters::Convolution::convolve_rows_mirror (PointCloudOu } template void -pcl::filters::Convolution::convolve_cols (PointCloudOut& output) +Convolution::convolve_cols (PointCloudOut& output) { using namespace pcl::common; @@ -544,9 +427,10 @@ pcl::filters::Convolution::convolve_cols (PointCloudOut& outp int last = input_->height - half_width_; if (input_->is_dense) { -#ifdef _OPENMP -#pragma omp parallel for shared (output) num_threads (threads_) -#endif +#pragma omp parallel for \ + default(none) \ + shared(height, last, output, width) \ + num_threads(threads_) for(int i = 0; i < width; ++i) { for (int j = 0; j < half_width_; ++j) @@ -561,9 +445,10 @@ pcl::filters::Convolution::convolve_cols (PointCloudOut& outp } else { -#ifdef _OPENMP -#pragma omp parallel for shared (output) num_threads (threads_) -#endif +#pragma omp parallel for \ + default(none) \ + shared(height, last, output, width) \ + num_threads(threads_) for(int i = 0; i < width; ++i) { for (int j = 0; j < half_width_; ++j) @@ -579,7 +464,7 @@ pcl::filters::Convolution::convolve_cols (PointCloudOut& outp } template void -pcl::filters::Convolution::convolve_cols_duplicate (PointCloudOut& output) +Convolution::convolve_cols_duplicate (PointCloudOut& output) { using namespace pcl::common; @@ -589,9 +474,10 @@ pcl::filters::Convolution::convolve_cols_duplicate (PointClou int h = last -1; if (input_->is_dense) { -#ifdef _OPENMP -#pragma omp parallel for shared (output) num_threads (threads_) -#endif +#pragma omp parallel for \ + default(none) \ + shared(h, height, last, output, width) \ + num_threads(threads_) for(int i = 0; i < width; ++i) { for (int j = half_width_; j < last; ++j) @@ -606,9 +492,10 @@ pcl::filters::Convolution::convolve_cols_duplicate (PointClou } else { -#ifdef _OPENMP -#pragma omp parallel for shared (output) num_threads (threads_) -#endif +#pragma omp parallel for \ + default(none) \ + shared(h, height, last, output, width) \ + num_threads(threads_) for(int i = 0; i < width; ++i) { for (int j = half_width_; j < last; ++j) @@ -624,7 +511,7 @@ pcl::filters::Convolution::convolve_cols_duplicate (PointClou } template void -pcl::filters::Convolution::convolve_cols_mirror (PointCloudOut& output) +Convolution::convolve_cols_mirror (PointCloudOut& output) { using namespace pcl::common; @@ -634,9 +521,10 @@ pcl::filters::Convolution::convolve_cols_mirror (PointCloudOu int h = last -1; if (input_->is_dense) { -#ifdef _OPENMP -#pragma omp parallel for shared (output) num_threads (threads_) -#endif +#pragma omp parallel for \ + default(none) \ + shared(h, height, last, output, width) \ + num_threads(threads_) for(int i = 0; i < width; ++i) { for (int j = half_width_; j < last; ++j) @@ -651,9 +539,10 @@ pcl::filters::Convolution::convolve_cols_mirror (PointCloudOu } else { -#ifdef _OPENMP -#pragma omp parallel for shared (output) num_threads (threads_) -#endif +#pragma omp parallel for \ + default(none) \ + shared(h, height, last, output, width) \ + num_threads(threads_) for(int i = 0; i < width; ++i) { for (int j = half_width_; j < last; ++j) @@ -668,4 +557,6 @@ pcl::filters::Convolution::convolve_cols_mirror (PointCloudOu } } -#endif //PCL_FILTERS_CONVOLUTION_IMPL_HPP +} // namespace filters +} // namespace pcl + diff --git a/filters/include/pcl/filters/impl/convolution_3d.hpp b/filters/include/pcl/filters/impl/convolution_3d.hpp index 2421ddad..eb7cafb3 100644 --- a/filters/include/pcl/filters/impl/convolution_3d.hpp +++ b/filters/include/pcl/filters/impl/convolution_3d.hpp @@ -42,7 +42,6 @@ #include #include -#include #include #include @@ -242,9 +241,11 @@ pcl::filters::Convolution3D::convolve (PointCloudO std::vector nn_indices; std::vector nn_distances; -#ifdef _OPENMP -#pragma omp parallel for shared (output) private (nn_indices, nn_distances) num_threads (threads_) -#endif +#pragma omp parallel for \ + default(none) \ + shared(output) \ + private(nn_indices, nn_distances) \ + num_threads(threads_) for (std::int64_t point_idx = 0; point_idx < static_cast (surface_->size ()); ++point_idx) { const PointInT& point_in = surface_->points [point_idx]; diff --git a/filters/include/pcl/filters/impl/crop_box.hpp b/filters/include/pcl/filters/impl/crop_box.hpp index 97f5aacb..aff1d608 100644 --- a/filters/include/pcl/filters/impl/crop_box.hpp +++ b/filters/include/pcl/filters/impl/crop_box.hpp @@ -44,32 +44,6 @@ #include #include -/////////////////////////////////////////////////////////////////////////////// -template void -pcl::CropBox::applyFilter (PointCloud &output) -{ - std::vector indices; - if (keep_organized_) - { - bool temp = extract_removed_indices_; - extract_removed_indices_ = true; - applyFilter (indices); - extract_removed_indices_ = temp; - - output = *input_; - for (const auto ri : *removed_indices_) // ri = removed index - output.points[ri].x = output.points[ri].y = output.points[ri].z = user_filter_value_; - if (!std::isfinite (user_filter_value_)) - output.is_dense = false; - } - else - { - output.is_dense = true; - applyFilter (indices); - pcl::copyPointCloud (*input_, indices, output); - } -} - /////////////////////////////////////////////////////////////////////////////// template void pcl::CropBox::applyFilter (std::vector &indices) diff --git a/filters/include/pcl/filters/impl/fast_bilateral.hpp b/filters/include/pcl/filters/impl/fast_bilateral.hpp index bddfc655..f5301778 100644 --- a/filters/include/pcl/filters/impl/fast_bilateral.hpp +++ b/filters/include/pcl/filters/impl/fast_bilateral.hpp @@ -37,14 +37,18 @@ * $Id$ * */ + #ifndef PCL_FILTERS_IMPL_FAST_BILATERAL_HPP_ #define PCL_FILTERS_IMPL_FAST_BILATERAL_HPP_ #include -////////////////////////////////////////////////////////////////////////////////////////////// + +namespace pcl +{ + template void -pcl::FastBilateralFilter::applyFilter (PointCloud &output) +FastBilateralFilter::applyFilter (PointCloud &output) { if (!input_->isOrganized ()) { @@ -164,12 +168,10 @@ pcl::FastBilateralFilter::applyFilter (PointCloud &output) } - -////////////////////////////////////////////////////////////////////////////////////////////// template std::size_t -pcl::FastBilateralFilter::Array3D::clamp (const std::size_t min_value, - const std::size_t max_value, - const std::size_t x) +FastBilateralFilter::Array3D::clamp (const std::size_t min_value, + const std::size_t max_value, + const std::size_t x) { if (x >= min_value && x <= max_value) { @@ -182,11 +184,11 @@ pcl::FastBilateralFilter::Array3D::clamp (const std::size_t min_value, return (max_value); } -////////////////////////////////////////////////////////////////////////////////////////////// + template Eigen::Vector2f -pcl::FastBilateralFilter::Array3D::trilinear_interpolation (const float x, - const float y, - const float z) +FastBilateralFilter::Array3D::trilinear_interpolation (const float x, + const float y, + const float z) { const std::size_t x_index = clamp (0, x_dim_ - 1, static_cast (x)); const std::size_t xx_index = clamp (0, x_dim_ - 1, x_index + 1); @@ -212,4 +214,7 @@ pcl::FastBilateralFilter::Array3D::trilinear_interpolation (const float x_alpha * y_alpha * z_alpha * (*this)(xx_index, yy_index, zz_index); } +} // namespace pcl + #endif /* PCL_FILTERS_IMPL_FAST_BILATERAL_HPP_ */ + diff --git a/filters/include/pcl/filters/impl/fast_bilateral_omp.hpp b/filters/include/pcl/filters/impl/fast_bilateral_omp.hpp index d56d4656..4bfe5fd7 100644 --- a/filters/include/pcl/filters/impl/fast_bilateral_omp.hpp +++ b/filters/include/pcl/filters/impl/fast_bilateral_omp.hpp @@ -91,9 +91,10 @@ pcl::FastBilateralFilterOMP::applyFilter (PointCloud &output) PCL_WARN ("[pcl::FastBilateralFilterOMP] Given an empty cloud. Doing nothing.\n"); return; } -#ifdef _OPENMP -#pragma omp parallel for num_threads (threads_) -#endif +#pragma omp parallel for \ + default(none) \ + shared(base_min, base_max, output) \ + num_threads(threads_) for (long int i = 0; i < static_cast (output.size ()); ++i) if (!std::isfinite (output.at(i).z)) output.at(i).z = base_max; @@ -108,8 +109,16 @@ pcl::FastBilateralFilterOMP::applyFilter (PointCloud &output) const std::size_t small_depth = static_cast (base_delta / sigma_r_) + 1 + 2 * padding_z; Array3D data (small_width, small_height, small_depth); -#ifdef _OPENMP -#pragma omp parallel for num_threads (threads_) +#if OPENMP_LEGACY_CONST_DATA_SHARING_RULE +#pragma omp parallel for \ + default(none) \ + shared(base_min, data, output) \ + num_threads(threads_) +#else +#pragma omp parallel for \ + default(none) \ + shared(base_min, data, output, small_height, small_width) \ + num_threads(threads_) #endif for (long int i = 0; i < static_cast (small_width * small_height); ++i) { @@ -149,8 +158,16 @@ pcl::FastBilateralFilterOMP::applyFilter (PointCloud &output) { Array3D* current_buffer = (n_iter % 2 == 1 ? &buffer : &data); Array3D* current_data =(n_iter % 2 == 1 ? &data : &buffer); -#ifdef _OPENMP -#pragma omp parallel for num_threads (threads_) +#if OPENMP_LEGACY_CONST_DATA_SHARING_RULE +#pragma omp parallel for \ + default(none) \ + shared(current_buffer, current_data, dim, offset) \ + num_threads(threads_) +#else +#pragma omp parallel for \ + default(none) \ + shared(current_buffer, current_data, dim, offset, small_depth, small_height, small_width) \ + num_threads(threads_) #endif for(long int i = 0; i < static_cast ((small_width - 2)*(small_height - 2)); ++i) { @@ -174,9 +191,10 @@ pcl::FastBilateralFilterOMP::applyFilter (PointCloud &output) for (std::vector >::iterator d = data.begin (); d != data.end (); ++d) *d /= ((*d)[0] != 0) ? (*d)[1] : 1; -#ifdef _OPENMP -#pragma omp parallel for num_threads (threads_) -#endif +#pragma omp parallel for \ + default(none) \ + shared(base_min, data, output) \ + num_threads(threads_) for (long int i = 0; i < static_cast (input_->size ()); ++i) { std::size_t x = static_cast (i % input_->width); @@ -190,9 +208,10 @@ pcl::FastBilateralFilterOMP::applyFilter (PointCloud &output) } else { -#ifdef _OPENMP -#pragma omp parallel for num_threads (threads_) -#endif +#pragma omp parallel for \ + default(none) \ + shared(base_min, data, output) \ + num_threads(threads_) for (long i = 0; i < static_cast (input_->size ()); ++i) { std::size_t x = static_cast (i % input_->width); diff --git a/filters/include/pcl/filters/impl/filter.hpp b/filters/include/pcl/filters/impl/filter.hpp index 03d17c1d..6c63735e 100644 --- a/filters/include/pcl/filters/impl/filter.hpp +++ b/filters/include/pcl/filters/impl/filter.hpp @@ -35,10 +35,10 @@ * */ -#ifndef PCL_FILTERS_IMPL_FILTER_H_ -#define PCL_FILTERS_IMPL_FILTER_H_ +#pragma once #include +#include // for pcl::isFinite #include ////////////////////////////////////////////////////////////////////////// @@ -142,5 +142,3 @@ pcl::removeNaNNormalsFromPointCloud (const pcl::PointCloud &cloud_in, #define PCL_INSTANTIATE_removeNaNFromPointCloud(T) template PCL_EXPORTS void pcl::removeNaNFromPointCloud(const pcl::PointCloud&, pcl::PointCloud&, std::vector&); #define PCL_INSTANTIATE_removeNaNNormalsFromPointCloud(T) template PCL_EXPORTS void pcl::removeNaNNormalsFromPointCloud(const pcl::PointCloud&, pcl::PointCloud&, std::vector&); -#endif // PCL_FILTERS_IMPL_FILTER_H_ - diff --git a/filters/include/pcl/filters/impl/filter_indices.hpp b/filters/include/pcl/filters/impl/filter_indices.hpp index 60ada058..cee70c36 100644 --- a/filters/include/pcl/filters/impl/filter_indices.hpp +++ b/filters/include/pcl/filters/impl/filter_indices.hpp @@ -74,7 +74,42 @@ pcl::removeNaNFromPointCloud (const pcl::PointCloud &cloud_in, } } +template void +pcl::FilterIndices::applyFilter (PointCloud &output) +{ + std::vector indices; + if (keep_organized_) + { + if (!extract_removed_indices_) + { + PCL_WARN ("[pcl::FilterIndices::applyFilter] extract_removed_indices_ was set to 'true' to keep the point cloud organized.\n"); + extract_removed_indices_ = true; + } + applyFilter (indices); + + output = *input_; + + // To preserve legacy behavior, only coordinates xyz are filtered. + // Copying a PointXYZ initialized with the user_filter_value_ into a generic + // PointT, ensures only the xyz coordinates, if they exist at destination, + // are overwritten. + const PointXYZ ufv (user_filter_value_, user_filter_value_, user_filter_value_); + for (const auto ri : *removed_indices_) // ri = removed index + copyPoint(ufv, output[ri]); + if (!std::isfinite (user_filter_value_)) + output.is_dense = false; + } + else + { + output.is_dense = true; + applyFilter (indices); + pcl::copyPointCloud (*input_, indices, output); + } +} + + #define PCL_INSTANTIATE_removeNanFromPointCloud(T) template PCL_EXPORTS void pcl::removeNaNFromPointCloud(const pcl::PointCloud&, std::vector&); +#define PCL_INSTANTIATE_FilterIndices(T) template class PCL_EXPORTS pcl::FilterIndices; #endif // PCL_FILTERS_IMPL_FILTER_INDICES_H_ diff --git a/filters/include/pcl/filters/impl/frustum_culling.hpp b/filters/include/pcl/filters/impl/frustum_culling.hpp index d4e4ce59..9d159799 100644 --- a/filters/include/pcl/filters/impl/frustum_culling.hpp +++ b/filters/include/pcl/filters/impl/frustum_culling.hpp @@ -42,35 +42,6 @@ #include #include -/////////////////////////////////////////////////////////////////////////////// -template void -pcl::FrustumCulling::applyFilter (PointCloud& output) -{ - std::vector indices; - if (keep_organized_) - { - bool temp = extract_removed_indices_; - extract_removed_indices_ = true; - applyFilter (indices); - extract_removed_indices_ = temp; - copyPointCloud (*input_, output); - - for (std::size_t rii = 0; rii < removed_indices_->size (); ++rii) - { - PointT &pt_to_remove = output.at ((*removed_indices_)[rii]); - pt_to_remove.x = pt_to_remove.y = pt_to_remove.z = user_filter_value_; - if (!std::isfinite (user_filter_value_)) - output.is_dense = false; - } - } - else - { - output.is_dense = true; - applyFilter (indices); - copyPointCloud (*input_, indices, output); - } -} - /////////////////////////////////////////////////////////////////////////////// template void pcl::FrustumCulling::applyFilter (std::vector &indices) diff --git a/filters/include/pcl/filters/impl/local_maximum.hpp b/filters/include/pcl/filters/impl/local_maximum.hpp index 4ba3d9e1..daec3e76 100644 --- a/filters/include/pcl/filters/impl/local_maximum.hpp +++ b/filters/include/pcl/filters/impl/local_maximum.hpp @@ -39,10 +39,10 @@ * */ -#ifndef PCL_FILTERS_IMPL_LOCAL_MAXIMUM_H_ -#define PCL_FILTERS_IMPL_LOCAL_MAXIMUM_H_ +#pragma once #include +#include // for pcl::isFinite #include #include #include @@ -187,5 +187,3 @@ pcl::LocalMaximum::applyFilterIndices (std::vector &indices) #define PCL_INSTANTIATE_LocalMaximum(T) template class PCL_EXPORTS pcl::LocalMaximum; -#endif // PCL_FILTERS_IMPL_LOCAL_MAXIMUM_H_ - diff --git a/filters/include/pcl/filters/impl/median_filter.hpp b/filters/include/pcl/filters/impl/median_filter.hpp index ff510aff..61359555 100644 --- a/filters/include/pcl/filters/impl/median_filter.hpp +++ b/filters/include/pcl/filters/impl/median_filter.hpp @@ -37,11 +37,11 @@ * */ -#ifndef PCL_FILTERS_IMPL_MEDIAN_FILTER_HPP_ -#define PCL_FILTERS_IMPL_MEDIAN_FILTER_HPP_ +#pragma once #include #include +#include // for pcl::isFinite template void pcl::MedianFilter::applyFilter (PointCloud &output) @@ -88,5 +88,3 @@ pcl::MedianFilter::applyFilter (PointCloud &output) } } - -#endif /* PCL_FILTERS_IMPL_MEDIAN_FILTER_HPP_ */ diff --git a/filters/include/pcl/filters/impl/model_outlier_removal.hpp b/filters/include/pcl/filters/impl/model_outlier_removal.hpp index a09c42bc..8ffd1724 100644 --- a/filters/include/pcl/filters/impl/model_outlier_removal.hpp +++ b/filters/include/pcl/filters/impl/model_outlier_removal.hpp @@ -35,11 +35,11 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef PCL_FILTERS_IMPL_MODEL_OUTLIER_REMOVAL_HPP_ -#define PCL_FILTERS_IMPL_MODEL_OUTLIER_REMOVAL_HPP_ +#pragma once #include #include +#include // for pcl::isFinite #include #include #include @@ -141,31 +141,6 @@ pcl::ModelOutlierRemoval::initSACModel (pcl::SacModel model_type) return (true); } -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::ModelOutlierRemoval::applyFilter (PointCloud &output) -{ - std::vector indices; - if (keep_organized_) - { - bool temp = extract_removed_indices_; - extract_removed_indices_ = true; - applyFilterIndices (indices); - extract_removed_indices_ = temp; - - output = *input_; - for (int rii = 0; rii < static_cast (removed_indices_->size ()); ++rii) // rii = removed indices iterator - output.points[ (*removed_indices_)[rii]].x = output.points[ (*removed_indices_)[rii]].y = output.points[ (*removed_indices_)[rii]].z = user_filter_value_; - if (!std::isfinite (user_filter_value_)) - output.is_dense = false; - } - else - { - applyFilterIndices (indices); - copyPointCloud (*input_, indices, output); - } -} - //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template void pcl::ModelOutlierRemoval::applyFilterIndices (std::vector &indices) @@ -264,4 +239,3 @@ pcl::ModelOutlierRemoval::applyFilterIndices (std::vector &indices) #define PCL_INSTANTIATE_ModelOutlierRemoval(T) template class PCL_EXPORTS pcl::ModelOutlierRemoval; -#endif // PCL_FILTERS_IMPL_MODEL_OUTLIER_REMOVAL_HPP_ diff --git a/filters/include/pcl/filters/impl/morphological_filter.hpp b/filters/include/pcl/filters/impl/morphological_filter.hpp index 2ed31e3e..25c8407f 100644 --- a/filters/include/pcl/filters/impl/morphological_filter.hpp +++ b/filters/include/pcl/filters/impl/morphological_filter.hpp @@ -52,11 +52,12 @@ #include #include -/////////////////////////////////////////////////////////////////////////////////////////// +namespace pcl +{ template void -pcl::applyMorphologicalOperator (const typename pcl::PointCloud::ConstPtr &cloud_in, - float resolution, const int morphological_operator, - pcl::PointCloud &cloud_out) +applyMorphologicalOperator (const typename pcl::PointCloud::ConstPtr &cloud_in, + float resolution, const int morphological_operator, + pcl::PointCloud &cloud_out) { if (cloud_in->empty ()) return; @@ -202,7 +203,8 @@ pcl::applyMorphologicalOperator (const typename pcl::PointCloud::ConstPt return; } +} // namespace pcl + #define PCL_INSTANTIATE_applyMorphologicalOperator(T) template PCL_EXPORTS void pcl::applyMorphologicalOperator (const pcl::PointCloud::ConstPtr &, float, const int, pcl::PointCloud &); #endif //#ifndef PCL_FILTERS_IMPL_MORPHOLOGICAL_FILTER_H_ - diff --git a/filters/include/pcl/filters/impl/normal_space.hpp b/filters/include/pcl/filters/impl/normal_space.hpp index f4b8f485..fab50f10 100644 --- a/filters/include/pcl/filters/impl/normal_space.hpp +++ b/filters/include/pcl/filters/impl/normal_space.hpp @@ -59,40 +59,10 @@ pcl::NormalSpaceSampling::initCompute () return false; } - boost::mt19937 rng (static_cast (seed_)); - boost::uniform_int uniform_distrib (0, unsigned (input_->size ())); - delete rng_uniform_distribution_; - rng_uniform_distribution_ = new boost::variate_generator > (rng, uniform_distrib); - + rng_.seed (seed_); return (true); } -/////////////////////////////////////////////////////////////////////////////// -template void -pcl::NormalSpaceSampling::applyFilter (PointCloud &output) -{ - std::vector indices; - if (keep_organized_) - { - bool temp = extract_removed_indices_; - extract_removed_indices_ = true; - applyFilter (indices); - extract_removed_indices_ = temp; - - output = *input_; - for (int rii = 0; rii < static_cast (removed_indices_->size ()); ++rii) // rii = removed indices iterator - output.points[(*removed_indices_)[rii]].x = output.points[(*removed_indices_)[rii]].y = output.points[(*removed_indices_)[rii]].z = user_filter_value_; - if (!std::isfinite (user_filter_value_)) - output.is_dense = false; - } - else - { - output.is_dense = true; - applyFilter (indices); - pcl::copyPointCloud (*input_, indices, output); - } -} - /////////////////////////////////////////////////////////////////////////////// template bool pcl::NormalSpaceSampling::isEntireBinSampled (boost::dynamic_bitset<> &array, @@ -109,66 +79,12 @@ pcl::NormalSpaceSampling::isEntireBinSampled (boost::dynamic_bi /////////////////////////////////////////////////////////////////////////////// template unsigned int -pcl::NormalSpaceSampling::findBin (const float *normal, unsigned int) +pcl::NormalSpaceSampling::findBin (const float *normal) { - unsigned int bin_number = 0; - // Holds the bin numbers for direction cosines in x,y,z directions - unsigned int t[3] = {0,0,0}; - - // dcos is the direction cosine. - float dcos = 0.0; - float bin_size = 0.0; - // max_cos and min_cos are the maximum and minimum values of std::cos(theta) respectively - float max_cos = 1.0; - float min_cos = -1.0; - -// dcos = std::cos (normal[0]); - dcos = normal[0]; - bin_size = (max_cos - min_cos) / static_cast (binsx_); - - // Finding bin number for direction cosine in x direction - unsigned int k = 0; - for (float i = min_cos; (i + bin_size) < (max_cos - bin_size); i += bin_size , k++) - { - if (dcos >= i && dcos <= (i+bin_size)) - { - break; - } - } - t[0] = k; - -// dcos = std::cos (normal[1]); - dcos = normal[1]; - bin_size = (max_cos - min_cos) / static_cast (binsy_); - - // Finding bin number for direction cosine in y direction - k = 0; - for (float i = min_cos; (i + bin_size) < (max_cos - bin_size); i += bin_size , k++) - { - if (dcos >= i && dcos <= (i+bin_size)) - { - break; - } - } - t[1] = k; - -// dcos = std::cos (normal[2]); - dcos = normal[2]; - bin_size = (max_cos - min_cos) / static_cast (binsz_); - - // Finding bin number for direction cosine in z direction - k = 0; - for (float i = min_cos; (i + bin_size) < (max_cos - bin_size); i += bin_size , k++) - { - if (dcos >= i && dcos <= (i+bin_size)) - { - break; - } - } - t[2] = k; - - bin_number = t[0] * (binsy_*binsz_) + t[1] * binsz_ + t[2]; - return bin_number; + const unsigned ix = static_cast (std::round (0.5f * (binsx_ - 1.f) * (normal[0] + 1.f))); + const unsigned iy = static_cast (std::round (0.5f * (binsy_ - 1.f) * (normal[1] + 1.f))); + const unsigned iz = static_cast (std::round (0.5f * (binsz_ - 1.f) * (normal[2] + 1.f))); + return ix * (binsy_*binsz_) + iy * binsz_ + iz; } /////////////////////////////////////////////////////////////////////////////// @@ -195,10 +111,10 @@ pcl::NormalSpaceSampling::applyFilter (std::vector &indice for (unsigned int i = 0; i < n_bins; i++) normals_hg.emplace_back(); - for (std::vector::const_iterator it = indices_->begin (); it != indices_->end (); ++it) + for (const auto index : *indices_) { - unsigned int bin_number = findBin (input_normals_->points[*it].normal, n_bins); - normals_hg[bin_number].push_back (*it); + unsigned int bin_number = findBin ((*input_normals_)[index].normal); + normals_hg[bin_number].push_back (index); } @@ -239,11 +155,12 @@ pcl::NormalSpaceSampling::applyFilter (std::vector &indice unsigned int pos = 0; unsigned int random_index = 0; + std::uniform_int_distribution rng_uniform_distribution (0u, M - 1u); // Picking up a sample at random from jth bin do { - random_index = static_cast ((*rng_uniform_distribution_) () % M); + random_index = rng_uniform_distribution (rng_); pos = start_index[j] + random_index; } while (is_sampled_flag.test (pos)); diff --git a/filters/include/pcl/filters/impl/passthrough.hpp b/filters/include/pcl/filters/impl/passthrough.hpp index 269a9d73..c67bf743 100644 --- a/filters/include/pcl/filters/impl/passthrough.hpp +++ b/filters/include/pcl/filters/impl/passthrough.hpp @@ -43,32 +43,6 @@ #include #include -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::PassThrough::applyFilter (PointCloud &output) -{ - std::vector indices; - if (keep_organized_) - { - bool temp = extract_removed_indices_; - extract_removed_indices_ = true; - applyFilterIndices (indices); - extract_removed_indices_ = temp; - - output = *input_; - for (const auto ri : *removed_indices_) // ri = removed index - output.points[ri].x = output.points[ri].y = output.points[ri].z = user_filter_value_; - if (!std::isfinite (user_filter_value_)) - output.is_dense = false; - } - else - { - output.is_dense = true; - applyFilterIndices (indices); - copyPointCloud (*input_, indices, output); - } -} - //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template void pcl::PassThrough::applyFilterIndices (std::vector &indices) diff --git a/filters/include/pcl/filters/impl/plane_clipper3D.hpp b/filters/include/pcl/filters/impl/plane_clipper3D.hpp index 0adb6814..1ccf9607 100644 --- a/filters/include/pcl/filters/impl/plane_clipper3D.hpp +++ b/filters/include/pcl/filters/impl/plane_clipper3D.hpp @@ -182,41 +182,40 @@ pcl::PlaneClipper3D::clipPointCloud3D (const pcl::PointCloud& cl if (indices.empty ()) { clipped.reserve (cloud_in.size ()); - /* -#if 0 - Eigen::MatrixXf points = cloud_in.getMatrixXfMap (4, sizeof (PointT) / sizeof (float), offsetof(PointT,x) / sizeof (float)); - Eigen::VectorXf distances = plane_params_.transpose () * points; - for (unsigned rIdx = 0; rIdx < cloud_in.size (); ++ rIdx) - { - if (distances (rIdx, 0) >= -plane_params_[3]) - clipped.push_back (rIdx); - } -#else - Eigen::Matrix4Xf points (4, cloud_in.size ()); - for (unsigned rIdx = 0; rIdx < cloud_in.size (); ++ rIdx) - { - points (0, rIdx) = cloud_in[rIdx].x; - points (1, rIdx) = cloud_in[rIdx].y; - points (2, rIdx) = cloud_in[rIdx].z; - points (3, rIdx) = 1; - } - Eigen::VectorXf distances = plane_params_.transpose () * points; - for (unsigned rIdx = 0; rIdx < cloud_in.size (); ++ rIdx) - { - if (distances (rIdx, 0) >= 0) - clipped.push_back (rIdx); - } - -#endif - //std::cout << "points : " << points.rows () << " x " << points.cols () << " * " << plane_params_.transpose ().rows () << " x " << plane_params_.transpose ().cols () << std::endl; +// #if 0 +// Eigen::MatrixXf points = cloud_in.getMatrixXfMap (4, sizeof (PointT) / sizeof (float), offsetof(PointT,x) / sizeof (float)); +// Eigen::VectorXf distances = plane_params_.transpose () * points; +// for (unsigned rIdx = 0; rIdx < cloud_in.size (); ++ rIdx) +// { +// if (distances (rIdx, 0) >= -plane_params_[3]) +// clipped.push_back (rIdx); +// } +// #else +// Eigen::Matrix4Xf points (4, cloud_in.size ()); +// for (unsigned rIdx = 0; rIdx < cloud_in.size (); ++ rIdx) +// { +// points (0, rIdx) = cloud_in[rIdx].x; +// points (1, rIdx) = cloud_in[rIdx].y; +// points (2, rIdx) = cloud_in[rIdx].z; +// points (3, rIdx) = 1; +// } +// Eigen::VectorXf distances = plane_params_.transpose () * points; +// for (unsigned rIdx = 0; rIdx < cloud_in.size (); ++ rIdx) +// { +// if (distances (rIdx, 0) >= 0) +// clipped.push_back (rIdx); +// } +// +// #endif +// +// //std::cout << "points : " << points.rows () << " x " << points.cols () << " * " << plane_params_.transpose ().rows () << " x " << plane_params_.transpose ().cols () << std::endl; +// +// //std::cout << "distances: " << distances.rows () << " x " << distances.cols () << std::endl; - //std::cout << "distances: " << distances.rows () << " x " << distances.cols () << std::endl; - /*/ for (unsigned pIdx = 0; pIdx < cloud_in.size (); ++pIdx) if (clipPoint3D (cloud_in[pIdx])) clipped.push_back (pIdx); - //*/ } else { diff --git a/filters/include/pcl/filters/impl/pyramid.hpp b/filters/include/pcl/filters/impl/pyramid.hpp index 41ddf1ef..2ad5b449 100644 --- a/filters/include/pcl/filters/impl/pyramid.hpp +++ b/filters/include/pcl/filters/impl/pyramid.hpp @@ -37,18 +37,26 @@ * */ + #ifndef PCL_FILTERS_IMPL_PYRAMID_HPP #define PCL_FILTERS_IMPL_PYRAMID_HPP + +namespace pcl +{ + +namespace filters +{ + template bool -pcl::filters::Pyramid::initCompute () +Pyramid::initCompute () { if (!input_->isOrganized ()) { PCL_ERROR ("[pcl::fileters::%s::initCompute] Number of levels should be at least 2!", getClassName ().c_str ()); return (false); } - + if (levels_ < 2) { PCL_ERROR ("[pcl::fileters::%s::initCompute] Number of levels should be at least 2!", getClassName ().c_str ()); @@ -58,13 +66,13 @@ pcl::filters::Pyramid::initCompute () // std::size_t ratio (std::pow (2, levels_)); // std::size_t last_width = input_->width / ratio; // std::size_t last_height = input_->height / ratio; - + if (levels_ > 4) { PCL_ERROR ("[pcl::fileters::%s::initCompute] Number of levels should not exceed 4!", getClassName ().c_str ()); return (false); } - + if (large_) { Eigen::VectorXf k (5); @@ -82,12 +90,12 @@ pcl::filters::Pyramid::initCompute () if (threshold_ != std::numeric_limits::infinity ()) threshold_ *= threshold_; } - + return (true); } template void -pcl::filters::Pyramid::compute (std::vector& output) +Pyramid::compute (std::vector& output) { std::cout << "compute" << std::endl; if (!initCompute ()) @@ -112,23 +120,24 @@ pcl::filters::Pyramid::compute (std::vector& output) output[l].reset (new pcl::PointCloud (output[l-1]->width/2, output[l-1]->height/2)); const PointCloud &previous = *output[l-1]; PointCloud &next = *output[l]; -#ifdef _OPENMP -#pragma omp parallel for shared (next) num_threads(threads_) -#endif +#pragma omp parallel for \ + default(none) \ + shared(next) \ + num_threads(threads_) for(int i=0; i < next.height; ++i) { for(int j=0; j < next.width; ++j) { for(int m=0; m < kernel_rows; ++m) { - int mm = kernel_rows - 1 - m; - for(int n=0; n < kernel_cols; ++n) + int mm = kernel_rows - 1 - m; + for(int n=0; n < kernel_cols; ++n) { int nn = kernel_cols - 1 - n; int ii = 2*i + (m - kernel_center_y); int jj = 2*j + (n - kernel_center_x); - + if (ii < 0) ii = 0; if (ii >= previous.height) ii = previous.height - 1; if (jj < 0) jj = 0; @@ -147,9 +156,10 @@ pcl::filters::Pyramid::compute (std::vector& output) output[l].reset (new pcl::PointCloud (output[l-1]->width/2, output[l-1]->height/2)); const PointCloud &previous = *output[l-1]; PointCloud &next = *output[l]; -#ifdef _OPENMP -#pragma omp parallel for shared (next) num_threads(threads_) -#endif +#pragma omp parallel for \ + default(none) \ + shared(next) \ + num_threads(threads_) for(int i=0; i < next.height; ++i) { for(int j=0; j < next.width; ++j) @@ -186,383 +196,387 @@ pcl::filters::Pyramid::compute (std::vector& output) } } } - } + } } -namespace pcl + +template <> void +Pyramid::compute (std::vector::PointCloudPtr> &output) { - namespace filters + std::cout << "PointXYZRGB" << std::endl; + if (!initCompute ()) { - template <> void - Pyramid::compute (std::vector::PointCloudPtr> &output) - { - std::cout << "PointXYZRGB" << std::endl; - if (!initCompute ()) - { - PCL_ERROR ("[pcl::%s::compute] initCompute failed!\n", getClassName ().c_str ()); - return; - } - - int kernel_rows = static_cast (kernel_.rows ()); - int kernel_cols = static_cast (kernel_.cols ()); - int kernel_center_x = kernel_cols / 2; - int kernel_center_y = kernel_rows / 2; + PCL_ERROR ("[pcl::%s::compute] initCompute failed!\n", getClassName ().c_str ()); + return; + } - output.resize (levels_ + 1); - output[0].reset (new pcl::PointCloud); - *(output[0]) = *input_; + int kernel_rows = static_cast (kernel_.rows ()); + int kernel_cols = static_cast (kernel_.cols ()); + int kernel_center_x = kernel_cols / 2; + int kernel_center_y = kernel_rows / 2; + + output.resize (levels_ + 1); + output[0].reset (new pcl::PointCloud); + *(output[0]) = *input_; - if (input_->is_dense) + if (input_->is_dense) + { + for (int l = 1; l <= levels_; ++l) + { + output[l].reset (new pcl::PointCloud (output[l-1]->width/2, output[l-1]->height/2)); + const PointCloud &previous = *output[l-1]; + PointCloud &next = *output[l]; +#pragma omp parallel for \ + default(none) \ + shared(next) \ + num_threads(threads_) + for(int i=0; i < next.height; ++i) // rows { - for (int l = 1; l <= levels_; ++l) + for(int j=0; j < next.width; ++j) // columns { - output[l].reset (new pcl::PointCloud (output[l-1]->width/2, output[l-1]->height/2)); - const PointCloud &previous = *output[l-1]; - PointCloud &next = *output[l]; -#ifdef _OPENMP -#pragma omp parallel for shared (next) num_threads(threads_) -#endif - for(int i=0; i < next.height; ++i) // rows + float r = 0, g = 0, b = 0; + for(int m=0; m < kernel_rows; ++m) // kernel rows { - for(int j=0; j < next.width; ++j) // columns + int mm = kernel_rows - 1 - m; // row index of flipped kernel + for(int n=0; n < kernel_cols; ++n) // kernel columns { - float r = 0, g = 0, b = 0; - for(int m=0; m < kernel_rows; ++m) // kernel rows - { - int mm = kernel_rows - 1 - m; // row index of flipped kernel - for(int n=0; n < kernel_cols; ++n) // kernel columns - { - int nn = kernel_cols - 1 - n; // column index of flipped kernel - // index of input signal, used for checking boundary - int ii = 2*i + (m - kernel_center_y); - int jj = 2*j + (n - kernel_center_x); - - // ignore input samples which are out of bound - if (ii < 0) ii = 0; - if (ii >= previous.height) ii = previous.height - 1; - if (jj < 0) jj = 0; - if (jj >= previous.width) jj = previous.width - 1; - next.at (j,i).x += previous.at (jj,ii).x * kernel_ (mm,nn); - next.at (j,i).y += previous.at (jj,ii).y * kernel_ (mm,nn); - next.at (j,i).z += previous.at (jj,ii).z * kernel_ (mm,nn); - b += previous.at (jj,ii).b * kernel_ (mm,nn); - g += previous.at (jj,ii).g * kernel_ (mm,nn); - r += previous.at (jj,ii).r * kernel_ (mm,nn); - } - } - next.at (j,i).b = static_cast (b); - next.at (j,i).g = static_cast (g); - next.at (j,i).r = static_cast (r); + int nn = kernel_cols - 1 - n; // column index of flipped kernel + // index of input signal, used for checking boundary + int ii = 2*i + (m - kernel_center_y); + int jj = 2*j + (n - kernel_center_x); + + // ignore input samples which are out of bound + if (ii < 0) ii = 0; + if (ii >= previous.height) ii = previous.height - 1; + if (jj < 0) jj = 0; + if (jj >= previous.width) jj = previous.width - 1; + next.at (j,i).x += previous.at (jj,ii).x * kernel_ (mm,nn); + next.at (j,i).y += previous.at (jj,ii).y * kernel_ (mm,nn); + next.at (j,i).z += previous.at (jj,ii).z * kernel_ (mm,nn); + b += previous.at (jj,ii).b * kernel_ (mm,nn); + g += previous.at (jj,ii).g * kernel_ (mm,nn); + r += previous.at (jj,ii).r * kernel_ (mm,nn); } } + next.at (j,i).b = static_cast (b); + next.at (j,i).g = static_cast (g); + next.at (j,i).r = static_cast (r); } } - else + } + } + else + { + for (int l = 1; l <= levels_; ++l) + { + output[l].reset (new pcl::PointCloud (output[l-1]->width/2, output[l-1]->height/2)); + const PointCloud &previous = *output[l-1]; + PointCloud &next = *output[l]; +#pragma omp parallel for \ + default(none) \ + shared(next) \ + num_threads(threads_) + for(int i=0; i < next.height; ++i) { - for (int l = 1; l <= levels_; ++l) + for(int j=0; j < next.width; ++j) { - output[l].reset (new pcl::PointCloud (output[l-1]->width/2, output[l-1]->height/2)); - const PointCloud &previous = *output[l-1]; - PointCloud &next = *output[l]; -#ifdef _OPENMP -#pragma omp parallel for shared (next) num_threads(threads_) -#endif - for(int i=0; i < next.height; ++i) + float weight = 0; + float r = 0, g = 0, b = 0; + for(int m=0; m < kernel_rows; ++m) { - for(int j=0; j < next.width; ++j) + int mm = kernel_rows - 1 - m; + for(int n=0; n < kernel_cols; ++n) { - float weight = 0; - float r = 0, g = 0, b = 0; - for(int m=0; m < kernel_rows; ++m) - { - int mm = kernel_rows - 1 - m; - for(int n=0; n < kernel_cols; ++n) - { - int nn = kernel_cols - 1 - n; - int ii = 2*i + (m - kernel_center_y); - int jj = 2*j + (n - kernel_center_x); - if (ii < 0) ii = 0; - if (ii >= previous.height) ii = previous.height - 1; - if (jj < 0) jj = 0; - if (jj >= previous.width) jj = previous.width - 1; - if (!isFinite (previous.at (jj,ii))) - continue; - if (pcl::squaredEuclideanDistance (previous.at (2*j,2*i), previous.at (jj,ii)) < threshold_) - { - next.at (j,i).x += previous.at (jj,ii).x * kernel_ (mm,nn); - next.at (j,i).y += previous.at (jj,ii).y * kernel_ (mm,nn); - next.at (j,i).z += previous.at (jj,ii).z * kernel_ (mm,nn); - b += previous.at (jj,ii).b * kernel_ (mm,nn); - g += previous.at (jj,ii).g * kernel_ (mm,nn); - r += previous.at (jj,ii).r * kernel_ (mm,nn); - weight+= kernel_ (mm,nn); - } - } - } - if (weight == 0) - nullify (next.at (j,i)); - else + int nn = kernel_cols - 1 - n; + int ii = 2*i + (m - kernel_center_y); + int jj = 2*j + (n - kernel_center_x); + if (ii < 0) ii = 0; + if (ii >= previous.height) ii = previous.height - 1; + if (jj < 0) jj = 0; + if (jj >= previous.width) jj = previous.width - 1; + if (!isFinite (previous.at (jj,ii))) + continue; + if (pcl::squaredEuclideanDistance (previous.at (2*j,2*i), previous.at (jj,ii)) < threshold_) { - weight = 1.f/weight; - r*= weight; g*= weight; b*= weight; - next.at (j,i).x*= weight; next.at (j,i).y*= weight; next.at (j,i).z*= weight; - next.at (j,i).b = static_cast (b); - next.at (j,i).g = static_cast (g); - next.at (j,i).r = static_cast (r); + next.at (j,i).x += previous.at (jj,ii).x * kernel_ (mm,nn); + next.at (j,i).y += previous.at (jj,ii).y * kernel_ (mm,nn); + next.at (j,i).z += previous.at (jj,ii).z * kernel_ (mm,nn); + b += previous.at (jj,ii).b * kernel_ (mm,nn); + g += previous.at (jj,ii).g * kernel_ (mm,nn); + r += previous.at (jj,ii).r * kernel_ (mm,nn); + weight+= kernel_ (mm,nn); } } } + if (weight == 0) + nullify (next.at (j,i)); + else + { + weight = 1.f/weight; + r*= weight; g*= weight; b*= weight; + next.at (j,i).x*= weight; next.at (j,i).y*= weight; next.at (j,i).z*= weight; + next.at (j,i).b = static_cast (b); + next.at (j,i).g = static_cast (g); + next.at (j,i).r = static_cast (r); + } } - } - } - - template <> void - Pyramid::compute (std::vector::PointCloudPtr> &output) - { - std::cout << "PointXYZRGBA" << std::endl; - if (!initCompute ()) - { - PCL_ERROR ("[pcl::%s::compute] initCompute failed!\n", getClassName ().c_str ()); - return; } - - int kernel_rows = static_cast (kernel_.rows ()); - int kernel_cols = static_cast (kernel_.cols ()); - int kernel_center_x = kernel_cols / 2; - int kernel_center_y = kernel_rows / 2; + } + } +} + +template <> void +Pyramid::compute (std::vector::PointCloudPtr> &output) +{ + std::cout << "PointXYZRGBA" << std::endl; + if (!initCompute ()) + { + PCL_ERROR ("[pcl::%s::compute] initCompute failed!\n", getClassName ().c_str ()); + return; + } + + int kernel_rows = static_cast (kernel_.rows ()); + int kernel_cols = static_cast (kernel_.cols ()); + int kernel_center_x = kernel_cols / 2; + int kernel_center_y = kernel_rows / 2; - output.resize (levels_ + 1); - output[0].reset (new pcl::PointCloud); - *(output[0]) = *input_; + output.resize (levels_ + 1); + output[0].reset (new pcl::PointCloud); + *(output[0]) = *input_; - if (input_->is_dense) + if (input_->is_dense) + { + for (int l = 1; l <= levels_; ++l) + { + output[l].reset (new pcl::PointCloud (output[l-1]->width/2, output[l-1]->height/2)); + const PointCloud &previous = *output[l-1]; + PointCloud &next = *output[l]; +#pragma omp parallel for \ + default(none) \ + shared(next) \ + num_threads(threads_) + for(int i=0; i < next.height; ++i) // rows { - for (int l = 1; l <= levels_; ++l) + for(int j=0; j < next.width; ++j) // columns { - output[l].reset (new pcl::PointCloud (output[l-1]->width/2, output[l-1]->height/2)); - const PointCloud &previous = *output[l-1]; - PointCloud &next = *output[l]; -#ifdef _OPENMP -#pragma omp parallel for shared (next) num_threads(threads_) -#endif - for(int i=0; i < next.height; ++i) // rows + float r = 0, g = 0, b = 0, a = 0; + for(int m=0; m < kernel_rows; ++m) // kernel rows { - for(int j=0; j < next.width; ++j) // columns + int mm = kernel_rows - 1 - m; // row index of flipped kernel + for(int n=0; n < kernel_cols; ++n) // kernel columns { - float r = 0, g = 0, b = 0, a = 0; - for(int m=0; m < kernel_rows; ++m) // kernel rows - { - int mm = kernel_rows - 1 - m; // row index of flipped kernel - for(int n=0; n < kernel_cols; ++n) // kernel columns - { - int nn = kernel_cols - 1 - n; // column index of flipped kernel - // index of input signal, used for checking boundary - int ii = 2*i + (m - kernel_center_y); - int jj = 2*j + (n - kernel_center_x); - - // ignore input samples which are out of bound - if (ii < 0) ii = 0; - if (ii >= previous.height) ii = previous.height - 1; - if (jj < 0) jj = 0; - if (jj >= previous.width) jj = previous.width - 1; - next.at (j,i).x += previous.at (jj,ii).x * kernel_ (mm,nn); - next.at (j,i).y += previous.at (jj,ii).y * kernel_ (mm,nn); - next.at (j,i).z += previous.at (jj,ii).z * kernel_ (mm,nn); - b += previous.at (jj,ii).b * kernel_ (mm,nn); - g += previous.at (jj,ii).g * kernel_ (mm,nn); - r += previous.at (jj,ii).r * kernel_ (mm,nn); - a += previous.at (jj,ii).a * kernel_ (mm,nn); - } - } - next.at (j,i).b = static_cast (b); - next.at (j,i).g = static_cast (g); - next.at (j,i).r = static_cast (r); - next.at (j,i).a = static_cast (a); + int nn = kernel_cols - 1 - n; // column index of flipped kernel + // index of input signal, used for checking boundary + int ii = 2*i + (m - kernel_center_y); + int jj = 2*j + (n - kernel_center_x); + + // ignore input samples which are out of bound + if (ii < 0) ii = 0; + if (ii >= previous.height) ii = previous.height - 1; + if (jj < 0) jj = 0; + if (jj >= previous.width) jj = previous.width - 1; + next.at (j,i).x += previous.at (jj,ii).x * kernel_ (mm,nn); + next.at (j,i).y += previous.at (jj,ii).y * kernel_ (mm,nn); + next.at (j,i).z += previous.at (jj,ii).z * kernel_ (mm,nn); + b += previous.at (jj,ii).b * kernel_ (mm,nn); + g += previous.at (jj,ii).g * kernel_ (mm,nn); + r += previous.at (jj,ii).r * kernel_ (mm,nn); + a += previous.at (jj,ii).a * kernel_ (mm,nn); } } + next.at (j,i).b = static_cast (b); + next.at (j,i).g = static_cast (g); + next.at (j,i).r = static_cast (r); + next.at (j,i).a = static_cast (a); } } - else + } + } + else + { + for (int l = 1; l <= levels_; ++l) + { + output[l].reset (new pcl::PointCloud (output[l-1]->width/2, output[l-1]->height/2)); + const PointCloud &previous = *output[l-1]; + PointCloud &next = *output[l]; +#pragma omp parallel for \ + default(none) \ + shared(next) \ + num_threads(threads_) + for(int i=0; i < next.height; ++i) { - for (int l = 1; l <= levels_; ++l) + for(int j=0; j < next.width; ++j) { - output[l].reset (new pcl::PointCloud (output[l-1]->width/2, output[l-1]->height/2)); - const PointCloud &previous = *output[l-1]; - PointCloud &next = *output[l]; -#ifdef _OPENMP -#pragma omp parallel for shared (next) num_threads(threads_) -#endif - for(int i=0; i < next.height; ++i) + float weight = 0; + float r = 0, g = 0, b = 0, a = 0; + for(int m=0; m < kernel_rows; ++m) { - for(int j=0; j < next.width; ++j) + int mm = kernel_rows - 1 - m; + for(int n=0; n < kernel_cols; ++n) { - float weight = 0; - float r = 0, g = 0, b = 0, a = 0; - for(int m=0; m < kernel_rows; ++m) - { - int mm = kernel_rows - 1 - m; - for(int n=0; n < kernel_cols; ++n) - { - int nn = kernel_cols - 1 - n; - int ii = 2*i + (m - kernel_center_y); - int jj = 2*j + (n - kernel_center_x); - if (ii < 0) ii = 0; - if (ii >= previous.height) ii = previous.height - 1; - if (jj < 0) jj = 0; - if (jj >= previous.width) jj = previous.width - 1; - if (!isFinite (previous.at (jj,ii))) - continue; - if (pcl::squaredEuclideanDistance (previous.at (2*j,2*i), previous.at (jj,ii)) < threshold_) - { - next.at (j,i).x += previous.at (jj,ii).x * kernel_ (mm,nn); - next.at (j,i).y += previous.at (jj,ii).y * kernel_ (mm,nn); - next.at (j,i).z += previous.at (jj,ii).z * kernel_ (mm,nn); - b += previous.at (jj,ii).b * kernel_ (mm,nn); - g += previous.at (jj,ii).g * kernel_ (mm,nn); - r += previous.at (jj,ii).r * kernel_ (mm,nn); - a += previous.at (jj,ii).a * kernel_ (mm,nn); - weight+= kernel_ (mm,nn); - } - } - } - if (weight == 0) - nullify (next.at (j,i)); - else + int nn = kernel_cols - 1 - n; + int ii = 2*i + (m - kernel_center_y); + int jj = 2*j + (n - kernel_center_x); + if (ii < 0) ii = 0; + if (ii >= previous.height) ii = previous.height - 1; + if (jj < 0) jj = 0; + if (jj >= previous.width) jj = previous.width - 1; + if (!isFinite (previous.at (jj,ii))) + continue; + if (pcl::squaredEuclideanDistance (previous.at (2*j,2*i), previous.at (jj,ii)) < threshold_) { - weight = 1.f/weight; - r*= weight; g*= weight; b*= weight; a*= weight; - next.at (j,i).x*= weight; next.at (j,i).y*= weight; next.at (j,i).z*= weight; - next.at (j,i).b = static_cast (b); - next.at (j,i).g = static_cast (g); - next.at (j,i).r = static_cast (r); - next.at (j,i).a = static_cast (a); + next.at (j,i).x += previous.at (jj,ii).x * kernel_ (mm,nn); + next.at (j,i).y += previous.at (jj,ii).y * kernel_ (mm,nn); + next.at (j,i).z += previous.at (jj,ii).z * kernel_ (mm,nn); + b += previous.at (jj,ii).b * kernel_ (mm,nn); + g += previous.at (jj,ii).g * kernel_ (mm,nn); + r += previous.at (jj,ii).r * kernel_ (mm,nn); + a += previous.at (jj,ii).a * kernel_ (mm,nn); + weight+= kernel_ (mm,nn); } } } + if (weight == 0) + nullify (next.at (j,i)); + else + { + weight = 1.f/weight; + r*= weight; g*= weight; b*= weight; a*= weight; + next.at (j,i).x*= weight; next.at (j,i).y*= weight; next.at (j,i).z*= weight; + next.at (j,i).b = static_cast (b); + next.at (j,i).g = static_cast (g); + next.at (j,i).r = static_cast (r); + next.at (j,i).a = static_cast (a); + } } - } + } } + } +} - template<> void - Pyramid::nullify (pcl::RGB& p) - { - p.r = 0; p.g = 0; p.b = 0; - } +template<> void +Pyramid::nullify (pcl::RGB& p) +{ + p.r = 0; p.g = 0; p.b = 0; +} - template <> void - Pyramid::compute (std::vector::PointCloudPtr> &output) - { - std::cout << "RGB" << std::endl; - if (!initCompute ()) - { - PCL_ERROR ("[pcl::%s::compute] initCompute failed!\n", getClassName ().c_str ()); - return; - } - - int kernel_rows = static_cast (kernel_.rows ()); - int kernel_cols = static_cast (kernel_.cols ()); - int kernel_center_x = kernel_cols / 2; - int kernel_center_y = kernel_rows / 2; +template <> void +Pyramid::compute (std::vector::PointCloudPtr> &output) +{ + std::cout << "RGB" << std::endl; + if (!initCompute ()) + { + PCL_ERROR ("[pcl::%s::compute] initCompute failed!\n", getClassName ().c_str ()); + return; + } - output.resize (levels_ + 1); - output[0].reset (new pcl::PointCloud); - *(output[0]) = *input_; + int kernel_rows = static_cast (kernel_.rows ()); + int kernel_cols = static_cast (kernel_.cols ()); + int kernel_center_x = kernel_cols / 2; + int kernel_center_y = kernel_rows / 2; - if (input_->is_dense) + output.resize (levels_ + 1); + output[0].reset (new pcl::PointCloud); + *(output[0]) = *input_; + + if (input_->is_dense) + { + for (int l = 1; l <= levels_; ++l) + { + output[l].reset (new pcl::PointCloud (output[l-1]->width/2, output[l-1]->height/2)); + const PointCloud &previous = *output[l-1]; + PointCloud &next = *output[l]; +#pragma omp parallel for \ + default(none) \ + shared(next) \ + num_threads(threads_) + for(int i=0; i < next.height; ++i) { - for (int l = 1; l <= levels_; ++l) + for(int j=0; j < next.width; ++j) { - output[l].reset (new pcl::PointCloud (output[l-1]->width/2, output[l-1]->height/2)); - const PointCloud &previous = *output[l-1]; - PointCloud &next = *output[l]; -#ifdef _OPENMP -#pragma omp parallel for shared (next) num_threads(threads_) -#endif - for(int i=0; i < next.height; ++i) + float r = 0, g = 0, b = 0; + for(int m=0; m < kernel_rows; ++m) { - for(int j=0; j < next.width; ++j) + int mm = kernel_rows - 1 - m; + for(int n=0; n < kernel_cols; ++n) { - float r = 0, g = 0, b = 0; - for(int m=0; m < kernel_rows; ++m) - { - int mm = kernel_rows - 1 - m; - for(int n=0; n < kernel_cols; ++n) - { - int nn = kernel_cols - 1 - n; - int ii = 2*i + (m - kernel_center_y); - int jj = 2*j + (n - kernel_center_x); - if (ii < 0) ii = 0; - if (ii >= previous.height) ii = previous.height - 1; - if (jj < 0) jj = 0; - if (jj >= previous.width) jj = previous.width - 1; - b += previous.at (jj,ii).b * kernel_ (mm,nn); - g += previous.at (jj,ii).g * kernel_ (mm,nn); - r += previous.at (jj,ii).r * kernel_ (mm,nn); - } - } - next.at (j,i).b = static_cast (b); - next.at (j,i).g = static_cast (g); - next.at (j,i).r = static_cast (r); + int nn = kernel_cols - 1 - n; + int ii = 2*i + (m - kernel_center_y); + int jj = 2*j + (n - kernel_center_x); + if (ii < 0) ii = 0; + if (ii >= previous.height) ii = previous.height - 1; + if (jj < 0) jj = 0; + if (jj >= previous.width) jj = previous.width - 1; + b += previous.at (jj,ii).b * kernel_ (mm,nn); + g += previous.at (jj,ii).g * kernel_ (mm,nn); + r += previous.at (jj,ii).r * kernel_ (mm,nn); } } + next.at (j,i).b = static_cast (b); + next.at (j,i).g = static_cast (g); + next.at (j,i).r = static_cast (r); } } - else + } + } + else + { + for (int l = 1; l <= levels_; ++l) + { + output[l].reset (new pcl::PointCloud (output[l-1]->width/2, output[l-1]->height/2)); + const PointCloud &previous = *output[l-1]; + PointCloud &next = *output[l]; +#pragma omp parallel for \ + default(none) \ + shared(next) \ + num_threads(threads_) + for(int i=0; i < next.height; ++i) { - for (int l = 1; l <= levels_; ++l) + for(int j=0; j < next.width; ++j) { - output[l].reset (new pcl::PointCloud (output[l-1]->width/2, output[l-1]->height/2)); - const PointCloud &previous = *output[l-1]; - PointCloud &next = *output[l]; -#ifdef _OPENMP -#pragma omp parallel for shared (next) num_threads(threads_) -#endif - for(int i=0; i < next.height; ++i) + float weight = 0; + float r = 0, g = 0, b = 0; + for(int m=0; m < kernel_rows; ++m) { - for(int j=0; j < next.width; ++j) + int mm = kernel_rows - 1 - m; + for(int n=0; n < kernel_cols; ++n) { - float weight = 0; - float r = 0, g = 0, b = 0; - for(int m=0; m < kernel_rows; ++m) - { - int mm = kernel_rows - 1 - m; - for(int n=0; n < kernel_cols; ++n) - { - int nn = kernel_cols - 1 - n; - int ii = 2*i + (m - kernel_center_y); - int jj = 2*j + (n - kernel_center_x); - if (ii < 0) ii = 0; - if (ii >= previous.height) ii = previous.height - 1; - if (jj < 0) jj = 0; - if (jj >= previous.width) jj = previous.width - 1; - if (!isFinite (previous.at (jj,ii))) - continue; - if (pcl::squaredEuclideanDistance (previous.at (2*j,2*i), previous.at (jj,ii)) < threshold_) - { - b += previous.at (jj,ii).b * kernel_ (mm,nn); - g += previous.at (jj,ii).g * kernel_ (mm,nn); - r += previous.at (jj,ii).r * kernel_ (mm,nn); - weight+= kernel_ (mm,nn); - } - } - } - if (weight == 0) - nullify (next.at (j,i)); - else + int nn = kernel_cols - 1 - n; + int ii = 2*i + (m - kernel_center_y); + int jj = 2*j + (n - kernel_center_x); + if (ii < 0) ii = 0; + if (ii >= previous.height) ii = previous.height - 1; + if (jj < 0) jj = 0; + if (jj >= previous.width) jj = previous.width - 1; + if (!isFinite (previous.at (jj,ii))) + continue; + if (pcl::squaredEuclideanDistance (previous.at (2*j,2*i), previous.at (jj,ii)) < threshold_) { - weight = 1.f/weight; - r*= weight; g*= weight; b*= weight; - next.at (j,i).b = static_cast (b); - next.at (j,i).g = static_cast (g); - next.at (j,i).r = static_cast (r); + b += previous.at (jj,ii).b * kernel_ (mm,nn); + g += previous.at (jj,ii).g * kernel_ (mm,nn); + r += previous.at (jj,ii).r * kernel_ (mm,nn); + weight+= kernel_ (mm,nn); } } } + if (weight == 0) + nullify (next.at (j,i)); + else + { + weight = 1.f/weight; + r*= weight; g*= weight; b*= weight; + next.at (j,i).b = static_cast (b); + next.at (j,i).g = static_cast (g); + next.at (j,i).r = static_cast (r); + } } - } + } } - } } +} // namespace filters +} // namespace pcl + #endif + diff --git a/filters/include/pcl/filters/impl/radius_outlier_removal.hpp b/filters/include/pcl/filters/impl/radius_outlier_removal.hpp index c3f96d79..9f73cd02 100644 --- a/filters/include/pcl/filters/impl/radius_outlier_removal.hpp +++ b/filters/include/pcl/filters/impl/radius_outlier_removal.hpp @@ -43,31 +43,6 @@ #include #include -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::RadiusOutlierRemoval::applyFilter (PointCloud &output) -{ - std::vector indices; - if (keep_organized_) - { - bool temp = extract_removed_indices_; - extract_removed_indices_ = true; - applyFilterIndices (indices); - extract_removed_indices_ = temp; - - output = *input_; - for (const auto ri : *removed_indices_) // ri = removed index - output.points[ri].x = output.points[ri].y = output.points[ri].z = user_filter_value_; - if (!std::isfinite (user_filter_value_)) - output.is_dense = false; - } - else - { - applyFilterIndices (indices); - copyPointCloud (*input_, indices, output); - } -} - //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template void pcl::RadiusOutlierRemoval::applyFilterIndices (std::vector &indices) diff --git a/filters/include/pcl/filters/impl/random_sample.hpp b/filters/include/pcl/filters/impl/random_sample.hpp index d7bebbb7..9db1143e 100644 --- a/filters/include/pcl/filters/impl/random_sample.hpp +++ b/filters/include/pcl/filters/impl/random_sample.hpp @@ -40,52 +40,9 @@ #include #include -#include +#include -/////////////////////////////////////////////////////////////////////////////// -template void -pcl::RandomSample::applyFilter (PointCloud &output) -{ - std::vector indices; - if (keep_organized_) - { - bool temp = extract_removed_indices_; - extract_removed_indices_ = true; - applyFilter (indices); - extract_removed_indices_ = temp; - copyPointCloud (*input_, output); - // Get X, Y, Z fields - const auto fields = pcl::getFields (); - std::vector offsets; - for (const auto &field : fields) - { - if (field.name == "x" || - field.name == "y" || - field.name == "z") - offsets.push_back (field.offset); - } - // For every "removed" point, set the x,y,z fields to user_filter_value_ - const static float user_filter_value = user_filter_value_; - for (const auto ri : *removed_indices_) // ri = removed index - { - std::uint8_t* pt_data = reinterpret_cast (&output[ri]); - for (const unsigned long &offset : offsets) - { - memcpy (pt_data + offset, &user_filter_value, sizeof (float)); - } - } - if (!std::isfinite (user_filter_value_)) - output.is_dense = false; - } - else - { - output.is_dense = true; - applyFilter (indices); - copyPointCloud (*input_, indices, output); - } -} - /////////////////////////////////////////////////////////////////////////////// template void diff --git a/filters/include/pcl/filters/impl/sampling_surface_normal.hpp b/filters/include/pcl/filters/impl/sampling_surface_normal.hpp index 38f2dd52..14f0eb9a 100644 --- a/filters/include/pcl/filters/impl/sampling_surface_normal.hpp +++ b/filters/include/pcl/filters/impl/sampling_surface_normal.hpp @@ -41,6 +41,7 @@ #include #include #include +#include // for pcl::isFinite #include /////////////////////////////////////////////////////////////////////////////// diff --git a/filters/include/pcl/filters/impl/statistical_outlier_removal.hpp b/filters/include/pcl/filters/impl/statistical_outlier_removal.hpp index 73110ef8..d09a9597 100644 --- a/filters/include/pcl/filters/impl/statistical_outlier_removal.hpp +++ b/filters/include/pcl/filters/impl/statistical_outlier_removal.hpp @@ -43,31 +43,6 @@ #include #include -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::StatisticalOutlierRemoval::applyFilter (PointCloud &output) -{ - std::vector indices; - if (keep_organized_) - { - bool temp = extract_removed_indices_; - extract_removed_indices_ = true; - applyFilterIndices (indices); - extract_removed_indices_ = temp; - - output = *input_; - for (const auto ri : *removed_indices_) // ri = removed index - output.points[ri].x = output.points[ri].y = output.points[ri].z = user_filter_value_; - if (!std::isfinite (user_filter_value_)) - output.is_dense = false; - } - else - { - applyFilterIndices (indices); - copyPointCloud (*input_, indices, output); - } -} - //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template void pcl::StatisticalOutlierRemoval::applyFilterIndices (std::vector &indices) diff --git a/filters/include/pcl/filters/impl/voxel_grid.hpp b/filters/include/pcl/filters/impl/voxel_grid.hpp index 6ba0bc6c..669b5c6f 100644 --- a/filters/include/pcl/filters/impl/voxel_grid.hpp +++ b/filters/include/pcl/filters/impl/voxel_grid.hpp @@ -42,6 +42,7 @@ #include #include #include +#include /////////////////////////////////////////////////////////////////////////////////////////// template void @@ -205,6 +206,7 @@ struct cloud_point_index_idx unsigned int idx; unsigned int cloud_point_index; + cloud_point_index_idx() = default; cloud_point_index_idx (unsigned int idx_, unsigned int cloud_point_index_) : idx (idx_), cloud_point_index (cloud_point_index_) {} bool operator < (const cloud_point_index_idx &p) const { return (idx < p.idx); } }; @@ -339,8 +341,9 @@ pcl::VoxelGrid::applyFilter (PointCloud &output) // Second pass: sort the index_vector vector using value representing target cell as index // in effect all points belonging to the same output cell will be next to each other - std::sort (index_vector.begin (), index_vector.end (), std::less ()); - + auto rightshift_func = [](const cloud_point_index_idx &x, const unsigned offset) { return x.idx >> offset; }; + boost::sort::spreadsort::integer_sort(index_vector.begin(), index_vector.end(), rightshift_func); + // Third pass: count output cells // we need to skip all the same, adjacent idx values unsigned int total = 0; diff --git a/filters/include/pcl/filters/model_outlier_removal.h b/filters/include/pcl/filters/model_outlier_removal.h index 31dcbd1d..edba6ab3 100644 --- a/filters/include/pcl/filters/model_outlier_removal.h +++ b/filters/include/pcl/filters/model_outlier_removal.h @@ -82,7 +82,7 @@ namespace pcl */ inline ModelOutlierRemoval (bool extract_removed_indices = false) : - FilterIndices::FilterIndices (extract_removed_indices) + FilterIndices (extract_removed_indices) { thresh_ = 0; normals_distance_weight_ = 0; @@ -199,12 +199,6 @@ namespace pcl using FilterIndices::extract_removed_indices_; using FilterIndices::removed_indices_; - /** \brief Filtered results are stored in a separate point cloud. - * \param[out] output The resultant point cloud. - */ - void - applyFilter (PointCloud &output) override; - /** \brief Filtered results are indexed by an indices array. * \param[out] indices The resultant indices. */ diff --git a/filters/include/pcl/filters/normal_refinement.h b/filters/include/pcl/filters/normal_refinement.h index 5d88ef3e..027e7cd1 100644 --- a/filters/include/pcl/filters/normal_refinement.h +++ b/filters/include/pcl/filters/normal_refinement.h @@ -37,6 +37,8 @@ #pragma once +#include +#include #include namespace pcl @@ -51,11 +53,13 @@ namespace pcl * \ingroup filters */ template inline std::vector - assignNormalWeights (const PointCloud&, - int, + assignNormalWeights (const PointCloud& cloud, + int index, const std::vector& k_indices, const std::vector& k_sqr_distances) { + pcl::utils::ignore(cloud); + pcl::utils::ignore(index); // Check inputs if (k_indices.size () != k_sqr_distances.size ()) PCL_ERROR("[pcl::assignNormalWeights] inequal size of neighbor indices and distances!\n"); diff --git a/filters/include/pcl/filters/normal_space.h b/filters/include/pcl/filters/normal_space.h index 3a3f65ef..522db676 100644 --- a/filters/include/pcl/filters/normal_space.h +++ b/filters/include/pcl/filters/normal_space.h @@ -39,8 +39,10 @@ #include #include + #include #include +#include // std::mt19937 namespace pcl { @@ -77,17 +79,10 @@ namespace pcl , binsy_ () , binsz_ () , input_normals_ () - , rng_uniform_distribution_ (nullptr) { filter_name_ = "NormalSpaceSampling"; } - /** \brief Destructor. */ - ~NormalSpaceSampling () - { - delete rng_uniform_distribution_; - } - /** \brief Set number of indices to be sampled. * \param[in] sample the number of sample indices */ @@ -164,12 +159,6 @@ namespace pcl /** \brief The normals computed at each point in the input cloud */ NormalsConstPtr input_normals_; - /** \brief Sample of point indices into a separate PointCloud - * \param[out] output the resultant point cloud - */ - void - applyFilter (PointCloud &output) override; - /** \brief Sample of point indices * \param[out] indices the resultant point cloud indices */ @@ -182,10 +171,9 @@ namespace pcl private: /** \brief Finds the bin number of the input normal, returns the bin number * \param[in] normal the input normal - * \param[in] nbins total number of bins */ unsigned int - findBin (const float *normal, unsigned int nbins); + findBin (const float *normal); /** \brief Checks of the entire bin is sampled, returns true or false * \param[out] array flag which says whether a point is sampled or not @@ -195,8 +183,8 @@ namespace pcl bool isEntireBinSampled (boost::dynamic_bitset<> &array, unsigned int start_index, unsigned int length); - /** \brief Uniform random distribution. */ - boost::variate_generator > *rng_uniform_distribution_; + /** \brief Random engine */ + std::mt19937 rng_; }; } diff --git a/filters/include/pcl/filters/passthrough.h b/filters/include/pcl/filters/passthrough.h index 3f5e2b6d..d0f541d4 100644 --- a/filters/include/pcl/filters/passthrough.h +++ b/filters/include/pcl/filters/passthrough.h @@ -95,7 +95,7 @@ namespace pcl * \param[in] extract_removed_indices Set to true if you want to be able to extract the indices of points being removed (default = false). */ PassThrough (bool extract_removed_indices = false) : - FilterIndices::FilterIndices (extract_removed_indices), + FilterIndices (extract_removed_indices), filter_field_name_ (""), filter_limit_min_ (FLT_MIN), filter_limit_max_ (FLT_MAX) @@ -150,6 +150,7 @@ namespace pcl * \warning This method will be removed in the future. Use setNegative() instead. * \param[in] limit_negative return data inside the interval (false) or outside (true) */ + PCL_DEPRECATED(1, 13, "use inherited FilterIndices::setNegative() instead") inline void setFilterLimitsNegative (const bool limit_negative) { @@ -160,6 +161,7 @@ namespace pcl * \warning This method will be removed in the future. Use getNegative() instead. * \param[out] limit_negative true if data \b outside the interval [min; max] is to be returned, false otherwise */ + PCL_DEPRECATED(1, 13, "use inherited FilterIndices::getNegative() instead") inline void getFilterLimitsNegative (bool &limit_negative) const { @@ -187,12 +189,6 @@ namespace pcl using FilterIndices::extract_removed_indices_; using FilterIndices::removed_indices_; - /** \brief Filtered results are stored in a separate point cloud. - * \param[out] output The resultant point cloud. - */ - void - applyFilter (PointCloud &output) override; - /** \brief Filtered results are indexed by an indices array. * \param[out] indices The resultant indices. */ @@ -287,7 +283,7 @@ namespace pcl * Default: false. * \param[in] limit_negative return data inside the interval (false) or outside (true) */ - PCL_DEPRECATED("use inherited FilterIndices::setNegative() instead") + PCL_DEPRECATED(1, 12, "use inherited FilterIndices::setNegative() instead") inline void setFilterLimitsNegative (const bool limit_negative) { @@ -297,7 +293,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 */ - PCL_DEPRECATED("use inherited FilterIndices::getNegative() instead") + PCL_DEPRECATED(1, 12, "use inherited FilterIndices::getNegative() instead") inline void getFilterLimitsNegative (bool &limit_negative) const { @@ -307,7 +303,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 */ - PCL_DEPRECATED("use inherited FilterIndices::getNegative() instead") + PCL_DEPRECATED(1, 12, "use inherited FilterIndices::getNegative() instead") inline bool getFilterLimitsNegative () const { diff --git a/filters/include/pcl/filters/pyramid.h b/filters/include/pcl/filters/pyramid.h index 3bb442b6..c37e0cb6 100644 --- a/filters/include/pcl/filters/pyramid.h +++ b/filters/include/pcl/filters/pyramid.h @@ -39,8 +39,8 @@ #pragma once +#include #include -#include #include #include diff --git a/filters/include/pcl/filters/radius_outlier_removal.h b/filters/include/pcl/filters/radius_outlier_removal.h index d87d74bd..99a69029 100644 --- a/filters/include/pcl/filters/radius_outlier_removal.h +++ b/filters/include/pcl/filters/radius_outlier_removal.h @@ -86,7 +86,7 @@ namespace pcl * \param[in] extract_removed_indices Set to true if you want to be able to extract the indices of points being removed (default = false). */ RadiusOutlierRemoval (bool extract_removed_indices = false) : - FilterIndices::FilterIndices (extract_removed_indices), + FilterIndices (extract_removed_indices), searcher_ (), search_radius_ (0.0), min_pts_radius_ (1) @@ -149,12 +149,6 @@ namespace pcl using FilterIndices::extract_removed_indices_; using FilterIndices::removed_indices_; - /** \brief Filtered results are stored in a separate point cloud. - * \param[out] output The resultant point cloud. - */ - void - applyFilter (PointCloud &output) override; - /** \brief Filtered results are indexed by an indices array. * \param[out] indices The resultant indices. */ diff --git a/filters/include/pcl/filters/random_sample.h b/filters/include/pcl/filters/random_sample.h index c023955d..e29f4c8c 100644 --- a/filters/include/pcl/filters/random_sample.h +++ b/filters/include/pcl/filters/random_sample.h @@ -123,12 +123,6 @@ namespace pcl /** \brief Random number seed. */ unsigned int seed_; - /** \brief Sample of point indices into a separate PointCloud - * \param output the resultant point cloud - */ - void - applyFilter (PointCloud &output) override; - /** \brief Sample of point indices * \param indices the resultant point cloud indices */ diff --git a/filters/include/pcl/filters/statistical_outlier_removal.h b/filters/include/pcl/filters/statistical_outlier_removal.h index 08451b64..bcc8a465 100644 --- a/filters/include/pcl/filters/statistical_outlier_removal.h +++ b/filters/include/pcl/filters/statistical_outlier_removal.h @@ -95,7 +95,7 @@ namespace pcl * \param[in] extract_removed_indices Set to true if you want to be able to extract the indices of points being removed (default = false). */ StatisticalOutlierRemoval (bool extract_removed_indices = false) : - FilterIndices::FilterIndices (extract_removed_indices), + FilterIndices (extract_removed_indices), searcher_ (), mean_k_ (1), std_mul_ (0.0) @@ -153,12 +153,6 @@ namespace pcl using FilterIndices::extract_removed_indices_; using FilterIndices::removed_indices_; - /** \brief Filtered results are stored in a separate point cloud. - * \param[out] output The resultant point cloud. - */ - void - applyFilter (PointCloud &output) override; - /** \brief Filtered results are indexed by an indices array. * \param[out] indices The resultant indices. */ diff --git a/filters/src/convolution.cpp b/filters/src/convolution.cpp new file mode 100644 index 00000000..41ffd02b --- /dev/null +++ b/filters/src/convolution.cpp @@ -0,0 +1,193 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2020-, Open Perception + * + * 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 + +namespace pcl { +namespace filters { +template <> +pcl::PointXYZRGB +Convolution::convolveOneRowDense(int i, int j) +{ + pcl::PointXYZRGB result; + float r = 0, g = 0, b = 0; + for (int k = kernel_width_, l = i - half_width_; k > -1; --k, ++l) { + result.x += (*input_)(l, j).x * kernel_[k]; + result.y += (*input_)(l, j).y * kernel_[k]; + result.z += (*input_)(l, j).z * kernel_[k]; + r += kernel_[k] * static_cast((*input_)(l, j).r); + g += kernel_[k] * static_cast((*input_)(l, j).g); + b += kernel_[k] * static_cast((*input_)(l, j).b); + } + result.r = static_cast(r); + result.g = static_cast(g); + result.b = static_cast(b); + return (result); +} + +template <> +pcl::PointXYZRGB +Convolution::convolveOneColDense(int i, int j) +{ + pcl::PointXYZRGB result; + float r = 0, g = 0, b = 0; + for (int k = kernel_width_, l = j - half_width_; k > -1; --k, ++l) { + result.x += (*input_)(i, l).x * kernel_[k]; + result.y += (*input_)(i, l).y * kernel_[k]; + result.z += (*input_)(i, l).z * kernel_[k]; + r += kernel_[k] * static_cast((*input_)(i, l).r); + g += kernel_[k] * static_cast((*input_)(i, l).g); + b += kernel_[k] * static_cast((*input_)(i, l).b); + } + result.r = static_cast(r); + result.g = static_cast(g); + result.b = static_cast(b); + return (result); +} + +template <> +pcl::PointXYZRGB +Convolution::convolveOneRowNonDense(int i, int j) +{ + pcl::PointXYZRGB result; + float weight = 0; + float r = 0, g = 0, b = 0; + for (int k = kernel_width_, l = i - half_width_; k > -1; --k, ++l) { + if (!isFinite((*input_)(l, j))) + continue; + if (pcl::squaredEuclideanDistance((*input_)(i, j), (*input_)(l, j)) < + distance_threshold_) { + result.x += (*input_)(l, j).x * kernel_[k]; + result.y += (*input_)(l, j).y * kernel_[k]; + result.z += (*input_)(l, j).z * kernel_[k]; + r += kernel_[k] * static_cast((*input_)(l, j).r); + g += kernel_[k] * static_cast((*input_)(l, j).g); + b += kernel_[k] * static_cast((*input_)(l, j).b); + weight += kernel_[k]; + } + } + + if (weight == 0) + result.x = result.y = result.z = std::numeric_limits::quiet_NaN(); + else { + weight = 1.f / weight; + r *= weight; + g *= weight; + b *= weight; + result.x *= weight; + result.y *= weight; + result.z *= weight; + result.r = static_cast(r); + result.g = static_cast(g); + result.b = static_cast(b); + } + return (result); +} + +template <> +pcl::PointXYZRGB +Convolution::convolveOneColNonDense(int i, int j) +{ + pcl::PointXYZRGB result; + float weight = 0; + float r = 0, g = 0, b = 0; + for (int k = kernel_width_, l = j - half_width_; k > -1; --k, ++l) { + if (!isFinite((*input_)(i, l))) + continue; + if (pcl::squaredEuclideanDistance((*input_)(i, j), (*input_)(i, l)) < + distance_threshold_) { + result.x += (*input_)(i, l).x * kernel_[k]; + result.y += (*input_)(i, l).y * kernel_[k]; + result.z += (*input_)(i, l).z * kernel_[k]; + r += kernel_[k] * static_cast((*input_)(i, l).r); + g += kernel_[k] * static_cast((*input_)(i, l).g); + b += kernel_[k] * static_cast((*input_)(i, l).b); + weight += kernel_[k]; + } + } + if (weight == 0) + result.x = result.y = result.z = std::numeric_limits::quiet_NaN(); + else { + weight = 1.f / weight; + r *= weight; + g *= weight; + b *= weight; + result.x *= weight; + result.y *= weight; + result.z *= weight; + result.r = static_cast(r); + result.g = static_cast(g); + result.b = static_cast(b); + } + return (result); +} + +template <> +pcl::RGB +Convolution::convolveOneRowDense(int i, int j) +{ + pcl::RGB result; + float r = 0, g = 0, b = 0; + for (int k = kernel_width_, l = i - half_width_; k > -1; --k, ++l) { + r += kernel_[k] * static_cast((*input_)(l, j).r); + g += kernel_[k] * static_cast((*input_)(l, j).g); + b += kernel_[k] * static_cast((*input_)(l, j).b); + } + result.r = static_cast(r); + result.g = static_cast(g); + result.b = static_cast(b); + return (result); +} + +template <> +pcl::RGB +Convolution::convolveOneColDense(int i, int j) +{ + pcl::RGB result; + float r = 0, g = 0, b = 0; + for (int k = kernel_width_, l = j - half_width_; k > -1; --k, ++l) { + r += kernel_[k] * static_cast((*input_)(i, l).r); + g += kernel_[k] * static_cast((*input_)(i, l).g); + b += kernel_[k] * static_cast((*input_)(i, l).b); + } + result.r = static_cast(r); + result.g = static_cast(g); + result.b = static_cast(b); + return (result); +} +} // namespace filters +} // namespace pcl diff --git a/filters/src/filter_indices.cpp b/filters/src/filter_indices.cpp index 7dc9d333..4735ef3b 100644 --- a/filters/src/filter_indices.cpp +++ b/filters/src/filter_indices.cpp @@ -62,6 +62,7 @@ pcl::FilterIndices::filter (std::vector &indices) // Instantiations of specific point types PCL_INSTANTIATE(removeNanFromPointCloud, PCL_XYZ_POINT_TYPES) +PCL_INSTANTIATE(FilterIndices, PCL_POINT_TYPES) #endif // PCL_NO_PRECOMPILE diff --git a/filters/src/radius_outlier_removal.cpp b/filters/src/radius_outlier_removal.cpp index 485bb804..10d4bd28 100644 --- a/filters/src/radius_outlier_removal.cpp +++ b/filters/src/radius_outlier_removal.cpp @@ -40,7 +40,7 @@ #include #include -#include +#include /////////////////////////////////////////////////////////////////////////////////////////// void diff --git a/filters/src/voxel_grid.cpp b/filters/src/voxel_grid.cpp index 68e44dba..92e5f71d 100644 --- a/filters/src/voxel_grid.cpp +++ b/filters/src/voxel_grid.cpp @@ -41,6 +41,7 @@ #include #include #include +#include using Array4size_t = Eigen::Array; @@ -377,7 +378,8 @@ pcl::VoxelGrid::applyFilter (PCLPointCloud2 &output) // Second pass: sort the index_vector vector using value representing target cell as index // in effect all points belonging to the same output cell will be next to each other - std::sort (index_vector.begin (), index_vector.end (), std::less ()); + auto rightshift_func = [](const cloud_point_index_idx &x, const unsigned offset) { return x.idx >> offset; }; + boost::sort::spreadsort::integer_sort(index_vector.begin(), index_vector.end(), rightshift_func); // Third pass: count output cells // we need to skip all the same, adjacenent idx values @@ -549,4 +551,3 @@ PCL_INSTANTIATE(getMinMax3D, PCL_XYZ_POINT_TYPES) PCL_INSTANTIATE(VoxelGrid, PCL_XYZ_POINT_TYPES) #endif // PCL_NO_PRECOMPILE - diff --git a/geometry/geometry.doxy b/geometry/geometry.doxy index 7e404ce8..051d18e0 100644 --- a/geometry/geometry.doxy +++ b/geometry/geometry.doxy @@ -5,7 +5,7 @@ The PCL geometry library contains computational geometry data structures and algorithms. - \section secRecognitionRequirements Requirements + \section secGeometryRequirements Requirements - \ref common "common" */ diff --git a/geometry/include/pcl/geometry/boost.h b/geometry/include/pcl/geometry/boost.h index d5421caa..75b6a6b5 100644 --- a/geometry/include/pcl/geometry/boost.h +++ b/geometry/include/pcl/geometry/boost.h @@ -45,5 +45,4 @@ #endif #include -#include #include diff --git a/geometry/include/pcl/geometry/mesh_base.h b/geometry/include/pcl/geometry/mesh_base.h index 9a3f20ee..1f96b121 100644 --- a/geometry/include/pcl/geometry/mesh_base.h +++ b/geometry/include/pcl/geometry/mesh_base.h @@ -46,6 +46,7 @@ #include #include #include +#include #include #include diff --git a/geometry/include/pcl/geometry/planar_polygon.h b/geometry/include/pcl/geometry/planar_polygon.h index bd9f56fb..8a419c88 100644 --- a/geometry/include/pcl/geometry/planar_polygon.h +++ b/geometry/include/pcl/geometry/planar_polygon.h @@ -40,6 +40,7 @@ #pragma once #include +#include #include #include #include diff --git a/geometry/include/pcl/geometry/polygon_mesh.h b/geometry/include/pcl/geometry/polygon_mesh.h index 3c743236..027e9264 100644 --- a/geometry/include/pcl/geometry/polygon_mesh.h +++ b/geometry/include/pcl/geometry/polygon_mesh.h @@ -40,6 +40,7 @@ #pragma once +#include #include #include diff --git a/geometry/include/pcl/geometry/polygon_operations.h b/geometry/include/pcl/geometry/polygon_operations.h index f43540ca..5df66828 100644 --- a/geometry/include/pcl/geometry/polygon_operations.h +++ b/geometry/include/pcl/geometry/polygon_operations.h @@ -33,8 +33,6 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * - * $Id: point_operators.h 4389 2012-02-10 10:10:26Z nizar $ - * */ #pragma once diff --git a/geometry/include/pcl/geometry/quad_mesh.h b/geometry/include/pcl/geometry/quad_mesh.h index 76dd113e..22b442db 100644 --- a/geometry/include/pcl/geometry/quad_mesh.h +++ b/geometry/include/pcl/geometry/quad_mesh.h @@ -40,6 +40,7 @@ #pragma once +#include #include #include diff --git a/geometry/include/pcl/geometry/triangle_mesh.h b/geometry/include/pcl/geometry/triangle_mesh.h index ceb62570..d5f7f5bf 100644 --- a/geometry/include/pcl/geometry/triangle_mesh.h +++ b/geometry/include/pcl/geometry/triangle_mesh.h @@ -42,6 +42,7 @@ #include +#include #include #include diff --git a/gpu/CMakeLists.txt b/gpu/CMakeLists.txt index 289fd71d..99f36bf6 100644 --- a/gpu/CMakeLists.txt +++ b/gpu/CMakeLists.txt @@ -20,4 +20,4 @@ topological_sort(PCL_GPU_MODULES_NAMES PCL_ _DEPENDS) sort_relative(PCL_GPU_MODULES_NAMES_UNSORTED PCL_GPU_MODULES_NAMES PCL_GPU_MODULES_DIRS) foreach(subdir ${PCL_GPU_MODULES_DIRS}) add_subdirectory("${CMAKE_CURRENT_SOURCE_DIR}/${subdir}") -endforeach() \ No newline at end of file +endforeach() diff --git a/gpu/containers/include/pcl/gpu/containers/impl/device_array.hpp b/gpu/containers/include/pcl/gpu/containers/impl/device_array.hpp index 323c0743..cac65148 100644 --- a/gpu/containers/include/pcl/gpu/containers/impl/device_array.hpp +++ b/gpu/containers/include/pcl/gpu/containers/impl/device_array.hpp @@ -34,82 +34,92 @@ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com) */ + #ifndef PCL_GPU_CONTAINER_DEVICE_ARRAY_IMPL_HPP_ #define PCL_GPU_CONTAINER_DEVICE_ARRAY_IMPL_HPP_ +namespace pcl +{ + +namespace gpu +{ + ///////////////////// Inline implementations of DeviceArray //////////////////////////////////////////// -template inline pcl::gpu::DeviceArray::DeviceArray() {} -template inline pcl::gpu::DeviceArray::DeviceArray(std::size_t size) : DeviceMemory(size * elem_size) {} -template inline pcl::gpu::DeviceArray::DeviceArray(T *ptr, std::size_t size) : DeviceMemory(ptr, size * elem_size) {} -template inline pcl::gpu::DeviceArray::DeviceArray(const DeviceArray& other) : DeviceMemory(other) {} -template inline pcl::gpu::DeviceArray& pcl::gpu::DeviceArray::operator=(const DeviceArray& other) +template inline DeviceArray::DeviceArray() {} +template inline DeviceArray::DeviceArray(std::size_t size) : DeviceMemory(size * elem_size) {} +template inline DeviceArray::DeviceArray(T *ptr, std::size_t size) : DeviceMemory(ptr, size * elem_size) {} +template inline DeviceArray::DeviceArray(const DeviceArray& other) : DeviceMemory(other) {} +template inline DeviceArray& DeviceArray::operator=(const DeviceArray& other) { DeviceMemory::operator=(other); return *this; } -template inline void pcl::gpu::DeviceArray::create(std::size_t size) +template inline void DeviceArray::create(std::size_t size) { DeviceMemory::create(size * elem_size); } -template inline void pcl::gpu::DeviceArray::release() +template inline void DeviceArray::release() { DeviceMemory::release(); } -template inline void pcl::gpu::DeviceArray::copyTo(DeviceArray& other) const +template inline void DeviceArray::copyTo(DeviceArray& other) const { DeviceMemory::copyTo(other); } -template inline void pcl::gpu::DeviceArray::upload(const T *host_ptr, std::size_t size) +template inline void DeviceArray::upload(const T *host_ptr, std::size_t size) { DeviceMemory::upload(host_ptr, size * elem_size); } -template inline void pcl::gpu::DeviceArray::download(T *host_ptr) const +template inline void DeviceArray::download(T *host_ptr) const { DeviceMemory::download( host_ptr ); } -template void pcl::gpu::DeviceArray::swap(DeviceArray& other_arg) { DeviceMemory::swap(other_arg); } +template void DeviceArray::swap(DeviceArray& other_arg) { DeviceMemory::swap(other_arg); } -template inline pcl::gpu::DeviceArray::operator T*() { return ptr(); } -template inline pcl::gpu::DeviceArray::operator const T*() const { return ptr(); } -template inline std::size_t pcl::gpu::DeviceArray::size() const { return sizeBytes() / elem_size; } +template inline DeviceArray::operator T*() { return ptr(); } +template inline DeviceArray::operator const T*() const { return ptr(); } +template inline std::size_t DeviceArray::size() const { return sizeBytes() / elem_size; } -template inline T* pcl::gpu::DeviceArray::ptr() { return DeviceMemory::ptr(); } -template inline const T* pcl::gpu::DeviceArray::ptr() const { return DeviceMemory::ptr(); } +template inline T* DeviceArray::ptr() { return DeviceMemory::ptr(); } +template inline const T* DeviceArray::ptr() const { return DeviceMemory::ptr(); } -template template inline void pcl::gpu::DeviceArray::upload(const std::vector& data) { upload(&data[0], data.size()); } -template template inline void pcl::gpu::DeviceArray::download(std::vector& data) const { data.resize(size()); if (!data.empty()) download(&data[0]); } +template template inline void DeviceArray::upload(const std::vector& data) { upload(&data[0], data.size()); } +template template inline void DeviceArray::download(std::vector& data) const { data.resize(size()); if (!data.empty()) download(&data[0]); } ///////////////////// Inline implementations of DeviceArray2D //////////////////////////////////////////// -template inline pcl::gpu::DeviceArray2D::DeviceArray2D() {} -template inline pcl::gpu::DeviceArray2D::DeviceArray2D(int rows, int cols) : DeviceMemory2D(rows, cols * elem_size) {} -template inline pcl::gpu::DeviceArray2D::DeviceArray2D(int rows, int cols, void *data, std::size_t stepBytes) : DeviceMemory2D(rows, cols * elem_size, data, stepBytes) {} -template inline pcl::gpu::DeviceArray2D::DeviceArray2D(const DeviceArray2D& other) : DeviceMemory2D(other) {} -template inline pcl::gpu::DeviceArray2D& pcl::gpu::DeviceArray2D::operator=(const DeviceArray2D& other) +template inline DeviceArray2D::DeviceArray2D() {} +template inline DeviceArray2D::DeviceArray2D(int rows, int cols) : DeviceMemory2D(rows, cols * elem_size) {} +template inline DeviceArray2D::DeviceArray2D(int rows, int cols, void *data, std::size_t stepBytes) : DeviceMemory2D(rows, cols * elem_size, data, stepBytes) {} +template inline DeviceArray2D::DeviceArray2D(const DeviceArray2D& other) : DeviceMemory2D(other) {} +template inline DeviceArray2D& DeviceArray2D::operator=(const DeviceArray2D& other) { DeviceMemory2D::operator=(other); return *this; } -template inline void pcl::gpu::DeviceArray2D::create(int rows, int cols) +template inline void DeviceArray2D::create(int rows, int cols) { DeviceMemory2D::create(rows, cols * elem_size); } -template inline void pcl::gpu::DeviceArray2D::release() +template inline void DeviceArray2D::release() { DeviceMemory2D::release(); } -template inline void pcl::gpu::DeviceArray2D::copyTo(DeviceArray2D& other) const +template inline void DeviceArray2D::copyTo(DeviceArray2D& other) const { DeviceMemory2D::copyTo(other); } -template inline void pcl::gpu::DeviceArray2D::upload(const void *host_ptr, std::size_t host_step, int rows, int cols) +template inline void DeviceArray2D::upload(const void *host_ptr, std::size_t host_step, int rows, int cols) { DeviceMemory2D::upload(host_ptr, host_step, rows, cols * elem_size); } -template inline void pcl::gpu::DeviceArray2D::download(void *host_ptr, std::size_t host_step) const +template inline void DeviceArray2D::download(void *host_ptr, std::size_t host_step) const { DeviceMemory2D::download( host_ptr, host_step ); } -template template inline void pcl::gpu::DeviceArray2D::upload(const std::vector& data, int cols) +template template inline void DeviceArray2D::upload(const std::vector& data, int cols) { upload(&data[0], cols * elem_size, data.size()/cols, cols); } -template template inline void pcl::gpu::DeviceArray2D::download(std::vector& data, int& elem_step) const +template template inline void DeviceArray2D::download(std::vector& data, int& elem_step) const { elem_step = cols(); data.resize(cols() * rows()); if (!data.empty()) download(&data[0], colsBytes()); } -template void pcl::gpu::DeviceArray2D::swap(DeviceArray2D& other_arg) { DeviceMemory2D::swap(other_arg); } +template void DeviceArray2D::swap(DeviceArray2D& other_arg) { DeviceMemory2D::swap(other_arg); } + +template inline T* DeviceArray2D::ptr(int y) { return DeviceMemory2D::ptr(y); } +template inline const T* DeviceArray2D::ptr(int y) const { return DeviceMemory2D::ptr(y); } + +template inline DeviceArray2D::operator T*() { return ptr(); } +template inline DeviceArray2D::operator const T*() const { return ptr(); } -template inline T* pcl::gpu::DeviceArray2D::ptr(int y) { return DeviceMemory2D::ptr(y); } -template inline const T* pcl::gpu::DeviceArray2D::ptr(int y) const { return DeviceMemory2D::ptr(y); } - -template inline pcl::gpu::DeviceArray2D::operator T*() { return ptr(); } -template inline pcl::gpu::DeviceArray2D::operator const T*() const { return ptr(); } +template inline int DeviceArray2D::cols() const { return DeviceMemory2D::colsBytes()/elem_size; } +template inline int DeviceArray2D::rows() const { return DeviceMemory2D::rows(); } -template inline int pcl::gpu::DeviceArray2D::cols() const { return DeviceMemory2D::colsBytes()/elem_size; } -template inline int pcl::gpu::DeviceArray2D::rows() const { return DeviceMemory2D::rows(); } +template inline std::size_t DeviceArray2D::elem_step() const { return DeviceMemory2D::step()/elem_size; } -template inline std::size_t pcl::gpu::DeviceArray2D::elem_step() const { return DeviceMemory2D::step()/elem_size; } +} // namespace gpu +} // namespace pcl +#endif /* PCL_GPU_CONTAINER_DEVICE_ARRAY_IMPL_HPP_ */ -#endif /* PCL_GPU_CONTAINER_DEVICE_ARRAY_IMPL_HPP_ */ diff --git a/gpu/containers/include/pcl/gpu/containers/impl/device_memory.hpp b/gpu/containers/include/pcl/gpu/containers/impl/device_memory.hpp index c368a6f0..265a8e82 100644 --- a/gpu/containers/include/pcl/gpu/containers/impl/device_memory.hpp +++ b/gpu/containers/include/pcl/gpu/containers/impl/device_memory.hpp @@ -37,25 +37,49 @@ #ifndef PCL_GPU_CONTAINER_DEVICE_MEMORY_IMPL_HPP_ #define PCL_GPU_CONTAINER_DEVICE_MEMORY_IMPL_HPP_ +namespace pcl +{ + +namespace gpu +{ + ///////////////////// Inline implementations of DeviceMemory //////////////////////////////////////////// +template inline T* +DeviceMemory::ptr() +{ + return (T*)data_; +} + +template inline const T* +DeviceMemory::ptr() const +{ + return (const T*)data_; +} -template inline T* pcl::gpu::DeviceMemory::ptr() { return ( T*)data_; } -template inline const T* pcl::gpu::DeviceMemory::ptr() const { return (const T*)data_; } - -template inline pcl::gpu::DeviceMemory::operator pcl::gpu::PtrSz() const +template inline DeviceMemory::operator +PtrSz() const { PtrSz result; result.data = (U*)ptr(); result.size = sizeBytes_/sizeof(U); - return result; + return result; } ///////////////////// Inline implementations of DeviceMemory2D //////////////////////////////////////////// - -template T* pcl::gpu::DeviceMemory2D::ptr(int y_arg) { return ( T*)(( char*)data_ + y_arg * step_); } -template const T* pcl::gpu::DeviceMemory2D::ptr(int y_arg) const { return (const T*)((const char*)data_ + y_arg * step_); } - -template pcl::gpu::DeviceMemory2D::operator pcl::gpu::PtrStep() const +template T* +DeviceMemory2D::ptr(int y_arg) +{ + return (T*)((char*)data_ + y_arg * step_); +} + +template const T* +DeviceMemory2D::ptr(int y_arg) const +{ + return (const T*)((const char*)data_ + y_arg * step_); +} + +template DeviceMemory2D::operator +PtrStep() const { PtrStep result; result.data = (U*)ptr(); @@ -63,7 +87,8 @@ template pcl::gpu::DeviceMemory2D::operator pcl::gpu::PtrStep() con return result; } -template pcl::gpu::DeviceMemory2D::operator pcl::gpu::PtrStepSz() const +template DeviceMemory2D::operator +PtrStepSz() const { PtrStepSz result; result.data = (U*)ptr(); @@ -73,5 +98,7 @@ template pcl::gpu::DeviceMemory2D::operator pcl::gpu::PtrStepSz() c return result; } -#endif /* PCL_GPU_CONTAINER_DEVICE_MEMORY_IMPL_HPP_ */ +} // namespace gpu +} // namespace pcl +#endif /* PCL_GPU_CONTAINER_DEVICE_MEMORY_IMPL_HPP_ */ diff --git a/gpu/examples/octree/src/octree_search.cpp b/gpu/examples/octree/src/octree_search.cpp index c8911ccd..fba1f0f5 100644 --- a/gpu/examples/octree/src/octree_search.cpp +++ b/gpu/examples/octree/src/octree_search.cpp @@ -3,10 +3,11 @@ #include #include +#include #include #include -#include #include + #include int main (int argc, char** argv) diff --git a/gpu/features/include/pcl/gpu/features/features.hpp b/gpu/features/include/pcl/gpu/features/features.hpp index 9ddf1beb..2576f80e 100644 --- a/gpu/features/include/pcl/gpu/features/features.hpp +++ b/gpu/features/include/pcl/gpu/features/features.hpp @@ -99,7 +99,7 @@ namespace pcl NormalEstimation(); void compute(Normals& normals); void setViewPoint(float vpx, float vpy, float vpz); - void getViewPoint(float& vpx, float& vpy, float& vpz); + void getViewPoint(float& vpx, float& vpy, float& vpz) const; static void computeNormals(const PointCloud& cloud, const NeighborIndices& nn_indices, Normals& normals); static void flipNormalTowardsViewpoint(const PointCloud& cloud, float vp_x, float vp_y, float vp_z, Normals& normals); @@ -223,7 +223,7 @@ namespace pcl VFHEstimation(); void setViewPoint(float vpx, float vpy, float vpz); - void getViewPoint(float& vpx, float& vpy, float& vpz); + void getViewPoint(float& vpx, float& vpy, float& vpz) const; void setUseGivenNormal (bool use); void setNormalToUse (const NormalType& normal); diff --git a/gpu/features/src/features.cpp b/gpu/features/src/features.cpp index 5d370295..b3270c54 100644 --- a/gpu/features/src/features.cpp +++ b/gpu/features/src/features.cpp @@ -94,7 +94,7 @@ void pcl::gpu::NormalEstimation::setViewPoint (float vpx, float vpy, float vpz) vpx_ = vpx; vpy_ = vpy; vpz_ = vpz; } -void pcl::gpu::NormalEstimation::getViewPoint (float &vpx, float &vpy, float &vpz) +void pcl::gpu::NormalEstimation::getViewPoint (float &vpx, float &vpy, float &vpz) const { vpx = vpx_; vpy = vpy_; vpz = vpz_; } @@ -383,7 +383,7 @@ pcl::gpu::VFHEstimation::VFHEstimation() } void pcl::gpu::VFHEstimation::setViewPoint(float vpx, float vpy, float vpz) { vpx_ = vpx; vpy_ = vpy; vpz_ = vpz; } -void pcl::gpu::VFHEstimation::getViewPoint(float& vpx, float& vpy, float& vpz) { vpx = vpx_; vpy = vpy_; vpz = vpz_; } +void pcl::gpu::VFHEstimation::getViewPoint(float& vpx, float& vpy, float& vpz) const { vpx = vpx_; vpy = vpy_; vpz = vpz_; } void pcl::gpu::VFHEstimation::setUseGivenNormal (bool use) { use_given_normal_ = use; } void pcl::gpu::VFHEstimation::setNormalToUse (const NormalType& normal) { normal_to_use_ = normal; } diff --git a/gpu/kinfu/include/pcl/gpu/kinfu/color_volume.h b/gpu/kinfu/include/pcl/gpu/kinfu/color_volume.h index b8eb2fb8..8c843fa6 100644 --- a/gpu/kinfu/include/pcl/gpu/kinfu/color_volume.h +++ b/gpu/kinfu/include/pcl/gpu/kinfu/color_volume.h @@ -37,6 +37,7 @@ #pragma once +#include #include #include #include diff --git a/gpu/kinfu/include/pcl/gpu/kinfu/kinfu.h b/gpu/kinfu/include/pcl/gpu/kinfu/kinfu.h index cbe6e927..7e24a9ac 100644 --- a/gpu/kinfu/include/pcl/gpu/kinfu/kinfu.h +++ b/gpu/kinfu/include/pcl/gpu/kinfu/kinfu.h @@ -37,6 +37,7 @@ #pragma once +#include #include #include #include @@ -97,7 +98,7 @@ namespace pcl * \param[out] cy principal point y */ void - getDepthIntrinsics (float& fx, float& fy, float& cx, float& cy); + getDepthIntrinsics (float& fx, float& fy, float& cx, float& cy) const; /** \brief Sets initial camera pose relative to volume coordinate space diff --git a/gpu/kinfu/include/pcl/gpu/kinfu/marching_cubes.h b/gpu/kinfu/include/pcl/gpu/kinfu/marching_cubes.h index 6c36245e..f1ee497d 100644 --- a/gpu/kinfu/include/pcl/gpu/kinfu/marching_cubes.h +++ b/gpu/kinfu/include/pcl/gpu/kinfu/marching_cubes.h @@ -37,6 +37,7 @@ #pragma once +#include #include #include #include diff --git a/gpu/kinfu/include/pcl/gpu/kinfu/raycaster.h b/gpu/kinfu/include/pcl/gpu/kinfu/raycaster.h index 2bd37d0e..07a701da 100644 --- a/gpu/kinfu/include/pcl/gpu/kinfu/raycaster.h +++ b/gpu/kinfu/include/pcl/gpu/kinfu/raycaster.h @@ -37,11 +37,11 @@ #pragma once +#include #include #include #include #include -#include #include namespace pcl diff --git a/gpu/kinfu/include/pcl/gpu/kinfu/tsdf_volume.h b/gpu/kinfu/include/pcl/gpu/kinfu/tsdf_volume.h index 2efa4d31..366e71e3 100644 --- a/gpu/kinfu/include/pcl/gpu/kinfu/tsdf_volume.h +++ b/gpu/kinfu/include/pcl/gpu/kinfu/tsdf_volume.h @@ -37,6 +37,7 @@ #pragma once +#include #include #include #include diff --git a/gpu/kinfu/src/kinfu.cpp b/gpu/kinfu/src/kinfu.cpp index e009f714..04cf5f4d 100644 --- a/gpu/kinfu/src/kinfu.cpp +++ b/gpu/kinfu/src/kinfu.cpp @@ -114,7 +114,7 @@ pcl::gpu::KinfuTracker::setDepthIntrinsics (float fx, float fy, float cx, float /////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void -pcl::gpu::KinfuTracker::getDepthIntrinsics (float& fx, float& fy, float& cx, float& cy) +pcl::gpu::KinfuTracker::getDepthIntrinsics (float& fx, float& fy, float& cx, float& cy) const { fx = fx_; fy = fy_; diff --git a/gpu/kinfu/src/tsdf_volume.cpp b/gpu/kinfu/src/tsdf_volume.cpp index 51ea9312..c708a768 100644 --- a/gpu/kinfu/src/tsdf_volume.cpp +++ b/gpu/kinfu/src/tsdf_volume.cpp @@ -304,7 +304,9 @@ pcl::gpu::TsdfVolume::downloadTsdf (std::vector& tsdf) const tsdf.resize (volume_.cols() * volume_.rows()); volume_.download(&tsdf[0], volume_.cols() * sizeof(int)); -#pragma omp parallel for +#pragma omp parallel for \ + default(none) \ + shared(tsdf) for(int i = 0; i < (int) tsdf.size(); ++i) { float tmp = reinterpret_cast(&tsdf[i])->x; @@ -322,7 +324,9 @@ pcl::gpu::TsdfVolume::downloadTsdfAndWeighs (std::vector& tsdf, std::vect weights.resize (volumeSize); volume_.download(&tsdf[0], volume_.cols() * sizeof(int)); -#pragma omp parallel for +#pragma omp parallel for \ + default(none) \ + shared(weights, tsdf) for(int i = 0; i < (int) tsdf.size(); ++i) { short2 elem = *reinterpret_cast(&tsdf[i]); diff --git a/gpu/kinfu/tools/camera_pose.h b/gpu/kinfu/tools/camera_pose.h index e54ba723..2bf9a18f 100644 --- a/gpu/kinfu/tools/camera_pose.h +++ b/gpu/kinfu/tools/camera_pose.h @@ -43,7 +43,7 @@ #include #include -#include +#include /** * @brief The CameraPoseProcessor class is an interface to extract diff --git a/gpu/kinfu/tools/evaluation.h b/gpu/kinfu/tools/evaluation.h index 0ea5ad9e..552a1f58 100644 --- a/gpu/kinfu/tools/evaluation.h +++ b/gpu/kinfu/tools/evaluation.h @@ -39,7 +39,7 @@ #include #include -#include +#include #include #include diff --git a/gpu/kinfu/tools/kinfu_app.cpp b/gpu/kinfu/tools/kinfu_app.cpp index f8c4ed1e..e1572b80 100644 --- a/gpu/kinfu/tools/kinfu_app.cpp +++ b/gpu/kinfu/tools/kinfu_app.cpp @@ -68,7 +68,7 @@ #include "evaluation.h" #include -#include +#include #include "tsdf_volume.h" #include "tsdf_volume.hpp" @@ -404,7 +404,7 @@ struct ImageView } void - showDepth (const PtrStepSz& depth) + showDepth (const PtrStepSz& depth) const { if (viz_) viewerDepth_->showShortImage (depth.data, depth.cols, depth.rows, 0, 5000, true, "short_image"); @@ -582,7 +582,7 @@ struct SceneCloudView } void - clearClouds (bool print_message = false) + clearClouds (bool print_message = false) const { if (!viz_) return; diff --git a/gpu/kinfu/tools/kinfu_app_sim.cpp b/gpu/kinfu/tools/kinfu_app_sim.cpp index fa5c7dc0..ca7d4271 100644 --- a/gpu/kinfu/tools/kinfu_app_sim.cpp +++ b/gpu/kinfu/tools/kinfu_app_sim.cpp @@ -84,7 +84,7 @@ #include "evaluation.h" #include -#include +#include #include "tsdf_volume.h" #include "tsdf_volume.hpp" @@ -101,7 +101,7 @@ using ScopeTimeT = pcl::ScopeTime; #include #include #include -#include +#include #ifdef _WIN32 # define WIN32_LEAN_AND_MEAN # include @@ -112,8 +112,6 @@ using ScopeTimeT = pcl::ScopeTime; #include "pcl/common/common.h" #include "pcl/common/transforms.h" #include -// define the following in order to eliminate the deprecated headers warning -#define VTK_EXCLUDE_STRSTREAM_HEADERS #include // #include diff --git a/gpu/kinfu/tools/record_tsdfvolume.cpp b/gpu/kinfu/tools/record_tsdfvolume.cpp index 18f34a79..fb5b4ebb 100644 --- a/gpu/kinfu/tools/record_tsdfvolume.cpp +++ b/gpu/kinfu/tools/record_tsdfvolume.cpp @@ -181,7 +181,9 @@ DeviceVolume::getVolume (pcl::TSDFVolume::Ptr &volume) device_volume_.download (&volume_vec[0], device_volume_.cols() * sizeof(int)); - #pragma omp parallel for + #pragma omp parallel for \ + default(none) \ + shared(volume, volume_vec, weights_vec) for(int i = 0; i < (int) volume->size(); ++i) { short2 *elem = (short2*)&volume_vec[i]; diff --git a/gpu/kinfu/tools/tsdf_volume.h b/gpu/kinfu/tools/tsdf_volume.h index ecf3feaa..5b700d6e 100644 --- a/gpu/kinfu/tools/tsdf_volume.h +++ b/gpu/kinfu/tools/tsdf_volume.h @@ -36,6 +36,7 @@ #pragma once +#include #include #include #include @@ -225,7 +226,7 @@ public: // Functionality /** \brief Converts volume to cloud of TSDF values - * \param[ou] cloud - the output point cloud + * \param[out] cloud - the output point cloud * \param[in] step - the decimation step to use */ void diff --git a/gpu/kinfu/tools/tsdf_volume.hpp b/gpu/kinfu/tools/tsdf_volume.hpp index 86bb94cd..6ffa5a44 100644 --- a/gpu/kinfu/tools/tsdf_volume.hpp +++ b/gpu/kinfu/tools/tsdf_volume.hpp @@ -168,7 +168,6 @@ pcl::TSDFVolume::convertToTsdfCloud (pcl::PointCloudreserve (std::min (cloud_size/10, 500000)); int volume_idx = 0, cloud_idx = 0; -//#pragma omp parallel for // if used, increment over idx not possible! use index calculation for (int z = 0; z < sz; z+=step) for (int y = 0; y < sy; y+=step) for (int x = 0; x < sx; x+=step, ++cloud_idx) @@ -245,7 +244,8 @@ pcl::TSDFVolume::extractNeighborhood (const Eigen::Vector3i &vo const Eigen::RowVector3i offset_vector (1, neighborhood_size, neighborhood_size*neighborhood_size); // loop over all voxels in 3D neighborhood - #pragma omp parallel for + #pragma omp parallel for \ + default(none) for (int z = min_index(2); z <= max_index(2); ++z) { for (int y = min_index(1); y <= max_index(1); ++y) @@ -297,7 +297,8 @@ pcl::TSDFVolume::addNeighborhood (const Eigen::Vector3i &voxel_ Eigen::Vector3i index = min_index; // loop over all voxels in 3D neighborhood - #pragma omp parallel for + #pragma omp parallel for \ + default(none) for (int z = min_index(2); z <= max_index(2); ++z) { for (int y = min_index(1); y <= max_index(1); ++y) @@ -329,7 +330,8 @@ pcl::TSDFVolume::addNeighborhood (const Eigen::Vector3i &voxel_ template void pcl::TSDFVolume::averageValues () { - #pragma omp parallel for + #pragma omp parallel for \ + default(none) for (std::size_t i = 0; i < volume_->size(); ++i) { WeightT &w = weights_->at(i); diff --git a/gpu/kinfu_large_scale/include/pcl/gpu/kinfu_large_scale/color_volume.h b/gpu/kinfu_large_scale/include/pcl/gpu/kinfu_large_scale/color_volume.h index 35379a19..5b58dcab 100644 --- a/gpu/kinfu_large_scale/include/pcl/gpu/kinfu_large_scale/color_volume.h +++ b/gpu/kinfu_large_scale/include/pcl/gpu/kinfu_large_scale/color_volume.h @@ -37,6 +37,7 @@ #pragma once +#include #include #include #include diff --git a/gpu/kinfu_large_scale/include/pcl/gpu/kinfu_large_scale/impl/standalone_marching_cubes.hpp b/gpu/kinfu_large_scale/include/pcl/gpu/kinfu_large_scale/impl/standalone_marching_cubes.hpp index 06d88993..a20af684 100644 --- a/gpu/kinfu_large_scale/include/pcl/gpu/kinfu_large_scale/impl/standalone_marching_cubes.hpp +++ b/gpu/kinfu_large_scale/include/pcl/gpu/kinfu_large_scale/impl/standalone_marching_cubes.hpp @@ -39,7 +39,7 @@ #define PCL_STANDALONE_MARCHING_CUBES_IMPL_HPP_ #include -#include +#include /////////////////////////////////////////////////////////////////////////////// template @@ -191,8 +191,9 @@ pcl::gpu::kinfuLS::StandaloneMarchingCubes::convertTsdfVectors (const Po constexpr int DIVISOR = std::numeric_limits::max(); ///For every point in the cloud -#pragma omp parallel for - +#pragma omp parallel for \ + default(none) \ + shared(cloud, output) for(int i = 0; i < (int) cloud.points.size (); ++i) { int x = cloud.points[i].x; diff --git a/gpu/kinfu_large_scale/include/pcl/gpu/kinfu_large_scale/kinfu.h b/gpu/kinfu_large_scale/include/pcl/gpu/kinfu_large_scale/kinfu.h index fe97d5fc..6c8f1f35 100644 --- a/gpu/kinfu_large_scale/include/pcl/gpu/kinfu_large_scale/kinfu.h +++ b/gpu/kinfu_large_scale/include/pcl/gpu/kinfu_large_scale/kinfu.h @@ -37,6 +37,7 @@ #pragma once +#include #include #include #include diff --git a/gpu/kinfu_large_scale/include/pcl/gpu/kinfu_large_scale/marching_cubes.h b/gpu/kinfu_large_scale/include/pcl/gpu/kinfu_large_scale/marching_cubes.h index aeb6ed8a..8cf8236e 100644 --- a/gpu/kinfu_large_scale/include/pcl/gpu/kinfu_large_scale/marching_cubes.h +++ b/gpu/kinfu_large_scale/include/pcl/gpu/kinfu_large_scale/marching_cubes.h @@ -37,6 +37,7 @@ #pragma once +#include #include #include #include diff --git a/gpu/kinfu_large_scale/include/pcl/gpu/kinfu_large_scale/point_intensity.h b/gpu/kinfu_large_scale/include/pcl/gpu/kinfu_large_scale/point_intensity.h index a581256e..97e1f609 100644 --- a/gpu/kinfu_large_scale/include/pcl/gpu/kinfu_large_scale/point_intensity.h +++ b/gpu/kinfu_large_scale/include/pcl/gpu/kinfu_large_scale/point_intensity.h @@ -39,6 +39,7 @@ #include +#include #include #include #include diff --git a/gpu/kinfu_large_scale/include/pcl/gpu/kinfu_large_scale/raycaster.h b/gpu/kinfu_large_scale/include/pcl/gpu/kinfu_large_scale/raycaster.h index f29c1079..c1513a11 100644 --- a/gpu/kinfu_large_scale/include/pcl/gpu/kinfu_large_scale/raycaster.h +++ b/gpu/kinfu_large_scale/include/pcl/gpu/kinfu_large_scale/raycaster.h @@ -37,11 +37,11 @@ #pragma once +#include #include #include #include #include -#include //#include #include diff --git a/gpu/kinfu_large_scale/include/pcl/gpu/kinfu_large_scale/screenshot_manager.h b/gpu/kinfu_large_scale/include/pcl/gpu/kinfu_large_scale/screenshot_manager.h index 261b86bf..fc03d4fb 100644 --- a/gpu/kinfu_large_scale/include/pcl/gpu/kinfu_large_scale/screenshot_manager.h +++ b/gpu/kinfu_large_scale/include/pcl/gpu/kinfu_large_scale/screenshot_manager.h @@ -91,7 +91,7 @@ namespace pcl /**Write camera pose to file*/ void - writePose(const std::string &filename_pose, const Eigen::Vector3f &teVecs, const Eigen::Matrix &erreMats); + writePose(const std::string &filename_pose, const Eigen::Vector3f &teVecs, const Eigen::Matrix &erreMats) const; /**Counter of the number of screenshots taken*/ int screenshot_counter; diff --git a/gpu/kinfu_large_scale/include/pcl/gpu/kinfu_large_scale/tsdf_volume.h b/gpu/kinfu_large_scale/include/pcl/gpu/kinfu_large_scale/tsdf_volume.h index 9f5bee57..d4478579 100644 --- a/gpu/kinfu_large_scale/include/pcl/gpu/kinfu_large_scale/tsdf_volume.h +++ b/gpu/kinfu_large_scale/include/pcl/gpu/kinfu_large_scale/tsdf_volume.h @@ -37,6 +37,7 @@ #pragma once +#include #include #include #include @@ -244,7 +245,7 @@ namespace pcl } /** \brief Converts volume stored on host to cloud of TSDF values - * \param[ou] cloud - the output point cloud + * \param[out] cloud - the output point cloud * \param[in] step - the decimation step to use */ void diff --git a/gpu/kinfu_large_scale/src/screenshot_manager.cpp b/gpu/kinfu_large_scale/src/screenshot_manager.cpp index f37e41a4..15e6a9a8 100644 --- a/gpu/kinfu_large_scale/src/screenshot_manager.cpp +++ b/gpu/kinfu_large_scale/src/screenshot_manager.cpp @@ -97,7 +97,7 @@ namespace pcl ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void - ScreenshotManager::writePose(const std::string &filename_pose, const Eigen::Vector3f &teVecs, const Eigen::Matrix &erreMats) + ScreenshotManager::writePose(const std::string &filename_pose, const Eigen::Vector3f &teVecs, const Eigen::Matrix &erreMats) const { std::ofstream poseFile; poseFile.open (filename_pose.c_str()); diff --git a/gpu/kinfu_large_scale/src/tsdf_volume.cpp b/gpu/kinfu_large_scale/src/tsdf_volume.cpp index 7cfc12e1..3c6de779 100644 --- a/gpu/kinfu_large_scale/src/tsdf_volume.cpp +++ b/gpu/kinfu_large_scale/src/tsdf_volume.cpp @@ -367,7 +367,6 @@ pcl::gpu::kinfuLS::TsdfVolume::convertToTsdfCloud ( pcl::PointCloudreserve (std::min (cloud_size/10, 500000)); int volume_idx = 0, cloud_idx = 0; - // #pragma omp parallel for // if used, increment over idx not possible! use index calculation for (int z = 0; z < sz; z+=step) for (int y = 0; y < sy; y+=step) for (int x = 0; x < sx; x+=step, ++cloud_idx) @@ -393,7 +392,9 @@ pcl::gpu::kinfuLS::TsdfVolume::downloadTsdf (std::vector& tsdf) const tsdf.resize (volume_.cols() * volume_.rows()); volume_.download(&tsdf[0], volume_.cols() * sizeof(int)); -#pragma omp parallel for +#pragma omp parallel for \ + default(none) \ + shared(tsdf) for(int i = 0; i < (int) tsdf.size(); ++i) { float tmp = reinterpret_cast(&tsdf[i])->x; @@ -418,7 +419,9 @@ pcl::gpu::kinfuLS::TsdfVolume::downloadTsdfAndWeights (std::vector& tsdf, weights.resize (volumeSize); volume_.download(&tsdf[0], volume_.cols() * sizeof(int)); - #pragma omp parallel for + #pragma omp parallel for \ + default(none) \ + shared(tsdf, weights) for(int i = 0; i < (int) tsdf.size(); ++i) { short2 elem = *reinterpret_cast(&tsdf[i]); diff --git a/gpu/kinfu_large_scale/tools/evaluation.h b/gpu/kinfu_large_scale/tools/evaluation.h index 0fd6537c..b4fa69a2 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 diff --git a/gpu/kinfu_large_scale/tools/kinfuLS_app.cpp b/gpu/kinfu_large_scale/tools/kinfuLS_app.cpp index 82cf19e8..350fd615 100644 --- a/gpu/kinfu_large_scale/tools/kinfuLS_app.cpp +++ b/gpu/kinfu_large_scale/tools/kinfuLS_app.cpp @@ -51,6 +51,7 @@ Work in progress: patch by Marco (AUG,19th 2012) #include #include +#include #include #include @@ -83,7 +84,7 @@ Work in progress: patch by Marco (AUG,19th 2012) #include "evaluation.h" #include -#include +#include #ifdef HAVE_OPENCV #include diff --git a/gpu/kinfu_large_scale/tools/kinfu_app_sim.cpp b/gpu/kinfu_large_scale/tools/kinfu_app_sim.cpp index f57aaeab..fa8ab915 100644 --- a/gpu/kinfu_large_scale/tools/kinfu_app_sim.cpp +++ b/gpu/kinfu_large_scale/tools/kinfu_app_sim.cpp @@ -84,7 +84,7 @@ #include "evaluation.h" #include -#include +#include //#include "tsdf_volume.h" //#include "tsdf_volume.hpp" @@ -99,7 +99,6 @@ using ScopeTimeT = pcl::ScopeTime; #include #include #include -#include #ifdef _WIN32 # define WIN32_LEAN_AND_MEAN # include @@ -110,8 +109,6 @@ using ScopeTimeT = pcl::ScopeTime; #include "pcl/common/common.h" #include "pcl/common/transforms.h" #include -// define the following in order to eliminate the deprecated headers warning -#define VTK_EXCLUDE_STRSTREAM_HEADERS #include // #include diff --git a/gpu/kinfu_large_scale/tools/record_maps_rgb.cpp b/gpu/kinfu_large_scale/tools/record_maps_rgb.cpp index 080ba8d1..483c38c9 100644 --- a/gpu/kinfu_large_scale/tools/record_maps_rgb.cpp +++ b/gpu/kinfu_large_scale/tools/record_maps_rgb.cpp @@ -34,7 +34,7 @@ #include #include -#include +#include #include #include #include //fps calculations diff --git a/gpu/kinfu_large_scale/tools/record_tsdfvolume.cpp b/gpu/kinfu_large_scale/tools/record_tsdfvolume.cpp index 731066c6..937fdef2 100644 --- a/gpu/kinfu_large_scale/tools/record_tsdfvolume.cpp +++ b/gpu/kinfu_large_scale/tools/record_tsdfvolume.cpp @@ -179,7 +179,9 @@ DeviceVolume::getVolume (pcl::TSDFVolume::Ptr &volume) device_volume_.download (&volume_vec[0], device_volume_.cols() * sizeof(int)); - #pragma omp parallel for + #pragma omp parallel for \ + default(none) \ + shared(volume, volume_vec, weights_vec) for(int i = 0; i < (int) volume->size(); ++i) { short2 *elem = (short2*)&volume_vec[i]; diff --git a/gpu/octree/include/pcl/gpu/octree/octree.hpp b/gpu/octree/include/pcl/gpu/octree/octree.hpp index a3305d75..fbb41809 100644 --- a/gpu/octree/include/pcl/gpu/octree/octree.hpp +++ b/gpu/octree/include/pcl/gpu/octree/octree.hpp @@ -39,6 +39,7 @@ #include +#include #include #include #include @@ -94,7 +95,7 @@ namespace pcl void build(); /** \brief Returns true if tree has been built */ - bool isBuilt(); + bool isBuilt() const; /** \brief Downloads Octree from GPU to search using CPU function. It use useful for single (not-batch) search */ void internalDownload(); diff --git a/gpu/octree/src/octree.cpp b/gpu/octree/src/octree.cpp index aa848851..f28ad10b 100644 --- a/gpu/octree/src/octree.cpp +++ b/gpu/octree/src/octree.cpp @@ -78,7 +78,6 @@ pcl::gpu::Octree::~Octree() { clear(); } void pcl::gpu::Octree::clear() { - if (impl) delete static_cast(impl); } @@ -95,7 +94,7 @@ void pcl::gpu::Octree::build() built_ = true; } -bool pcl::gpu::Octree::isBuilt() +bool pcl::gpu::Octree::isBuilt() const { return built_; } diff --git a/gpu/octree/test/data_source.hpp b/gpu/octree/test/data_source.hpp index 80d67cc8..31503620 100644 --- a/gpu/octree/test/data_source.hpp +++ b/gpu/octree/test/data_source.hpp @@ -34,6 +34,7 @@ * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com) */ + #ifndef _PCL_TEST_GPU_OCTREE_DATAGEN_ #define _PCL_TEST_GPU_OCTREE_DATAGEN_ @@ -48,15 +49,22 @@ EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(pcl::PointXYZ) #endif + +namespace pcl +{ + +namespace gpu +{ + struct DataGenerator { - using PointType = pcl::gpu::Octree::PointType; + using PointType = Octree::PointType; - std::size_t data_size; + std::size_t data_size; std::size_t tests_num; float cube_size; - float max_radius; + float max_radius; float shared_radius; @@ -74,43 +82,43 @@ struct DataGenerator } void operator()() - { + { srand (0); points.resize(data_size); for(std::size_t i = 0; i < data_size; ++i) - { - points[i].x = ((float)rand())/RAND_MAX * cube_size; - points[i].y = ((float)rand())/RAND_MAX * cube_size; + { + points[i].x = ((float)rand())/RAND_MAX * cube_size; + points[i].y = ((float)rand())/RAND_MAX * cube_size; points[i].z = ((float)rand())/RAND_MAX * cube_size; } - + queries.resize(tests_num); radiuses.resize(tests_num); for (std::size_t i = 0; i < tests_num; ++i) - { - queries[i].x = ((float)rand())/RAND_MAX * cube_size; - queries[i].y = ((float)rand())/RAND_MAX * cube_size; - queries[i].z = ((float)rand())/RAND_MAX * cube_size; - radiuses[i] = ((float)rand())/RAND_MAX * max_radius; - }; + { + queries[i].x = ((float)rand())/RAND_MAX * cube_size; + queries[i].y = ((float)rand())/RAND_MAX * cube_size; + queries[i].z = ((float)rand())/RAND_MAX * cube_size; + radiuses[i] = ((float)rand())/RAND_MAX * max_radius; + }; for(std::size_t i = 0; i < tests_num/2; ++i) indices.push_back(i*2); } void bruteForceSearch(bool log = false, float radius = -1.f) - { + { if (log) std::cout << "BruteForceSearch"; int value100 = std::min(tests_num, 50); - int step = tests_num/value100; + int step = tests_num/value100; bfresutls.resize(tests_num); for(std::size_t i = 0; i < tests_num; ++i) - { + { if (log && i % step == 0) { std::cout << "."; @@ -119,7 +127,7 @@ struct DataGenerator std::vector& curr_res = bfresutls[i]; curr_res.clear(); - + float query_radius = radius > 0 ? radius : radiuses[i]; const PointType& query = queries[i]; @@ -141,8 +149,8 @@ struct DataGenerator std::cout << "Done" << std::endl; } - void printParams() const - { + void printParams() const + { std::cout << "Points number = " << data_size << std::endl; std::cout << "Queries number = " << tests_num << std::endl; std::cout << "Cube size = " << cube_size << std::endl; @@ -152,8 +160,8 @@ struct DataGenerator template struct ConvPoint - { - Dst operator()(const PointType& src) const + { + Dst operator()(const PointType& src) const { Dst dst; dst.x = src.x; @@ -162,10 +170,10 @@ struct DataGenerator return dst; } }; - }; -#endif /* _PCL_TEST_GPU_OCTREE_DATAGEN_ */ - +} // namespace gpu +} // namespace pcl +#endif /* _PCL_TEST_GPU_OCTREE_DATAGEN_ */ diff --git a/gpu/people/include/pcl/gpu/people/bodyparts_detector.h b/gpu/people/include/pcl/gpu/people/bodyparts_detector.h index d1d7b6f2..ee560dc8 100644 --- a/gpu/people/include/pcl/gpu/people/bodyparts_detector.h +++ b/gpu/people/include/pcl/gpu/people/bodyparts_detector.h @@ -36,6 +36,7 @@ #pragma once +#include #include #include #include @@ -44,8 +45,7 @@ #include #include #include "pcl/gpu/people/person_attribs.h" -#include -#include + #include #include diff --git a/gpu/people/include/pcl/gpu/people/face_detector.h b/gpu/people/include/pcl/gpu/people/face_detector.h index 57321de6..31a53b7b 100644 --- a/gpu/people/include/pcl/gpu/people/face_detector.h +++ b/gpu/people/include/pcl/gpu/people/face_detector.h @@ -36,11 +36,12 @@ #pragma once +#include #include #include #include -#include + #include #include @@ -162,7 +163,7 @@ namespace pcl /** \brief Get the cuda GPU device id in use **/ int - getDeviceId() {return cuda_dev_id_;} + getDeviceId() const {return cuda_dev_id_;} private: bool largest_object_; /** \brief only give back largest object **/ diff --git a/gpu/people/include/pcl/gpu/people/organized_plane_detector.h b/gpu/people/include/pcl/gpu/people/organized_plane_detector.h index 9ae24ffb..e4be7d67 100644 --- a/gpu/people/include/pcl/gpu/people/organized_plane_detector.h +++ b/gpu/people/include/pcl/gpu/people/organized_plane_detector.h @@ -36,6 +36,7 @@ #pragma once +#include #include #include #include @@ -45,7 +46,6 @@ #include #include -#include #include #include diff --git a/gpu/people/include/pcl/gpu/people/person_attribs.h b/gpu/people/include/pcl/gpu/people/person_attribs.h index 6b81bb42..fd9201bd 100644 --- a/gpu/people/include/pcl/gpu/people/person_attribs.h +++ b/gpu/people/include/pcl/gpu/people/person_attribs.h @@ -3,7 +3,7 @@ #include #include #include -#include +#include #include diff --git a/gpu/people/src/cuda/nvidia/NCVHaarObjectDetection.hpp b/gpu/people/src/cuda/nvidia/NCVHaarObjectDetection.hpp index ca21e0e2..c1a1b69c 100644 --- a/gpu/people/src/cuda/nvidia/NCVHaarObjectDetection.hpp +++ b/gpu/people/src/cuda/nvidia/NCVHaarObjectDetection.hpp @@ -138,27 +138,27 @@ struct HaarFeatureDescriptor32 return NCV_SUCCESS; } - __device__ __host__ NcvBool isTilted() + __device__ __host__ NcvBool isTilted() const { return (this->desc & HaarFeatureDescriptor32_Interpret_MaskFlagTilted) != 0; } - __device__ __host__ NcvBool isLeftNodeLeaf() + __device__ __host__ NcvBool isLeftNodeLeaf() const { return (this->desc & HaarFeatureDescriptor32_Interpret_MaskFlagLeftNodeLeaf) != 0; } - __device__ __host__ NcvBool isRightNodeLeaf() + __device__ __host__ NcvBool isRightNodeLeaf() const { return (this->desc & HaarFeatureDescriptor32_Interpret_MaskFlagRightNodeLeaf) != 0; } - __device__ __host__ Ncv32u getNumFeatures() + __device__ __host__ Ncv32u getNumFeatures() const { return (this->desc >> HaarFeatureDescriptor32_NumFeatures_Shift) & HaarFeatureDescriptor32_CreateCheck_MaxNumFeatures; } - __device__ __host__ Ncv32u getFeaturesOffset() + __device__ __host__ Ncv32u getFeaturesOffset() const { return this->desc & HaarFeatureDescriptor32_CreateCheck_MaxFeatureOffset; } @@ -185,7 +185,7 @@ struct HaarClassifierNodeDescriptor32 return (*(Ncv32f *)&this->_ui1.x); } - __host__ bool isLeaf() // TODO: check this hack don't know if is correct + __host__ bool isLeaf() const // TODO: check this hack don't know if is correct { return ( _ui1.x != 0); } @@ -291,12 +291,12 @@ struct HaarStage64 return *(Ncv32f*)&this->_ui2.x; } - __host__ __device__ Ncv32u getStartClassifierRootNodeOffset() + __host__ __device__ Ncv32u getStartClassifierRootNodeOffset() const { return (this->_ui2.y >> HaarStage64_Interpret_ShiftRootNodeOffset); } - __host__ __device__ Ncv32u getNumClassifierRootNodes() + __host__ __device__ Ncv32u getNumClassifierRootNodes() const { return (this->_ui2.y & HaarStage64_Interpret_MaskRootNodes); } diff --git a/gpu/people/src/cuda/nvidia/NPP_staging.hpp b/gpu/people/src/cuda/nvidia/NPP_staging.hpp index dfc0c219..c42deddd 100644 --- a/gpu/people/src/cuda/nvidia/NPP_staging.hpp +++ b/gpu/people/src/cuda/nvidia/NPP_staging.hpp @@ -75,7 +75,7 @@ NCV_EXPORTS cudaStream_t nppStSetActiveCUDAstream(cudaStream_t cudaStream); -/*@}*/ +/**@}*/ /** \defgroup nppi NPPST Image Processing @@ -131,7 +131,7 @@ enum NppStInterpMode /** Size of a buffer required for interpolation. - * + * * Requires several such buffers. See \see NppStInterpolationState. * * \param srcSize [IN] Frame size (both frames must be of the same size) @@ -177,17 +177,17 @@ NCVStatus nppiStInterpolateFrames(const NppStInterpolationState *pState); * \return NCV status code */ NCV_EXPORTS -NCVStatus nppiStFilterRowBorder_32f_C1R(const Ncv32f *pSrc, - NcvSize32u srcSize, +NCVStatus nppiStFilterRowBorder_32f_C1R(const Ncv32f *pSrc, + NcvSize32u srcSize, Ncv32u nSrcStep, - Ncv32f *pDst, - NcvSize32u dstSize, + Ncv32f *pDst, + NcvSize32u dstSize, Ncv32u nDstStep, - NcvRect32u oROI, + NcvRect32u oROI, NppStBorderType borderType, - const Ncv32f *pKernel, + const Ncv32f *pKernel, Ncv32s nKernelSize, - Ncv32s nAnchor, + Ncv32s nAnchor, Ncv32f multiplier); @@ -226,14 +226,14 @@ NCVStatus nppiStFilterColumnBorder_32f_C1R(const Ncv32f *pSrc, /** Size of buffer required for vector image warping. - * + * * \param srcSize [IN] Source image size * \param nSrcStep [IN] Source image line step * \param hpSize [OUT] Where to store computed size (host memory) * * \return NCV status code */ -NCV_EXPORTS +NCV_EXPORTS NCVStatus nppiStVectorWarpGetBufferSize(NcvSize32u srcSize, Ncv32u nSrcStep, Ncv32u *hpSize); @@ -318,7 +318,7 @@ NCVStatus nppiStVectorWarp_PSF2x2_32f_C1(const Ncv32f *pSrc, * \param xFactor [IN] Row scale factor * \param yFactor [IN] Column scale factor * \param interpolation [IN] Interpolation type - * + * * \return NCV status code */ NCV_EXPORTS @@ -790,7 +790,7 @@ NCVStatus nppiStSqrIntegral_8u64u_C1R_host(Ncv8u *h_src, Ncv32u srcStep, Ncv64u *h_dst, Ncv32u dstStep, NcvSize32u roiSize); -/*@}*/ +/**@}*/ /** \defgroup npps NPPST Signal Processing @@ -902,7 +902,7 @@ NCVStatus nppsStCompact_32f_host(Ncv32f *h_src, Ncv32u srcLen, Ncv32f *h_dst, Ncv32u *dstLen, Ncv32f elemRemove); -/*@}*/ +/**@}*/ #endif // _npp_staging_hpp_ diff --git a/gpu/people/src/cuda/shs.cu b/gpu/people/src/cuda/shs.cu index 1dfae5ac..aa801de2 100644 --- a/gpu/people/src/cuda/shs.cu +++ b/gpu/people/src/cuda/shs.cu @@ -178,7 +178,6 @@ void optimized_shs5(const PointCloud &cloud, float tolerance, const // omp_set_num_threads(1); // Process all points in the indices vector - //#pragma omp parallel for for (int k = 0; k < static_cast (indices_in.indices.size ()); ++k) { int i = indices_in.indices[k]; diff --git a/gpu/people/src/face_detector.cpp b/gpu/people/src/face_detector.cpp index 39e13733..711d9e33 100644 --- a/gpu/people/src/face_detector.cpp +++ b/gpu/people/src/face_detector.cpp @@ -102,7 +102,7 @@ pcl::gpu::people::FaceDetector::loadFromXML2(const std::string { read_xml(filename,pt); } - catch(boost::exception const& exb) + catch(boost::exception const&) { PCL_DEBUG("[pcl::gpu::people::FaceDetector::loadFromXML2] : (D) : Unable to read filename with boost exception\n"); return NCV_HAAR_XML_LOADING_EXCEPTION; @@ -301,7 +301,7 @@ pcl::gpu::people::FaceDetector::loadFromXML2(const std::string level1++; } } - catch(boost::exception const& exb) + catch(boost::exception const&) { PCL_DEBUG("[pcl::gpu::people::FaceDetector::loadFromXML2] : (D) : Unable to process content with boost exception\n"); return (NCV_HAAR_XML_LOADING_EXCEPTION); diff --git a/gpu/people/src/people_detector.cpp b/gpu/people/src/people_detector.cpp index 7f07745d..cd284138 100644 --- a/gpu/people/src/people_detector.cpp +++ b/gpu/people/src/people_detector.cpp @@ -486,9 +486,9 @@ pcl::gpu::people::PeopleDetector::shs5(const pcl::PointCloud &cloud, con // Process all points in the indices vector int total = static_cast (indices.size ()); -#ifdef _OPENMP -#pragma omp parallel for -#endif +#pragma omp parallel for \ + default(none) \ + shared(cloud, intr, hue, indices, mask, squared_radius, storage, total) for (int k = 0; k < total; ++k) { int i = indices[k]; diff --git a/gpu/people/src/trees.cpp b/gpu/people/src/trees.cpp index b17c1d33..616983d7 100644 --- a/gpu/people/src/trees.cpp +++ b/gpu/people/src/trees.cpp @@ -65,7 +65,7 @@ namespace pcl { Tex2Dfetcher( const std::uint16_t* dmap, int W, int H ) : m_dmap(dmap), m_W(W), m_H(H) {} - inline std::uint16_t operator () ( float uf, float vf ) + inline std::uint16_t operator () ( float uf, float vf ) const { int u = static_cast(uf); int v = static_cast(vf); diff --git a/gpu/people/tools/people_pcd_prob.cpp b/gpu/people/tools/people_pcd_prob.cpp index 697363a0..845eec3e 100644 --- a/gpu/people/tools/people_pcd_prob.cpp +++ b/gpu/people/tools/people_pcd_prob.cpp @@ -139,7 +139,7 @@ class PeoplePCDApp } void - writeXMLFile(std::string& filename) + writeXMLFile(std::string& filename) const { filebuf fb; fb.open (filename.c_str(), ios::out); @@ -149,7 +149,7 @@ class PeoplePCDApp } void - readXMLFile(std::string& filename) + readXMLFile(std::string& filename) const { filebuf fb; fb.open (filename.c_str(), ios::in); diff --git a/gpu/people/tools/people_tracking.cpp b/gpu/people/tools/people_tracking.cpp index 70a58ad3..5b6263e7 100644 --- a/gpu/people/tools/people_tracking.cpp +++ b/gpu/people/tools/people_tracking.cpp @@ -72,12 +72,27 @@ class PeopleTrackingApp interface->stop (); } - pcl::visualization::CloudViewer viewer; - pcl::people::trees::MultiTreeLiveProc* m_proc; - cv::Mat m_lmap; - cv::Mat m_cmap; - cv::Mat cmap; - cv::Mat m_bmap; + void load_tree(std::string treeFilenames[4], int numTrees) + { + std::ifstream fin0 (treeFilenames[0]); + assert(fin0.is_open()); + m_proc = std::make_unique (fin0); + + /// Load the other tree files + for (const auto& file : treeFilenames) + { + std::ifstream fin (file); + assert (fin.is_open()); + m_proc->addTree(fin); + } + } + + pcl::visualization::CloudViewer viewer; + std::unique_ptr m_proc; + cv::Mat m_lmap; + cv::Mat m_cmap; + cv::Mat cmap; + cv::Mat m_bmap; }; int print_help() @@ -111,18 +126,8 @@ int main(int argc, char** argv) PeopleTrackingApp app; /// Load the first tree - std::ifstream fin0(treeFilenames[0].c_str() ); - assert(fin0.is_open() ); - app.m_proc = new pcl::people::trees::MultiTreeLiveProc(fin0); - fin0.close(); - - /// Load the other tree files - for(int ti=1;tiaddTree(fin); - fin.close(); - } + app.load_tree(treeFilenames, numTrees); + /// Run the app app.run(); return 0; diff --git a/gpu/surface/src/convex_hull.cpp b/gpu/surface/src/convex_hull.cpp index 522a2d73..7c4410f6 100644 --- a/gpu/surface/src/convex_hull.cpp +++ b/gpu/surface/src/convex_hull.cpp @@ -59,7 +59,7 @@ pcl::device::FacetStream::FacetStream(std::size_t buffer_size) } bool -pcl::device::FacetStream::canSplit() +pcl::device::FacetStream::canSplit() const { return facet_count * 3 < verts_inds.cols(); } diff --git a/gpu/surface/src/internal.h b/gpu/surface/src/internal.h index 783c3c2b..47c76fba 100644 --- a/gpu/surface/src/internal.h +++ b/gpu/surface/src/internal.h @@ -80,7 +80,7 @@ namespace pcl void compactFacets(); - bool canSplit(); + bool canSplit() const; void splitFacets(); private: diff --git a/gpu/utils/include/pcl/gpu/utils/timers_cuda.hpp b/gpu/utils/include/pcl/gpu/utils/timers_cuda.hpp index ca33ac0f..e2444ca7 100644 --- a/gpu/utils/include/pcl/gpu/utils/timers_cuda.hpp +++ b/gpu/utils/include/pcl/gpu/utils/timers_cuda.hpp @@ -60,10 +60,10 @@ namespace pcl cudaEventDestroy(stop_); } - void start() { cudaEventRecord(start_, 0); } + void start() const { cudaEventRecord(start_, 0); } Timer& stop() { cudaEventRecord(stop_, 0); cudaEventSynchronize(stop_); return *this; } - float time() + float time() const { float elapsed_time; cudaEventElapsedTime(&elapsed_time, start_, stop_); diff --git a/io/CMakeLists.txt b/io/CMakeLists.txt index 1741230d..f2349710 100644 --- a/io/CMakeLists.txt +++ b/io/CMakeLists.txt @@ -288,7 +288,6 @@ set(incs ${DSSDK_GRABBER_INCLUDES} ${RSSDK_GRABBER_INCLUDES} ${RSSDK2_GRABBER_INCLUDES} - "include/pcl/${SUBSYS_NAME}/pxc_grabber.h" # contains only depreciation note ) set(compression_incs diff --git a/io/include/pcl/compression/color_coding.h b/io/include/pcl/compression/color_coding.h index 5ab9333e..52b4a608 100644 --- a/io/include/pcl/compression/color_coding.h +++ b/io/include/pcl/compression/color_coding.h @@ -45,357 +45,353 @@ namespace pcl { - namespace octree + +namespace octree +{ + +using namespace std; + +/** \brief @b ColorCoding class + * \note This class encodes 8-bit color information for octree-based point cloud compression. + * \note + * \note typename: PointT: type of point used in pointcloud + * \author Julius Kammerl (julius@kammerl.de) + */ +template +class ColorCoding +{ + // public typedefs + using PointCloud = pcl::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + +public: + /** \brief Constructor. + * + * */ + ColorCoding () : + output_ (), colorBitReduction_ (0) + { + } + + /** \brief Empty class constructor. */ + virtual + ~ColorCoding () + { + } + + /** \brief Define color bit depth of encoded color information. + * \param bitDepth_arg: amounts of bits for representing one color component + */ + inline + void + setBitDepth (unsigned char bitDepth_arg) + { + colorBitReduction_ = static_cast (8 - bitDepth_arg); + } + + /** \brief Retrieve color bit depth of encoded color information. + * \return amounts of bits for representing one color component + */ + inline unsigned char + getBitDepth () + { + return (static_cast (8 - colorBitReduction_)); + } + + /** \brief Set amount of voxels containing point color information and reserve memory + * \param voxelCount_arg: amounts of voxels + */ + inline void + setVoxelCount (unsigned int voxelCount_arg) + { + pointAvgColorDataVector_.reserve (voxelCount_arg * 3); + } + + /** \brief Set amount of points within point cloud to be encoded and reserve memory + * \param pointCount_arg: amounts of points within point cloud + * */ + inline + void + setPointCount (unsigned int pointCount_arg) + { + pointDiffColorDataVector_.reserve (pointCount_arg * 3); + } + + /** \brief Initialize encoding of color information + * */ + void + initializeEncoding () + { + pointAvgColorDataVector_.clear (); + + pointDiffColorDataVector_.clear (); + } + + /** \brief Initialize decoding of color information + * */ + void + initializeDecoding () + { + pointAvgColorDataVector_Iterator_ = pointAvgColorDataVector_.begin (); + + pointDiffColorDataVector_Iterator_ = pointDiffColorDataVector_.begin (); + } + + /** \brief Get reference to vector containing averaged color data + * */ + std::vector& + getAverageDataVector () + { + return pointAvgColorDataVector_; + } + + /** \brief Get reference to vector containing differential color data + * */ + std::vector& + getDifferentialDataVector () { - using namespace std; - - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - /** \brief @b ColorCoding class - * \note This class encodes 8-bit color information for octree-based point cloud compression. - * \note - * \note typename: PointT: type of point used in pointcloud - * \author Julius Kammerl (julius@kammerl.de) - */ - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - template - class ColorCoding + return pointDiffColorDataVector_; + } + + /** \brief Encode averaged color information for a subset of points from point cloud + * \param indexVector_arg indices defining a subset of points from points cloud + * \param rgba_offset_arg offset to color information + * \param inputCloud_arg input point cloud + * */ + void + encodeAverageOfPoints (const typename std::vector& indexVector_arg, unsigned char rgba_offset_arg, PointCloudConstPtr inputCloud_arg) + { + unsigned int avgRed = 0; + unsigned int avgGreen = 0; + unsigned int avgBlue = 0; + + // iterate over points + std::size_t len = indexVector_arg.size (); + for (std::size_t i = 0; i < len; i++) + { + // get color information from points + const int& idx = indexVector_arg[i]; + const char* idxPointPtr = reinterpret_cast (&inputCloud_arg->points[idx]); + const int& colorInt = *reinterpret_cast (idxPointPtr+rgba_offset_arg); + + // add color information + avgRed += (colorInt >> 0) & 0xFF; + avgGreen += (colorInt >> 8) & 0xFF; + avgBlue += (colorInt >> 16) & 0xFF; + + } + + // calculated average color information + if (len > 1) + { + avgRed /= static_cast (len); + avgGreen /= static_cast (len); + avgBlue /= static_cast (len); + } + + // remove least significant bits + avgRed >>= colorBitReduction_; + avgGreen >>= colorBitReduction_; + avgBlue >>= colorBitReduction_; + + // add to average color vector + pointAvgColorDataVector_.push_back (static_cast (avgRed)); + pointAvgColorDataVector_.push_back (static_cast (avgGreen)); + pointAvgColorDataVector_.push_back (static_cast (avgBlue)); + } + + /** \brief Encode color information of a subset of points from point cloud + * \param indexVector_arg indices defining a subset of points from points cloud + * \param rgba_offset_arg offset to color information + * \param inputCloud_arg input point cloud + * */ + void + encodePoints (const typename std::vector& indexVector_arg, unsigned char rgba_offset_arg, PointCloudConstPtr inputCloud_arg) + { + unsigned int avgRed; + unsigned int avgGreen; + unsigned int avgBlue; + + // initialize + avgRed = avgGreen = avgBlue = 0; + + // iterate over points + std::size_t len = indexVector_arg.size (); + for (std::size_t i = 0; i < len; i++) + { + // get color information from point + const int& idx = indexVector_arg[i]; + const char* idxPointPtr = reinterpret_cast (&inputCloud_arg->points[idx]); + const int& colorInt = *reinterpret_cast (idxPointPtr+rgba_offset_arg); + + // add color information + avgRed += (colorInt >> 0) & 0xFF; + avgGreen += (colorInt >> 8) & 0xFF; + avgBlue += (colorInt >> 16) & 0xFF; + + } + + if (len > 1) + { + unsigned char diffRed; + unsigned char diffGreen; + unsigned char diffBlue; + + // calculated average color information + avgRed /= static_cast (len); + avgGreen /= static_cast (len); + avgBlue /= static_cast (len); + + // iterate over points for differential encoding + for (std::size_t i = 0; i < len; i++) { + const int& idx = indexVector_arg[i]; + const char* idxPointPtr = reinterpret_cast (&inputCloud_arg->points[idx]); + const int& colorInt = *reinterpret_cast (idxPointPtr+rgba_offset_arg); + + // extract color components and do XOR encoding with predicted average color + diffRed = (static_cast (avgRed)) ^ static_cast (((colorInt >> 0) & 0xFF)); + diffGreen = (static_cast (avgGreen)) ^ static_cast (((colorInt >> 8) & 0xFF)); + diffBlue = (static_cast (avgBlue)) ^ static_cast (((colorInt >> 16) & 0xFF)); + + // remove least significant bits + diffRed = static_cast (diffRed >> colorBitReduction_); + diffGreen = static_cast (diffGreen >> colorBitReduction_); + diffBlue = static_cast (diffBlue >> colorBitReduction_); + + // add to differential color vector + pointDiffColorDataVector_.push_back (static_cast (diffRed)); + pointDiffColorDataVector_.push_back (static_cast (diffGreen)); + pointDiffColorDataVector_.push_back (static_cast (diffBlue)); + } + } + + // remove least significant bits from average color information + avgRed >>= colorBitReduction_; + avgGreen >>= colorBitReduction_; + avgBlue >>= colorBitReduction_; + + // add to differential color vector + pointAvgColorDataVector_.push_back (static_cast (avgRed)); + pointAvgColorDataVector_.push_back (static_cast (avgGreen)); + pointAvgColorDataVector_.push_back (static_cast (avgBlue)); - // public typedefs - using PointCloud = pcl::PointCloud; - using PointCloudPtr = typename PointCloud::Ptr; - using PointCloudConstPtr = typename PointCloud::ConstPtr; - - public: - - /** \brief Constructor. - * - * */ - ColorCoding () : - output_ (), colorBitReduction_ (0) - { - } - - /** \brief Empty class constructor. */ - virtual - ~ColorCoding () - { - } - - /** \brief Define color bit depth of encoded color information. - * \param bitDepth_arg: amounts of bits for representing one color component - */ - inline - void - setBitDepth (unsigned char bitDepth_arg) - { - colorBitReduction_ = static_cast (8 - bitDepth_arg); - } - - /** \brief Retrieve color bit depth of encoded color information. - * \return amounts of bits for representing one color component - */ - inline unsigned char - getBitDepth () - { - return (static_cast (8 - colorBitReduction_)); - } - - /** \brief Set amount of voxels containing point color information and reserve memory - * \param voxelCount_arg: amounts of voxels - */ - inline void - setVoxelCount (unsigned int voxelCount_arg) - { - pointAvgColorDataVector_.reserve (voxelCount_arg * 3); - } - - /** \brief Set amount of points within point cloud to be encoded and reserve memory - * \param pointCount_arg: amounts of points within point cloud - * */ - inline - void - setPointCount (unsigned int pointCount_arg) - { - pointDiffColorDataVector_.reserve (pointCount_arg * 3); - } - - /** \brief Initialize encoding of color information - * */ - void - initializeEncoding () - { - pointAvgColorDataVector_.clear (); - - pointDiffColorDataVector_.clear (); - } - - /** \brief Initialize decoding of color information - * */ - void - initializeDecoding () - { - pointAvgColorDataVector_Iterator_ = pointAvgColorDataVector_.begin (); - - pointDiffColorDataVector_Iterator_ = pointDiffColorDataVector_.begin (); - } - - /** \brief Get reference to vector containing averaged color data - * */ - std::vector& - getAverageDataVector () - { - return pointAvgColorDataVector_; - } - - /** \brief Get reference to vector containing differential color data - * */ - std::vector& - getDifferentialDataVector () - { - return pointDiffColorDataVector_; - } - - /** \brief Encode averaged color information for a subset of points from point cloud - * \param indexVector_arg indices defining a subset of points from points cloud - * \param rgba_offset_arg offset to color information - * \param inputCloud_arg input point cloud - * */ - void - encodeAverageOfPoints (const typename std::vector& indexVector_arg, unsigned char rgba_offset_arg, PointCloudConstPtr inputCloud_arg) - { - unsigned int avgRed = 0; - unsigned int avgGreen = 0; - unsigned int avgBlue = 0; - - // iterate over points - std::size_t len = indexVector_arg.size (); - for (std::size_t i = 0; i < len; i++) - { - // get color information from points - const int& idx = indexVector_arg[i]; - const char* idxPointPtr = reinterpret_cast (&inputCloud_arg->points[idx]); - const int& colorInt = *reinterpret_cast (idxPointPtr+rgba_offset_arg); - - // add color information - avgRed += (colorInt >> 0) & 0xFF; - avgGreen += (colorInt >> 8) & 0xFF; - avgBlue += (colorInt >> 16) & 0xFF; - - } - - // calculated average color information - if (len > 1) - { - avgRed /= static_cast (len); - avgGreen /= static_cast (len); - avgBlue /= static_cast (len); - } - - // remove least significant bits - avgRed >>= colorBitReduction_; - avgGreen >>= colorBitReduction_; - avgBlue >>= colorBitReduction_; - - // add to average color vector - pointAvgColorDataVector_.push_back (static_cast (avgRed)); - pointAvgColorDataVector_.push_back (static_cast (avgGreen)); - pointAvgColorDataVector_.push_back (static_cast (avgBlue)); - } - - /** \brief Encode color information of a subset of points from point cloud - * \param indexVector_arg indices defining a subset of points from points cloud - * \param rgba_offset_arg offset to color information - * \param inputCloud_arg input point cloud - * */ - void - encodePoints (const typename std::vector& indexVector_arg, unsigned char rgba_offset_arg, PointCloudConstPtr inputCloud_arg) - { - unsigned int avgRed; - unsigned int avgGreen; - unsigned int avgBlue; - - // initialize - avgRed = avgGreen = avgBlue = 0; - - // iterate over points - std::size_t len = indexVector_arg.size (); - for (std::size_t i = 0; i < len; i++) - { - // get color information from point - const int& idx = indexVector_arg[i]; - const char* idxPointPtr = reinterpret_cast (&inputCloud_arg->points[idx]); - const int& colorInt = *reinterpret_cast (idxPointPtr+rgba_offset_arg); - - // add color information - avgRed += (colorInt >> 0) & 0xFF; - avgGreen += (colorInt >> 8) & 0xFF; - avgBlue += (colorInt >> 16) & 0xFF; - - } - - if (len > 1) - { - unsigned char diffRed; - unsigned char diffGreen; - unsigned char diffBlue; - - // calculated average color information - avgRed /= static_cast (len); - avgGreen /= static_cast (len); - avgBlue /= static_cast (len); - - // iterate over points for differential encoding - for (std::size_t i = 0; i < len; i++) - { - const int& idx = indexVector_arg[i]; - const char* idxPointPtr = reinterpret_cast (&inputCloud_arg->points[idx]); - const int& colorInt = *reinterpret_cast (idxPointPtr+rgba_offset_arg); - - // extract color components and do XOR encoding with predicted average color - diffRed = (static_cast (avgRed)) ^ static_cast (((colorInt >> 0) & 0xFF)); - diffGreen = (static_cast (avgGreen)) ^ static_cast (((colorInt >> 8) & 0xFF)); - diffBlue = (static_cast (avgBlue)) ^ static_cast (((colorInt >> 16) & 0xFF)); - - // remove least significant bits - diffRed = static_cast (diffRed >> colorBitReduction_); - diffGreen = static_cast (diffGreen >> colorBitReduction_); - diffBlue = static_cast (diffBlue >> colorBitReduction_); - - // add to differential color vector - pointDiffColorDataVector_.push_back (static_cast (diffRed)); - pointDiffColorDataVector_.push_back (static_cast (diffGreen)); - pointDiffColorDataVector_.push_back (static_cast (diffBlue)); - } - } - - // remove least significant bits from average color information - avgRed >>= colorBitReduction_; - avgGreen >>= colorBitReduction_; - avgBlue >>= colorBitReduction_; - - // add to differential color vector - pointAvgColorDataVector_.push_back (static_cast (avgRed)); - pointAvgColorDataVector_.push_back (static_cast (avgGreen)); - pointAvgColorDataVector_.push_back (static_cast (avgBlue)); - - } - - /** \brief Decode color information - * \param outputCloud_arg output point cloud - * \param beginIdx_arg index indicating first point to be assigned with color information - * \param endIdx_arg index indicating last point to be assigned with color information - * \param rgba_offset_arg offset to color information - */ - void - decodePoints (PointCloudPtr outputCloud_arg, std::size_t beginIdx_arg, std::size_t endIdx_arg, unsigned char rgba_offset_arg) - { - assert (beginIdx_arg <= endIdx_arg); - - // amount of points to be decoded - unsigned int pointCount = static_cast (endIdx_arg - beginIdx_arg); - - // get averaged color information for current voxel - unsigned char avgRed = *(pointAvgColorDataVector_Iterator_++); - unsigned char avgGreen = *(pointAvgColorDataVector_Iterator_++); - unsigned char avgBlue = *(pointAvgColorDataVector_Iterator_++); - - // invert bit shifts during encoding - avgRed = static_cast (avgRed << colorBitReduction_); - avgGreen = static_cast (avgGreen << colorBitReduction_); - avgBlue = static_cast (avgBlue << colorBitReduction_); - - // iterate over points - for (std::size_t i = 0; i < pointCount; i++) - { - unsigned int colorInt; - if (pointCount > 1) - { - // get differential color information from input vector - unsigned char diffRed = static_cast (*(pointDiffColorDataVector_Iterator_++)); - unsigned char diffGreen = static_cast (*(pointDiffColorDataVector_Iterator_++)); - unsigned char diffBlue = static_cast (*(pointDiffColorDataVector_Iterator_++)); - - // invert bit shifts during encoding - diffRed = static_cast (diffRed << colorBitReduction_); - diffGreen = static_cast (diffGreen << colorBitReduction_); - diffBlue = static_cast (diffBlue << colorBitReduction_); - - // decode color information - colorInt = ((avgRed ^ diffRed) << 0) | - ((avgGreen ^ diffGreen) << 8) | - ((avgBlue ^ diffBlue) << 16); - } - else - { - // decode color information - colorInt = (avgRed << 0) | (avgGreen << 8) | (avgBlue << 16); - } - - char* idxPointPtr = reinterpret_cast (&outputCloud_arg->points[beginIdx_arg + i]); - int& pointColor = *reinterpret_cast (idxPointPtr+rgba_offset_arg); - // assign color to point from point cloud - pointColor=colorInt; - } - } - - /** \brief Set default color to points - * \param outputCloud_arg output point cloud - * \param beginIdx_arg index indicating first point to be assigned with color information - * \param endIdx_arg index indicating last point to be assigned with color information - * \param rgba_offset_arg offset to color information - * */ - void - setDefaultColor (PointCloudPtr outputCloud_arg, std::size_t beginIdx_arg, std::size_t endIdx_arg, unsigned char rgba_offset_arg) - { - assert (beginIdx_arg <= endIdx_arg); - - // amount of points to be decoded - unsigned int pointCount = static_cast (endIdx_arg - beginIdx_arg); - - // iterate over points - for (std::size_t i = 0; i < pointCount; i++) - { - char* idxPointPtr = reinterpret_cast (&outputCloud_arg->points[beginIdx_arg + i]); - int& pointColor = *reinterpret_cast (idxPointPtr+rgba_offset_arg); - // assign color to point from point cloud - pointColor = defaultColor_; - } - } - - - protected: - - /** \brief Pointer to output point cloud dataset. */ - PointCloudPtr output_; - - /** \brief Vector for storing average color information */ - std::vector pointAvgColorDataVector_; - - /** \brief Iterator on average color information vector */ - std::vector::const_iterator pointAvgColorDataVector_Iterator_; - - /** \brief Vector for storing differential color information */ - std::vector pointDiffColorDataVector_; - - /** \brief Iterator on differential color information vector */ - std::vector::const_iterator pointDiffColorDataVector_Iterator_; - - /** \brief Amount of bits to be removed from color components before encoding */ - unsigned char colorBitReduction_; - - // frame header identifier - static const int defaultColor_; - - }; - - // define default color - template - const int ColorCoding::defaultColor_ = ((255) << 0) | - ((255) << 8) | - ((255) << 16); + } + /** \brief Decode color information + * \param outputCloud_arg output point cloud + * \param beginIdx_arg index indicating first point to be assigned with color information + * \param endIdx_arg index indicating last point to be assigned with color information + * \param rgba_offset_arg offset to color information + */ + void + decodePoints (PointCloudPtr outputCloud_arg, std::size_t beginIdx_arg, std::size_t endIdx_arg, unsigned char rgba_offset_arg) + { + assert (beginIdx_arg <= endIdx_arg); + + // amount of points to be decoded + unsigned int pointCount = static_cast (endIdx_arg - beginIdx_arg); + + // get averaged color information for current voxel + unsigned char avgRed = *(pointAvgColorDataVector_Iterator_++); + unsigned char avgGreen = *(pointAvgColorDataVector_Iterator_++); + unsigned char avgBlue = *(pointAvgColorDataVector_Iterator_++); + + // invert bit shifts during encoding + avgRed = static_cast (avgRed << colorBitReduction_); + avgGreen = static_cast (avgGreen << colorBitReduction_); + avgBlue = static_cast (avgBlue << colorBitReduction_); + + // iterate over points + for (std::size_t i = 0; i < pointCount; i++) + { + unsigned int colorInt; + if (pointCount > 1) + { + // get differential color information from input vector + unsigned char diffRed = static_cast (*(pointDiffColorDataVector_Iterator_++)); + unsigned char diffGreen = static_cast (*(pointDiffColorDataVector_Iterator_++)); + unsigned char diffBlue = static_cast (*(pointDiffColorDataVector_Iterator_++)); + + // invert bit shifts during encoding + diffRed = static_cast (diffRed << colorBitReduction_); + diffGreen = static_cast (diffGreen << colorBitReduction_); + diffBlue = static_cast (diffBlue << colorBitReduction_); + + // decode color information + colorInt = ((avgRed ^ diffRed) << 0) | + ((avgGreen ^ diffGreen) << 8) | + ((avgBlue ^ diffBlue) << 16); + } + else + { + // decode color information + colorInt = (avgRed << 0) | (avgGreen << 8) | (avgBlue << 16); + } + + char* idxPointPtr = reinterpret_cast (&outputCloud_arg->points[beginIdx_arg + i]); + int& pointColor = *reinterpret_cast (idxPointPtr+rgba_offset_arg); + // assign color to point from point cloud + pointColor=colorInt; + } + } + + /** \brief Set default color to points + * \param outputCloud_arg output point cloud + * \param beginIdx_arg index indicating first point to be assigned with color information + * \param endIdx_arg index indicating last point to be assigned with color information + * \param rgba_offset_arg offset to color information + * */ + void + setDefaultColor (PointCloudPtr outputCloud_arg, std::size_t beginIdx_arg, std::size_t endIdx_arg, unsigned char rgba_offset_arg) + { + assert (beginIdx_arg <= endIdx_arg); + + // amount of points to be decoded + unsigned int pointCount = static_cast (endIdx_arg - beginIdx_arg); + + // iterate over points + for (std::size_t i = 0; i < pointCount; i++) + { + char* idxPointPtr = reinterpret_cast (&outputCloud_arg->points[beginIdx_arg + i]); + int& pointColor = *reinterpret_cast (idxPointPtr+rgba_offset_arg); + // assign color to point from point cloud + pointColor = defaultColor_; + } } -} + +protected: + /** \brief Pointer to output point cloud dataset. */ + PointCloudPtr output_; + + /** \brief Vector for storing average color information */ + std::vector pointAvgColorDataVector_; + + /** \brief Iterator on average color information vector */ + std::vector::const_iterator pointAvgColorDataVector_Iterator_; + + /** \brief Vector for storing differential color information */ + std::vector pointDiffColorDataVector_; + + /** \brief Iterator on differential color information vector */ + std::vector::const_iterator pointDiffColorDataVector_Iterator_; + + /** \brief Amount of bits to be removed from color components before encoding */ + unsigned char colorBitReduction_; + + // frame header identifier + static const int defaultColor_; +}; + +// define default color +template +const int ColorCoding::defaultColor_ = ((255) << 0) | + ((255) << 8) | + ((255) << 16); + +} // namespace octree +} // namespace pcl #define PCL_INSTANTIATE_ColorCoding(T) template class PCL_EXPORTS pcl::octree::ColorCoding; + diff --git a/io/include/pcl/compression/entropy_range_coder.h b/io/include/pcl/compression/entropy_range_coder.h index 34adb11f..74bfcfe5 100644 --- a/io/include/pcl/compression/entropy_range_coder.h +++ b/io/include/pcl/compression/entropy_range_coder.h @@ -170,7 +170,7 @@ namespace pcl * \param n_arg: some value * \return binary logarithm (log2) of argument n_arg */ - PCL_DEPRECATED("use std::log2 instead") + PCL_DEPRECATED(1, 12, "use std::log2 instead") inline double Log2 (double n_arg) { diff --git a/io/include/pcl/compression/organized_pointcloud_conversion.h b/io/include/pcl/compression/organized_pointcloud_conversion.h index 541d7253..70330e1b 100644 --- a/io/include/pcl/compression/organized_pointcloud_conversion.h +++ b/io/include/pcl/compression/organized_pointcloud_conversion.h @@ -41,6 +41,7 @@ #include #include +#include // for pcl::isFinite #include #include @@ -48,522 +49,510 @@ namespace pcl { - namespace io - { +namespace io +{ - template struct CompressionPointTraits - { - static const bool hasColor = false; - static const unsigned int channels = 1; - static const std::size_t bytesPerPoint = 3 * sizeof(float); - }; +template struct CompressionPointTraits +{ + static const bool hasColor = false; + static const unsigned int channels = 1; + static const std::size_t bytesPerPoint = 3 * sizeof(float); +}; - template<> - struct CompressionPointTraits - { - static const bool hasColor = true; - static const unsigned int channels = 4; - static const std::size_t bytesPerPoint = 3 * sizeof(float) + 3 * sizeof(std::uint8_t); - }; +template<> +struct CompressionPointTraits +{ + static const bool hasColor = true; + static const unsigned int channels = 4; + static const std::size_t bytesPerPoint = 3 * sizeof(float) + 3 * sizeof(std::uint8_t); +}; - template<> - struct CompressionPointTraits - { - static const bool hasColor = true; - static const unsigned int channels = 4; - static const std::size_t bytesPerPoint = 3 * sizeof(float) + 3 * sizeof(std::uint8_t); - }; - - ////////////////////////////////////////////////////////////////////////////////////////////// - template ::hasColor > - struct OrganizedConversion; - - ////////////////////////////////////////////////////////////////////////////////////////////// - // Uncolored point cloud specialization - ////////////////////////////////////////////////////////////////////////////////////////////// - template - struct OrganizedConversion - { - /** \brief Convert point cloud to disparity image - * \param[in] cloud_arg input point cloud - * \param[in] focalLength_arg focal length - * \param[in] disparityShift_arg disparity shift - * \param[in] disparityScale_arg disparity scaling - * \param[out] disparityData_arg output disparity image - * \ingroup io - */ - static void convert(const pcl::PointCloud& cloud_arg, - float focalLength_arg, - float disparityShift_arg, - float disparityScale_arg, - bool , - typename std::vector& disparityData_arg, - typename std::vector&) - { - std::size_t cloud_size = cloud_arg.points.size (); +template<> +struct CompressionPointTraits +{ + static const bool hasColor = true; + static const unsigned int channels = 4; + static const std::size_t bytesPerPoint = 3 * sizeof(float) + 3 * sizeof(std::uint8_t); +}; - // Clear image data - disparityData_arg.clear (); +template ::hasColor > +struct OrganizedConversion; - disparityData_arg.reserve (cloud_size); +// Uncolored point cloud specialization +template +struct OrganizedConversion +{ + /** \brief Convert point cloud to disparity image + * \param[in] cloud_arg input point cloud + * \param[in] focalLength_arg focal length + * \param[in] disparityShift_arg disparity shift + * \param[in] disparityScale_arg disparity scaling + * \param[out] disparityData_arg output disparity image + * \ingroup io + */ + static void convert(const pcl::PointCloud& cloud_arg, + float focalLength_arg, + float disparityShift_arg, + float disparityScale_arg, + bool , + typename std::vector& disparityData_arg, + typename std::vector&) + { + std::size_t cloud_size = cloud_arg.points.size (); - for (std::size_t i = 0; i < cloud_size; ++i) - { - // Get point from cloud - const PointT& point = cloud_arg.points[i]; + // Clear image data + disparityData_arg.clear (); - if (pcl::isFinite (point)) - { - // Inverse depth quantization - std::uint16_t disparity = static_cast ( focalLength_arg / (disparityScale_arg * point.z) + disparityShift_arg / disparityScale_arg); - disparityData_arg.push_back (disparity); - } - else - { - // Non-valid points are encoded with zeros - disparityData_arg.push_back (0); - } - } - } + disparityData_arg.reserve (cloud_size); - /** \brief Convert disparity image to point cloud - * \param[in] disparityData_arg input depth image - * \param[in] width_arg width of disparity image - * \param[in] height_arg height of disparity image - * \param[in] focalLength_arg focal length - * \param[in] disparityShift_arg disparity shift - * \param[in] disparityScale_arg disparity scaling - * \param[out] cloud_arg output point cloud - * \ingroup io - */ - static void convert(typename std::vector& disparityData_arg, - typename std::vector&, - bool, - std::size_t width_arg, - std::size_t height_arg, - float focalLength_arg, - float disparityShift_arg, - float disparityScale_arg, - pcl::PointCloud& cloud_arg) + for (std::size_t i = 0; i < cloud_size; ++i) + { + // Get point from cloud + const PointT& point = cloud_arg.points[i]; + + if (pcl::isFinite (point)) { - std::size_t cloud_size = width_arg * height_arg; + // Inverse depth quantization + std::uint16_t disparity = static_cast ( focalLength_arg / (disparityScale_arg * point.z) + disparityShift_arg / disparityScale_arg); + disparityData_arg.push_back (disparity); + } + else + { + // Non-valid points are encoded with zeros + disparityData_arg.push_back (0); + } + } + } - assert(disparityData_arg.size()==cloud_size); + /** \brief Convert disparity image to point cloud + * \param[in] disparityData_arg input depth image + * \param[in] width_arg width of disparity image + * \param[in] height_arg height of disparity image + * \param[in] focalLength_arg focal length + * \param[in] disparityShift_arg disparity shift + * \param[in] disparityScale_arg disparity scaling + * \param[out] cloud_arg output point cloud + * \ingroup io + */ + static void convert(typename std::vector& disparityData_arg, + typename std::vector&, + bool, + std::size_t width_arg, + std::size_t height_arg, + float focalLength_arg, + float disparityShift_arg, + float disparityScale_arg, + pcl::PointCloud& cloud_arg) + { + std::size_t cloud_size = width_arg * height_arg; - // Reset point cloud - cloud_arg.points.clear (); - cloud_arg.points.reserve (cloud_size); + assert(disparityData_arg.size()==cloud_size); - // Define point cloud parameters - cloud_arg.width = static_cast (width_arg); - cloud_arg.height = static_cast (height_arg); - cloud_arg.is_dense = false; + // Reset point cloud + cloud_arg.points.clear (); + cloud_arg.points.reserve (cloud_size); - // Calculate center of disparity image - int centerX = static_cast (width_arg / 2); - int centerY = static_cast (height_arg / 2); + // Define point cloud parameters + cloud_arg.width = static_cast (width_arg); + cloud_arg.height = static_cast (height_arg); + cloud_arg.is_dense = false; - const float fl_const = 1.0f / focalLength_arg; - static const float bad_point = std::numeric_limits::quiet_NaN (); + // Calculate center of disparity image + int centerX = static_cast (width_arg / 2); + int centerY = static_cast (height_arg / 2); - std::size_t i = 0; - for (int y = -centerY; y < centerY; ++y ) - for (int x = -centerX; x < centerX; ++x ) - { - PointT newPoint; - const std::uint16_t& pixel_disparity = disparityData_arg[i]; - ++i; + const float fl_const = 1.0f / focalLength_arg; + static const float bad_point = std::numeric_limits::quiet_NaN (); - if (pixel_disparity) - { - // Inverse depth decoding - float depth = focalLength_arg / (static_cast (pixel_disparity) * disparityScale_arg + disparityShift_arg); + std::size_t i = 0; + for (int y = -centerY; y < centerY; ++y ) + for (int x = -centerX; x < centerX; ++x ) + { + PointT newPoint; + const std::uint16_t& pixel_disparity = disparityData_arg[i]; + ++i; - // Generate new points - newPoint.x = static_cast (x) * depth * fl_const; - newPoint.y = static_cast (y) * depth * fl_const; - newPoint.z = depth; + if (pixel_disparity) + { + // Inverse depth decoding + float depth = focalLength_arg / (static_cast (pixel_disparity) * disparityScale_arg + disparityShift_arg); - } - else - { - // Generate bad point - newPoint.x = newPoint.y = newPoint.z = bad_point; - } + // Generate new points + newPoint.x = static_cast (x) * depth * fl_const; + newPoint.y = static_cast (y) * depth * fl_const; + newPoint.z = depth; - cloud_arg.points.push_back (newPoint); - } + } + else + { + // Generate bad point + newPoint.x = newPoint.y = newPoint.z = bad_point; + } + cloud_arg.points.push_back (newPoint); } + } + /** \brief Convert disparity image to point cloud + * \param[in] depthData_arg input depth image + * \param[in] width_arg width of disparity image + * \param[in] height_arg height of disparity image + * \param[in] focalLength_arg focal length + * \param[out] cloud_arg output point cloud + * \ingroup io + */ + static void convert(typename std::vector& depthData_arg, + typename std::vector&, + bool, + std::size_t width_arg, + std::size_t height_arg, + float focalLength_arg, + pcl::PointCloud& cloud_arg) + { + std::size_t cloud_size = width_arg * height_arg; - /** \brief Convert disparity image to point cloud - * \param[in] depthData_arg input depth image - * \param[in] width_arg width of disparity image - * \param[in] height_arg height of disparity image - * \param[in] focalLength_arg focal length - * \param[out] cloud_arg output point cloud - * \ingroup io - */ - static void convert(typename std::vector& depthData_arg, - typename std::vector&, - bool, - std::size_t width_arg, - std::size_t height_arg, - float focalLength_arg, - pcl::PointCloud& cloud_arg) - { - std::size_t cloud_size = width_arg * height_arg; - - assert(depthData_arg.size()==cloud_size); - - // Reset point cloud - cloud_arg.points.clear (); - cloud_arg.points.reserve (cloud_size); + assert(depthData_arg.size()==cloud_size); - // Define point cloud parameters - cloud_arg.width = static_cast (width_arg); - cloud_arg.height = static_cast (height_arg); - cloud_arg.is_dense = false; + // Reset point cloud + cloud_arg.points.clear (); + cloud_arg.points.reserve (cloud_size); - // Calculate center of disparity image - int centerX = static_cast (width_arg / 2); - int centerY = static_cast (height_arg / 2); + // Define point cloud parameters + cloud_arg.width = static_cast (width_arg); + cloud_arg.height = static_cast (height_arg); + cloud_arg.is_dense = false; - const float fl_const = 1.0f / focalLength_arg; - static const float bad_point = std::numeric_limits::quiet_NaN (); + // Calculate center of disparity image + int centerX = static_cast (width_arg / 2); + int centerY = static_cast (height_arg / 2); - std::size_t i = 0; - for (int y = -centerY; y < centerY; ++y ) - for (int x = -centerX; x < centerX; ++x ) - { - PointT newPoint; - const float& pixel_depth = depthData_arg[i]; - ++i; + const float fl_const = 1.0f / focalLength_arg; + static const float bad_point = std::numeric_limits::quiet_NaN (); - if (pixel_depth) - { - // Inverse depth decoding - float depth = focalLength_arg / pixel_depth; + std::size_t i = 0; + for (int y = -centerY; y < centerY; ++y ) + for (int x = -centerX; x < centerX; ++x ) + { + PointT newPoint; + const float& pixel_depth = depthData_arg[i]; + ++i; - // Generate new points - newPoint.x = static_cast (x) * depth * fl_const; - newPoint.y = static_cast (y) * depth * fl_const; - newPoint.z = depth; + if (pixel_depth) + { + // Inverse depth decoding + float depth = focalLength_arg / pixel_depth; - } - else - { - // Generate bad point - newPoint.x = newPoint.y = newPoint.z = bad_point; - } + // Generate new points + newPoint.x = static_cast (x) * depth * fl_const; + newPoint.y = static_cast (y) * depth * fl_const; + newPoint.z = depth; - cloud_arg.points.push_back (newPoint); - } + } + else + { + // Generate bad point + newPoint.x = newPoint.y = newPoint.z = bad_point; + } + cloud_arg.points.push_back (newPoint); } + } +}; + +// Colored point cloud specialization +template +struct OrganizedConversion +{ + /** \brief Convert point cloud to disparity image and rgb image + * \param[in] cloud_arg input point cloud + * \param[in] focalLength_arg focal length + * \param[in] disparityShift_arg disparity shift + * \param[in] disparityScale_arg disparity scaling + * \param[in] convertToMono convert color to mono/grayscale + * \param[out] disparityData_arg output disparity image + * \param[out] rgbData_arg output rgb image + * \ingroup io + */ + static void convert(const pcl::PointCloud& cloud_arg, + float focalLength_arg, + float disparityShift_arg, + float disparityScale_arg, + bool convertToMono, + typename std::vector& disparityData_arg, + typename std::vector& rgbData_arg) + { + std::size_t cloud_size = cloud_arg.points.size (); - }; + // Reset output vectors + disparityData_arg.clear (); + rgbData_arg.clear (); - ////////////////////////////////////////////////////////////////////////////////////////////// - // Colored point cloud specialization - ////////////////////////////////////////////////////////////////////////////////////////////// - template - struct OrganizedConversion + // Allocate memory + disparityData_arg.reserve (cloud_size); + if (convertToMono) { - /** \brief Convert point cloud to disparity image and rgb image - * \param[in] cloud_arg input point cloud - * \param[in] focalLength_arg focal length - * \param[in] disparityShift_arg disparity shift - * \param[in] disparityScale_arg disparity scaling - * \param[in] convertToMono convert color to mono/grayscale - * \param[out] disparityData_arg output disparity image - * \param[out] rgbData_arg output rgb image - * \ingroup io - */ - static void convert(const pcl::PointCloud& cloud_arg, - float focalLength_arg, - float disparityShift_arg, - float disparityScale_arg, - bool convertToMono, - typename std::vector& disparityData_arg, - typename std::vector& rgbData_arg) - { - std::size_t cloud_size = cloud_arg.points.size (); + rgbData_arg.reserve (cloud_size); + } else + { + rgbData_arg.reserve (cloud_size * 3); + } - // Reset output vectors - disparityData_arg.clear (); - rgbData_arg.clear (); + for (std::size_t i = 0; i < cloud_size; ++i) + { + const PointT& point = cloud_arg.points[i]; - // Allocate memory - disparityData_arg.reserve (cloud_size); + if (pcl::isFinite (point)) + { if (convertToMono) { - rgbData_arg.reserve (cloud_size); + // Encode point color + std::uint8_t grayvalue = static_cast(0.2989 * point.r + + 0.5870 * point.g + + 0.1140 * point.b); + + rgbData_arg.push_back (grayvalue); } else { - rgbData_arg.reserve (cloud_size * 3); + // Encode point color + rgbData_arg.push_back (point.r); + rgbData_arg.push_back (point.g); + rgbData_arg.push_back (point.b); } + // Inverse depth quantization + std::uint16_t disparity = static_cast (focalLength_arg / (disparityScale_arg * point.z) + disparityShift_arg / disparityScale_arg); - for (std::size_t i = 0; i < cloud_size; ++i) + // Encode disparity + disparityData_arg.push_back (disparity); + } + else + { + // Encode black point + if (convertToMono) { - const PointT& point = cloud_arg.points[i]; - - if (pcl::isFinite (point)) - { - if (convertToMono) - { - // Encode point color - std::uint8_t grayvalue = static_cast(0.2989 * point.r - + 0.5870 * point.g - + 0.1140 * point.b); + rgbData_arg.push_back (0); + } else + { + rgbData_arg.push_back (0); + rgbData_arg.push_back (0); + rgbData_arg.push_back (0); + } - rgbData_arg.push_back (grayvalue); - } else - { - // Encode point color - rgbData_arg.push_back (point.r); - rgbData_arg.push_back (point.g); - rgbData_arg.push_back (point.b); - } + // Encode bad point + disparityData_arg.push_back (0); + } + } + } + /** \brief Convert disparity image to point cloud + * \param[in] disparityData_arg output disparity image + * \param[in] rgbData_arg output rgb image + * \param[in] monoImage_arg input image is a single-channel mono image + * \param[in] width_arg width of disparity image + * \param[in] height_arg height of disparity image + * \param[in] focalLength_arg focal length + * \param[in] disparityShift_arg disparity shift + * \param[in] disparityScale_arg disparity scaling + * \param[out] cloud_arg output point cloud + * \ingroup io + */ + static void convert(typename std::vector& disparityData_arg, + typename std::vector& rgbData_arg, + bool monoImage_arg, + std::size_t width_arg, + std::size_t height_arg, + float focalLength_arg, + float disparityShift_arg, + float disparityScale_arg, + pcl::PointCloud& cloud_arg) + { + std::size_t cloud_size = width_arg*height_arg; + bool hasColor = (!rgbData_arg.empty ()); - // Inverse depth quantization - std::uint16_t disparity = static_cast (focalLength_arg / (disparityScale_arg * point.z) + disparityShift_arg / disparityScale_arg); + // Check size of input data + assert (disparityData_arg.size()==cloud_size); + if (hasColor) + { + if (monoImage_arg) + { + assert (rgbData_arg.size()==cloud_size); + } else + { + assert (rgbData_arg.size()==cloud_size*3); + } + } - // Encode disparity - disparityData_arg.push_back (disparity); - } - else - { + // Reset point cloud + cloud_arg.points.clear(); + cloud_arg.points.reserve(cloud_size); - // Encode black point - if (convertToMono) - { - rgbData_arg.push_back (0); - } else - { - rgbData_arg.push_back (0); - rgbData_arg.push_back (0); - rgbData_arg.push_back (0); - } + // Define point cloud parameters + cloud_arg.width = static_cast(width_arg); + cloud_arg.height = static_cast(height_arg); + cloud_arg.is_dense = false; - // Encode bad point - disparityData_arg.push_back (0); - } - } + // Calculate center of disparity image + int centerX = static_cast(width_arg/2); + int centerY = static_cast(height_arg/2); - } + const float fl_const = 1.0f/focalLength_arg; + static const float bad_point = std::numeric_limits::quiet_NaN (); - /** \brief Convert disparity image to point cloud - * \param[in] disparityData_arg output disparity image - * \param[in] rgbData_arg output rgb image - * \param[in] monoImage_arg input image is a single-channel mono image - * \param[in] width_arg width of disparity image - * \param[in] height_arg height of disparity image - * \param[in] focalLength_arg focal length - * \param[in] disparityShift_arg disparity shift - * \param[in] disparityScale_arg disparity scaling - * \param[out] cloud_arg output point cloud - * \ingroup io - */ - static void convert(typename std::vector& disparityData_arg, - typename std::vector& rgbData_arg, - bool monoImage_arg, - std::size_t width_arg, - std::size_t height_arg, - float focalLength_arg, - float disparityShift_arg, - float disparityScale_arg, - pcl::PointCloud& cloud_arg) + std::size_t i = 0; + for (int y = -centerY; y < centerY; ++y ) + for (int x = -centerX; x < centerX; ++x ) { - std::size_t cloud_size = width_arg*height_arg; - bool hasColor = (!rgbData_arg.empty ()); - - // Check size of input data - assert (disparityData_arg.size()==cloud_size); - if (hasColor) - { - if (monoImage_arg) - { - assert (rgbData_arg.size()==cloud_size); - } else - { - assert (rgbData_arg.size()==cloud_size*3); - } - } - - // Reset point cloud - cloud_arg.points.clear(); - cloud_arg.points.reserve(cloud_size); + PointT newPoint; - // Define point cloud parameters - cloud_arg.width = static_cast(width_arg); - cloud_arg.height = static_cast(height_arg); - cloud_arg.is_dense = false; + const std::uint16_t& pixel_disparity = disparityData_arg[i]; - // Calculate center of disparity image - int centerX = static_cast(width_arg/2); - int centerY = static_cast(height_arg/2); + if (pixel_disparity && (pixel_disparity!=0x7FF)) + { + float depth = focalLength_arg / (static_cast (pixel_disparity) * disparityScale_arg + disparityShift_arg); - const float fl_const = 1.0f/focalLength_arg; - static const float bad_point = std::numeric_limits::quiet_NaN (); + // Define point location + newPoint.z = depth; + newPoint.x = static_cast (x) * depth * fl_const; + newPoint.y = static_cast (y) * depth * fl_const; - std::size_t i = 0; - for (int y = -centerY; y < centerY; ++y ) - for (int x = -centerX; x < centerX; ++x ) + if (hasColor) { - PointT newPoint; - - const std::uint16_t& pixel_disparity = disparityData_arg[i]; - - if (pixel_disparity && (pixel_disparity!=0x7FF)) + if (monoImage_arg) { - float depth = focalLength_arg / (static_cast (pixel_disparity) * disparityScale_arg + disparityShift_arg); - - // Define point location - newPoint.z = depth; - newPoint.x = static_cast (x) * depth * fl_const; - newPoint.y = static_cast (y) * depth * fl_const; - - if (hasColor) - { - if (monoImage_arg) - { - // Define point color - newPoint.r = rgbData_arg[i]; - newPoint.g = rgbData_arg[i]; - newPoint.b = rgbData_arg[i]; - } else - { - // Define point color - newPoint.r = rgbData_arg[i*3+0]; - newPoint.g = rgbData_arg[i*3+1]; - newPoint.b = rgbData_arg[i*3+2]; - } - - } else - { - // Set white point color - newPoint.rgba = 0xffffffffu; - } + // Define point color + newPoint.r = rgbData_arg[i]; + newPoint.g = rgbData_arg[i]; + newPoint.b = rgbData_arg[i]; } else { - // Define bad point - newPoint.x = newPoint.y = newPoint.z = bad_point; - newPoint.rgb = 0.0f; + // Define point color + newPoint.r = rgbData_arg[i*3+0]; + newPoint.g = rgbData_arg[i*3+1]; + newPoint.b = rgbData_arg[i*3+2]; } - // Add point to cloud - cloud_arg.points.push_back(newPoint); - // Increment point iterator - ++i; - } - } - - /** \brief Convert disparity image to point cloud - * \param[in] depthData_arg output disparity image - * \param[in] rgbData_arg output rgb image - * \param[in] monoImage_arg input image is a single-channel mono image - * \param[in] width_arg width of disparity image - * \param[in] height_arg height of disparity image - * \param[in] focalLength_arg focal length - * \param[out] cloud_arg output point cloud - * \ingroup io - */ - static void convert(typename std::vector& depthData_arg, - typename std::vector& rgbData_arg, - bool monoImage_arg, - std::size_t width_arg, - std::size_t height_arg, - float focalLength_arg, - pcl::PointCloud& cloud_arg) - { - std::size_t cloud_size = width_arg*height_arg; - bool hasColor = (!rgbData_arg.empty ()); - - // Check size of input data - assert (depthData_arg.size()==cloud_size); - if (hasColor) - { - if (monoImage_arg) - { - assert (rgbData_arg.size()==cloud_size); } else { - assert (rgbData_arg.size()==cloud_size*3); + // Set white point color + newPoint.rgba = 0xffffffffu; } + } else + { + // Define bad point + newPoint.x = newPoint.y = newPoint.z = bad_point; + newPoint.rgb = 0.0f; } - // Reset point cloud - cloud_arg.points.clear(); - cloud_arg.points.reserve(cloud_size); + // Add point to cloud + cloud_arg.points.push_back(newPoint); + // Increment point iterator + ++i; + } + } - // Define point cloud parameters - cloud_arg.width = static_cast(width_arg); - cloud_arg.height = static_cast(height_arg); - cloud_arg.is_dense = false; + /** \brief Convert disparity image to point cloud + * \param[in] depthData_arg output disparity image + * \param[in] rgbData_arg output rgb image + * \param[in] monoImage_arg input image is a single-channel mono image + * \param[in] width_arg width of disparity image + * \param[in] height_arg height of disparity image + * \param[in] focalLength_arg focal length + * \param[out] cloud_arg output point cloud + * \ingroup io + */ + static void convert(typename std::vector& depthData_arg, + typename std::vector& rgbData_arg, + bool monoImage_arg, + std::size_t width_arg, + std::size_t height_arg, + float focalLength_arg, + pcl::PointCloud& cloud_arg) + { + std::size_t cloud_size = width_arg*height_arg; + bool hasColor = (!rgbData_arg.empty ()); - // Calculate center of disparity image - int centerX = static_cast(width_arg/2); - int centerY = static_cast(height_arg/2); + // Check size of input data + assert (depthData_arg.size()==cloud_size); + if (hasColor) + { + if (monoImage_arg) + { + assert (rgbData_arg.size()==cloud_size); + } else + { + assert (rgbData_arg.size()==cloud_size*3); + } + } - const float fl_const = 1.0f/focalLength_arg; - static const float bad_point = std::numeric_limits::quiet_NaN (); + // Reset point cloud + cloud_arg.points.clear(); + cloud_arg.points.reserve(cloud_size); - std::size_t i = 0; - for (int y = -centerY; y < centerY; ++y ) - for (int x = -centerX; x < centerX; ++x ) - { - PointT newPoint; + // Define point cloud parameters + cloud_arg.width = static_cast(width_arg); + cloud_arg.height = static_cast(height_arg); + cloud_arg.is_dense = false; - const float& pixel_depth = depthData_arg[i]; + // Calculate center of disparity image + int centerX = static_cast(width_arg/2); + int centerY = static_cast(height_arg/2); - if (pixel_depth==pixel_depth) + const float fl_const = 1.0f/focalLength_arg; + static const float bad_point = std::numeric_limits::quiet_NaN (); + + std::size_t i = 0; + for (int y = -centerY; y < centerY; ++y ) + for (int x = -centerX; x < centerX; ++x ) + { + PointT newPoint; + + const float& pixel_depth = depthData_arg[i]; + + if (pixel_depth==pixel_depth) + { + float depth = focalLength_arg / pixel_depth; + + // Define point location + newPoint.z = depth; + newPoint.x = static_cast (x) * depth * fl_const; + newPoint.y = static_cast (y) * depth * fl_const; + + if (hasColor) + { + if (monoImage_arg) { - float depth = focalLength_arg / pixel_depth; - - // Define point location - newPoint.z = depth; - newPoint.x = static_cast (x) * depth * fl_const; - newPoint.y = static_cast (y) * depth * fl_const; - - if (hasColor) - { - if (monoImage_arg) - { - // Define point color - newPoint.r = rgbData_arg[i]; - newPoint.g = rgbData_arg[i]; - newPoint.b = rgbData_arg[i]; - } else - { - // Define point color - newPoint.r = rgbData_arg[i*3+0]; - newPoint.g = rgbData_arg[i*3+1]; - newPoint.b = rgbData_arg[i*3+2]; - } - - } else - { - // Set white point color - newPoint.rgba = 0xffffffffu; - } + // Define point color + newPoint.r = rgbData_arg[i]; + newPoint.g = rgbData_arg[i]; + newPoint.b = rgbData_arg[i]; } else { - // Define bad point - newPoint.x = newPoint.y = newPoint.z = bad_point; - newPoint.rgb = 0.0f; + // Define point color + newPoint.r = rgbData_arg[i*3+0]; + newPoint.g = rgbData_arg[i*3+1]; + newPoint.b = rgbData_arg[i*3+2]; } - // Add point to cloud - cloud_arg.points.push_back(newPoint); - // Increment point iterator - ++i; + } else + { + // Set white point color + newPoint.rgba = 0xffffffffu; + } + } else + { + // Define bad point + newPoint.x = newPoint.y = newPoint.z = bad_point; + newPoint.rgb = 0.0f; } - } - }; + // Add point to cloud + cloud_arg.points.push_back(newPoint); + // Increment point iterator + ++i; + } } -} +}; + +} // namespace io +} // namespace pcl + diff --git a/io/include/pcl/compression/point_coding.h b/io/include/pcl/compression/point_coding.h index fe5041a8..3ff52074 100644 --- a/io/include/pcl/compression/point_coding.h +++ b/io/include/pcl/compression/point_coding.h @@ -45,161 +45,163 @@ namespace pcl { - namespace octree - { - /** \brief @b PointCoding class - * \note This class encodes 8-bit differential point information for octree-based point cloud compression. - * \note typename: PointT: type of point used in pointcloud - * \author Julius Kammerl (julius@kammerl.de) +namespace octree +{ +/** \brief @b PointCoding class + * \note This class encodes 8-bit differential point information for octree-based point cloud compression. + * \note typename: PointT: type of point used in pointcloud + * \author Julius Kammerl (julius@kammerl.de) + */ +template +class PointCoding +{ + // public typedefs + using PointCloud = pcl::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + + public: + /** \brief Constructor. */ + PointCoding () : + output_ (), + pointCompressionResolution_ (0.001f) // 1mm + { + } + + /** \brief Empty class constructor. */ + virtual + ~PointCoding () + { + } + + /** \brief Define precision of point information + * \param precision_arg: precision + */ + inline void + setPrecision (float precision_arg) + { + pointCompressionResolution_ = precision_arg; + } + + /** \brief Retrieve precision of point information + * \return precision */ - template - class PointCoding + inline float + getPrecision () { - // public typedefs - using PointCloud = pcl::PointCloud; - using PointCloudPtr = typename PointCloud::Ptr; - using PointCloudConstPtr = typename PointCloud::ConstPtr; - - public: - /** \brief Constructor. */ - PointCoding () : - output_ (), - pointCompressionResolution_ (0.001f) // 1mm - { - } - - /** \brief Empty class constructor. */ - virtual - ~PointCoding () - { - } - - /** \brief Define precision of point information - * \param precision_arg: precision - */ - inline void - setPrecision (float precision_arg) - { - pointCompressionResolution_ = precision_arg; - } - - /** \brief Retrieve precision of point information - * \return precision - */ - inline float - getPrecision () - { - return (pointCompressionResolution_); - } - - /** \brief Set amount of points within point cloud to be encoded and reserve memory - * \param pointCount_arg: amounts of points within point cloud - */ - inline void - setPointCount (unsigned int pointCount_arg) - { - pointDiffDataVector_.reserve (pointCount_arg * 3); - } - - /** \brief Initialize encoding of differential point */ - void - initializeEncoding () - { - pointDiffDataVector_.clear (); - } - - /** \brief Initialize decoding of differential point */ - void - initializeDecoding () - { - pointDiffDataVectorIterator_ = pointDiffDataVector_.begin (); - } - - /** \brief Get reference to vector containing differential color data */ - std::vector& - getDifferentialDataVector () - { - return (pointDiffDataVector_); - } - - /** \brief Encode differential point information for a subset of points from point cloud - * \param indexVector_arg indices defining a subset of points from points cloud - * \param referencePoint_arg coordinates of reference point - * \param inputCloud_arg input point cloud - */ - void - encodePoints (const typename std::vector& indexVector_arg, const double* referencePoint_arg, - PointCloudConstPtr inputCloud_arg) - { - std::size_t len = indexVector_arg.size (); - - // iterate over points within current voxel - for (std::size_t i = 0; i < len; i++) - { - unsigned char diffX, diffY, diffZ; - - // retrieve point from cloud - const int& idx = indexVector_arg[i]; - const PointT& idxPoint = inputCloud_arg->points[idx]; - - // differentially encode point coordinates and truncate overflow - diffX = static_cast (max (-127, min(127, static_cast ((idxPoint.x - referencePoint_arg[0]) / pointCompressionResolution_)))); - diffY = static_cast (max (-127, min(127, static_cast ((idxPoint.y - referencePoint_arg[1]) / pointCompressionResolution_)))); - diffZ = static_cast (max (-127, min(127, static_cast ((idxPoint.z - referencePoint_arg[2]) / pointCompressionResolution_)))); - - // store information in differential point vector - pointDiffDataVector_.push_back (diffX); - pointDiffDataVector_.push_back (diffY); - pointDiffDataVector_.push_back (diffZ); - } - } - - /** \brief Decode differential point information - * \param outputCloud_arg output point cloud - * \param referencePoint_arg coordinates of reference point - * \param beginIdx_arg index indicating first point to be assigned with color information - * \param endIdx_arg index indicating last point to be assigned with color information - */ - void - decodePoints (PointCloudPtr outputCloud_arg, const double* referencePoint_arg, std::size_t beginIdx_arg, - std::size_t endIdx_arg) - { - assert (beginIdx_arg <= endIdx_arg); - - unsigned int pointCount = static_cast (endIdx_arg - beginIdx_arg); - - // iterate over points within current voxel - for (std::size_t i = 0; i < pointCount; i++) - { - // retrieve differential point information - const unsigned char& diffX = static_cast (*(pointDiffDataVectorIterator_++)); - const unsigned char& diffY = static_cast (*(pointDiffDataVectorIterator_++)); - const unsigned char& diffZ = static_cast (*(pointDiffDataVectorIterator_++)); - - // retrieve point from point cloud - PointT& point = outputCloud_arg->points[beginIdx_arg + i]; - - // decode point position - point.x = static_cast (referencePoint_arg[0] + diffX * pointCompressionResolution_); - point.y = static_cast (referencePoint_arg[1] + diffY * pointCompressionResolution_); - point.z = static_cast (referencePoint_arg[2] + diffZ * pointCompressionResolution_); - } - } - - protected: - /** \brief Pointer to output point cloud dataset. */ - PointCloudPtr output_; - - /** \brief Vector for storing differential point information */ - std::vector pointDiffDataVector_; - - /** \brief Iterator on differential point information vector */ - std::vector::const_iterator pointDiffDataVectorIterator_; - - /** \brief Precision of point coding*/ - float pointCompressionResolution_; - }; - } -} + return (pointCompressionResolution_); + } + + /** \brief Set amount of points within point cloud to be encoded and reserve memory + * \param pointCount_arg: amounts of points within point cloud + */ + inline void + setPointCount (unsigned int pointCount_arg) + { + pointDiffDataVector_.reserve (pointCount_arg * 3); + } + + /** \brief Initialize encoding of differential point */ + void + initializeEncoding () + { + pointDiffDataVector_.clear (); + } + + /** \brief Initialize decoding of differential point */ + void + initializeDecoding () + { + pointDiffDataVectorIterator_ = pointDiffDataVector_.begin (); + } + + /** \brief Get reference to vector containing differential color data */ + std::vector& + getDifferentialDataVector () + { + return (pointDiffDataVector_); + } + + /** \brief Encode differential point information for a subset of points from point cloud + * \param indexVector_arg indices defining a subset of points from points cloud + * \param referencePoint_arg coordinates of reference point + * \param inputCloud_arg input point cloud + */ + void + encodePoints (const typename std::vector& indexVector_arg, const double* referencePoint_arg, + PointCloudConstPtr inputCloud_arg) + { + std::size_t len = indexVector_arg.size (); + + // iterate over points within current voxel + for (std::size_t i = 0; i < len; i++) + { + unsigned char diffX, diffY, diffZ; + + // retrieve point from cloud + const int& idx = indexVector_arg[i]; + const PointT& idxPoint = inputCloud_arg->points[idx]; + + // differentially encode point coordinates and truncate overflow + diffX = static_cast (max (-127, min(127, static_cast ((idxPoint.x - referencePoint_arg[0]) / pointCompressionResolution_)))); + diffY = static_cast (max (-127, min(127, static_cast ((idxPoint.y - referencePoint_arg[1]) / pointCompressionResolution_)))); + diffZ = static_cast (max (-127, min(127, static_cast ((idxPoint.z - referencePoint_arg[2]) / pointCompressionResolution_)))); + + // store information in differential point vector + pointDiffDataVector_.push_back (diffX); + pointDiffDataVector_.push_back (diffY); + pointDiffDataVector_.push_back (diffZ); + } + } + + /** \brief Decode differential point information + * \param outputCloud_arg output point cloud + * \param referencePoint_arg coordinates of reference point + * \param beginIdx_arg index indicating first point to be assigned with color information + * \param endIdx_arg index indicating last point to be assigned with color information + */ + void + decodePoints (PointCloudPtr outputCloud_arg, const double* referencePoint_arg, std::size_t beginIdx_arg, + std::size_t endIdx_arg) + { + assert (beginIdx_arg <= endIdx_arg); + + unsigned int pointCount = static_cast (endIdx_arg - beginIdx_arg); + + // iterate over points within current voxel + for (std::size_t i = 0; i < pointCount; i++) + { + // retrieve differential point information + const unsigned char& diffX = static_cast (*(pointDiffDataVectorIterator_++)); + const unsigned char& diffY = static_cast (*(pointDiffDataVectorIterator_++)); + const unsigned char& diffZ = static_cast (*(pointDiffDataVectorIterator_++)); + + // retrieve point from point cloud + PointT& point = outputCloud_arg->points[beginIdx_arg + i]; + + // decode point position + point.x = static_cast (referencePoint_arg[0] + diffX * pointCompressionResolution_); + point.y = static_cast (referencePoint_arg[1] + diffY * pointCompressionResolution_); + point.z = static_cast (referencePoint_arg[2] + diffZ * pointCompressionResolution_); + } + } + + protected: + /** \brief Pointer to output point cloud dataset. */ + PointCloudPtr output_; + + /** \brief Vector for storing differential point information */ + std::vector pointDiffDataVector_; + + /** \brief Iterator on differential point information vector */ + std::vector::const_iterator pointDiffDataVectorIterator_; + + /** \brief Precision of point coding*/ + float pointCompressionResolution_; +}; + +} // namespace octree +} // namespace pcl #define PCL_INSTANTIATE_ColorCoding(T) template class PCL_EXPORTS pcl::octree::ColorCoding; + diff --git a/io/include/pcl/io/ascii_io.h b/io/include/pcl/io/ascii_io.h index c7ebf40a..8ba24241 100644 --- a/io/include/pcl/io/ascii_io.h +++ b/io/include/pcl/io/ascii_io.h @@ -117,7 +117,7 @@ namespace pcl * \param[in] p a point type */ template - PCL_DEPRECATED("use parameterless setInputFields() instead") + PCL_DEPRECATED(1, 12, "use parameterless setInputFields() instead") inline void setInputFields (const PointT p) { (void) p; diff --git a/io/include/pcl/io/boost.h b/io/include/pcl/io/boost.h index 395f1949..685b36ae 100644 --- a/io/include/pcl/io/boost.h +++ b/io/include/pcl/io/boost.h @@ -38,7 +38,7 @@ #pragma once #if defined __GNUC__ -# pragma GCC system_header +# pragma GCC system_header #endif //https://bugreports.qt-project.org/browse/QTBUG-22829 #ifndef Q_MOC_RUN @@ -46,8 +46,6 @@ #include #include #include -#include -#include #include #include #include diff --git a/io/include/pcl/io/ensenso_grabber.h b/io/include/pcl/io/ensenso_grabber.h index c445e6c0..e95dd448 100644 --- a/io/include/pcl/io/ensenso_grabber.h +++ b/io/include/pcl/io/ensenso_grabber.h @@ -169,7 +169,7 @@ namespace pcl * @return True if successful, false otherwise * @warning A device must be opened and not running */ bool - grabSingleCloud (pcl::PointCloud &cloud); + grabSingleCloud (pcl::PointCloud &cloud) const; /** @brief Set up the Ensenso sensor and API to do 3D extrinsic calibration using the Ensenso 2D patterns * @param[in] grid_spacing @@ -256,7 +256,7 @@ namespace pcl setExtrinsicCalibration (const double euler_angle, Eigen::Vector3d &rotation_axis, const Eigen::Vector3d &translation, - const std::string target = "Hand"); + const std::string target = "Hand") const; /** @brief Update Link node in NxLib tree with an identity matrix * @param[in] target "Hand" or "Workspace" for example diff --git a/io/include/pcl/io/grabber.h b/io/include/pcl/io/grabber.h index b94d42d0..378b81a8 100644 --- a/io/include/pcl/io/grabber.h +++ b/io/include/pcl/io/grabber.h @@ -31,16 +31,18 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ - + #pragma once #include // needed for the grabber interface / observers #include +#include #include #include #include +#include #include #include #include @@ -57,8 +59,37 @@ namespace pcl class PCL_EXPORTS Grabber { public: + /** + * \brief Default ctor + */ + Grabber() = default; + + /** + * \brief No copy ctor since Grabber can't be copied + */ + Grabber(const Grabber&) = delete; + + /** + * \brief No copy assign operator since Grabber can't be copied + */ + Grabber& operator=(const Grabber&) = delete; + + /** + * \brief Move ctor + */ + Grabber(Grabber&&) = default; + + /** + * \brief Move assign operator + */ + Grabber& operator=(Grabber&&) = default; + /** \brief virtual destructor. */ - virtual inline ~Grabber () noexcept; + #if defined(_MSC_VER) + virtual inline ~Grabber () noexcept {} + #else + virtual inline ~Grabber () noexcept = default; + #endif /** \brief registers a callback function/method to a signal with the corresponding signature * \param[in] callback: the callback function/method @@ -72,7 +103,7 @@ namespace pcl * \return Connection object, that can be used to disconnect the callback method from the signal again. */ template class FunctionT> - PCL_DEPRECATED ("please assign the callback to a std::function.") + PCL_DEPRECATED (1, 12, "please assign the callback to a std::function.") boost::signals2::connection registerCallback (const FunctionT& callback) { @@ -82,19 +113,19 @@ namespace pcl /** \brief indicates whether a signal with given parameter-type exists or not * \return true if signal exists, false otherwise */ - template bool - providesCallback () const; + template bool + providesCallback () const 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. */ - virtual void + virtual void start () = 0; /** \brief For devices that are streaming, the streams are stopped. * This method has no effect for triggered devices. */ - virtual void + virtual void stop () = 0; /** \brief For devices that are streaming, stopped streams are started and running stream are stopped. @@ -107,17 +138,17 @@ namespace pcl /** \brief returns the name of the concrete subclass. * \return the name of the concrete driver. */ - virtual std::string + virtual std::string getName () const = 0; /** \brief Indicates whether the grabber is streaming or not. This value is not defined for triggered devices. * \return true if grabber is running / streaming. False otherwise. */ - virtual bool + virtual bool isRunning () const = 0; /** \brief returns fps. 0 if trigger based. */ - virtual float + virtual float getFramesPerSecond () const = 0; protected: @@ -125,41 +156,35 @@ namespace pcl virtual void signalsChanged () { } - template boost::signals2::signal* - find_signal () const; + template boost::signals2::signal* + find_signal () const noexcept; - template int - num_slots () const; + template int + num_slots () const noexcept; - template void + template void disconnect_all_slots (); - template void + template void block_signal (); - - template void + + template void unblock_signal (); - - inline void + + inline void block_signals (); - - inline void + + inline void unblock_signals (); - template boost::signals2::signal* + template boost::signals2::signal* createSignal (); - std::map signals_; + std::map> signals_; std::map > connections_; std::map > shared_connections_; } ; - Grabber::~Grabber () noexcept - { - for (auto &signal : signals_) - delete signal.second; - } - bool Grabber::toggle () { @@ -174,25 +199,24 @@ namespace pcl } template boost::signals2::signal* - Grabber::find_signal () const + Grabber::find_signal () const noexcept { using Signal = boost::signals2::signal; - std::map::const_iterator signal_it = signals_.find (typeid (T).name ()); + const auto signal_it = signals_.find (typeid (T).name ()); if (signal_it != signals_.end ()) - return (dynamic_cast (signal_it->second)); - - return (NULL); + { + return (static_cast (signal_it->second.get ())); + } + return nullptr; } template void Grabber::disconnect_all_slots () { - using Signal = boost::signals2::signal; - - if (signals_.find (typeid (T).name ()) != signals_.end ()) + const auto signal = find_signal (); + if (signal != nullptr) { - Signal* signal = dynamic_cast (signals_[typeid (T).name ()]); signal->disconnect_all_slots (); } } @@ -230,39 +254,56 @@ namespace pcl } template int - Grabber::num_slots () const + Grabber::num_slots () const noexcept { - using Signal = boost::signals2::signal; - - // see if we have a signal for this type - std::map::const_iterator signal_it = signals_.find (typeid (T).name ()); - if (signal_it != signals_.end ()) + const auto signal = find_signal (); + if (signal != nullptr) { - Signal* signal = dynamic_cast (signal_it->second); - return (static_cast (signal->num_slots ())); + return static_cast (signal->num_slots ()); } - return (0); + return 0; } template boost::signals2::signal* Grabber::createSignal () { using Signal = boost::signals2::signal; - - if (signals_.find (typeid (T).name ()) == signals_.end ()) + using Base = boost::signals2::signal_base; + // DefferedPtr serves 2 purposes: + // * allows MSVC to copy it around, can't do that with unique_ptr + // * performs dynamic allocation only when required. If the key is found, this + // struct is a no-op, otherwise it allocates when implicit conversion operator + // is called inside emplace/try_emplace + struct DefferedPtr { + operator std::unique_ptr() const { return std::make_unique(); } + }; + // TODO: remove later for C++17 features: structured bindings and try_emplace + #ifdef __cpp_structured_bindings + const auto [iterator, success] = + #else + typename decltype(signals_)::const_iterator iterator; + bool success; + std::tie (iterator, success) = + #endif + + #ifdef __cpp_lib_map_try_emplace + signals_.try_emplace ( + #else + signals_.emplace ( + #endif + std::string (typeid (T).name ()), DefferedPtr ()); + if (!success) { - Signal* signal = new Signal (); - signals_[typeid (T).name ()] = signal; - return (signal); + return nullptr; } - return (nullptr); + return static_cast (iterator->second.get ()); } template boost::signals2::connection Grabber::registerCallback (const std::function & callback) { - using Signal = boost::signals2::signal; - if (signals_.find (typeid (T).name ()) == signals_.end ()) + const auto signal = find_signal (); + if (signal == nullptr) { std::stringstream sstream; @@ -271,7 +312,6 @@ namespace pcl PCL_THROW_EXCEPTION (pcl::IOException, "[" << getName () << "] " << sstream.str ()); //return (boost::signals2::connection ()); } - Signal* signal = dynamic_cast (signals_[typeid (T).name ()]); boost::signals2::connection ret = signal->connect (callback); connections_[typeid (T).name ()].push_back (ret); @@ -281,11 +321,9 @@ namespace pcl } template bool - Grabber::providesCallback () const + Grabber::providesCallback () const noexcept { - if (signals_.find (typeid (T).name ()) == signals_.end ()) - return (false); - return (true); + return find_signal (); } } // namespace diff --git a/io/include/pcl/io/hdl_grabber.h b/io/include/pcl/io/hdl_grabber.h index d33352ea..c95410c6 100644 --- a/io/include/pcl/io/hdl_grabber.h +++ b/io/include/pcl/io/hdl_grabber.h @@ -71,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 PCL_DEPRECATED("use sig_cb_velodyne_hdl_scan_point_cloud_xyzrgba instead") + using sig_cb_velodyne_hdl_scan_point_cloud_xyzrgb PCL_DEPRECATED(1, 12, "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 @@ -97,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 PCL_DEPRECATED("use sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgba instead") + using sig_cb_velodyne_hdl_sweep_point_cloud_xyzrgb PCL_DEPRECATED(1, 12, "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] @@ -270,7 +270,7 @@ namespace pcl computeXYZI (pcl::PointXYZI& pointXYZI, std::uint16_t azimuth, HDLLaserReturn laserReturn, - HDLLaserCorrection correction); + HDLLaserCorrection correction) const; private: diff --git a/io/include/pcl/io/image.h b/io/include/pcl/io/image.h index 4cb43ee3..187b388e 100644 --- a/io/include/pcl/io/image.h +++ b/io/include/pcl/io/image.h @@ -33,21 +33,21 @@ * POSSIBILITY OF SUCH DAMAGE. * */ - -#pragma once -#include +#pragma once -#include -#include #include - #include +#include +#include +#include + +#include namespace pcl { namespace io - { + { /** * @brief Image interface class providing an interface to fill a RGB or Grayscale image buffer. @@ -212,3 +212,4 @@ namespace pcl } // namespace } + diff --git a/io/include/pcl/io/image_grabber.h b/io/include/pcl/io/image_grabber.h index 0b505c6a..b69a0f6d 100644 --- a/io/include/pcl/io/image_grabber.h +++ b/io/include/pcl/io/image_grabber.h @@ -40,17 +40,17 @@ #pragma once +#include +#include #include +#include #include #include -#include -#include - -#include #include #include + namespace pcl { /** \brief Base class for Image file grabber. @@ -75,59 +75,41 @@ namespace pcl */ ImageGrabberBase (const std::vector& depth_image_files, float frames_per_second, bool repeat); - /** \brief Copy constructor. - * \param[in] src the Image Grabber base object to copy into this - */ - ImageGrabberBase (const ImageGrabberBase &src) : impl_ () - { - *this = src; - } - - /** \brief Copy operator. - * \param[in] src the Image Grabber base object to copy into this - */ - ImageGrabberBase& - operator = (const ImageGrabberBase &src) - { - impl_ = src.impl_; - return (*this); - } - /** \brief Virtual destructor. */ ~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 + void start () override; - + /** \brief Stops playing the list of PCD files if frames_per_second is > 0. Otherwise the method has no effect. */ - void + void stop () override; - + /** \brief Triggers a callback with new data */ - virtual void + virtual void trigger (); /** \brief whether the grabber is started (publishing) or not. * \return true only if publishing. */ - bool + bool isRunning () const override; - + /** \return The name of the grabber */ - std::string + std::string getName () const override; - + /** \brief Rewinds to the first PCD file in the list.*/ - virtual void + virtual void rewind (); /** \brief Returns the frames_per_second. 0 if grabber is trigger-based */ - float + float getFramesPerSecond () const override; /** \brief Returns whether the repeat flag is on */ - bool + bool isRepeatOn () const; /** \brief Returns if the last frame is reached */ @@ -138,7 +120,7 @@ namespace pcl std::string getCurrentDepthFileName () const; - /** \brief Returns the filename of the previous indexed file + /** \brief Returns the filename of the previous indexed file * SDM: adding this back in, but is this useful, or confusing? */ std::string getPrevDepthFileName () const; @@ -157,18 +139,18 @@ namespace pcl void setRGBImageFiles (const std::vector& rgb_image_files); - /** \brief Define custom focal length and center pixel. This will override ANY other setting of parameters for the duration of the grabber's life, whether by factory defaults or explicitly read from a frame_[timestamp].xml file. + /** \brief Define custom focal length and center pixel. This will override ANY other setting of parameters for the duration of the grabber's life, whether by factory defaults or explicitly read from a frame_[timestamp].xml file. * \param[in] focal_length_x Horizontal focal length (fx) * \param[in] focal_length_y Vertical focal length (fy) * \param[in] principal_point_x Horizontal coordinates of the principal point (cx) * \param[in] principal_point_y Vertical coordinates of the principal point (cy) */ virtual void - setCameraIntrinsics (const double focal_length_x, - const double focal_length_y, - const double principal_point_x, + setCameraIntrinsics (const double focal_length_x, + const double focal_length_y, + const double principal_point_x, const double principal_point_y); - + /** \brief Get the current focal length and center pixel. If the intrinsics have been manually set with setCameraIntrinsics, this will return those values. Else, if start () has been called and the grabber has found a frame_[timestamp].xml file, this will return the most recent values read. Else, returns factory defaults. * \param[out] focal_length_x Horizontal focal length (fx) * \param[out] focal_length_y Vertical focal length (fy) @@ -176,16 +158,16 @@ namespace pcl * \param[out] principal_point_y Vertical coordinates of the principal point (cy) */ virtual void - getCameraIntrinsics (double &focal_length_x, - double &focal_length_y, - double &principal_point_x, + getCameraIntrinsics (double &focal_length_x, + double &focal_length_y, + double &principal_point_x, double &principal_point_y) const; /** \brief Define the units the depth data is stored in. * Defaults to mm (0.001), meaning a brightness of 1000 corresponds to 1 m*/ void setDepthImageUnits (float units); - + /** \brief Set the number of threads, if we wish to use OpenMP for quicker cloud population. * Note that for a standard (< 4 core) machine this is unlikely to yield a drastic speedup.*/ void @@ -196,14 +178,14 @@ namespace pcl */ std::size_t numFrames () const; - + /** \brief Gets the cloud in ROS form at location idx */ bool getCloudAt (std::size_t idx, pcl::PCLPointCloud2 &blob, Eigen::Vector4f &origin, Eigen::Quaternionf &orientation) const; private: - virtual void + virtual void publish (const pcl::PCLPointCloud2& blob, const Eigen::Vector4f& origin, const Eigen::Quaternionf& orientation) const = 0; @@ -220,23 +202,23 @@ namespace pcl using Ptr = shared_ptr; using ConstPtr = shared_ptr; - ImageGrabber (const std::string& dir, - float frames_per_second = 0, - bool repeat = false, + ImageGrabber (const std::string& dir, + float frames_per_second = 0, + bool repeat = false, bool pclzf_mode = false); - ImageGrabber (const std::string& depth_dir, - const std::string& rgb_dir, - float frames_per_second = 0, + ImageGrabber (const std::string& depth_dir, + const std::string& rgb_dir, + float frames_per_second = 0, bool repeat = false); - ImageGrabber (const std::vector& depth_image_files, - float frames_per_second = 0, + ImageGrabber (const std::vector& depth_image_files, + float frames_per_second = 0, bool repeat = false); - + /** \brief Empty destructor */ ~ImageGrabber () noexcept {} - + // Inherited from FileGrabber const typename pcl::PointCloud::ConstPtr operator[] (std::size_t idx) const override; @@ -246,29 +228,29 @@ namespace pcl size () const override; protected: - void + void publish (const pcl::PCLPointCloud2& blob, - const Eigen::Vector4f& origin, + const Eigen::Vector4f& origin, const Eigen::Quaternionf& orientation) const override; boost::signals2::signal::ConstPtr&)>* signal_; }; //////////////////////////////////////////////////////////////////////////////////////////////////////////////// template - ImageGrabber::ImageGrabber (const std::string& dir, - float frames_per_second, - bool repeat, + ImageGrabber::ImageGrabber (const std::string& dir, + float frames_per_second, + bool repeat, bool pclzf_mode) : ImageGrabberBase (dir, frames_per_second, repeat, pclzf_mode) { signal_ = createSignal::ConstPtr&)>(); } - + //////////////////////////////////////////////////////////////////////////////////////////////////////////////// template - ImageGrabber::ImageGrabber (const std::string& depth_dir, - const std::string& rgb_dir, - float frames_per_second, + ImageGrabber::ImageGrabber (const std::string& depth_dir, + const std::string& rgb_dir, + float frames_per_second, bool repeat) : ImageGrabberBase (depth_dir, rgb_dir, frames_per_second, repeat) { @@ -277,8 +259,8 @@ namespace pcl //////////////////////////////////////////////////////////////////////////////////////////////////////////////// template - ImageGrabber::ImageGrabber (const std::vector& depth_image_files, - float frames_per_second, + ImageGrabber::ImageGrabber (const std::vector& depth_image_files, + float frames_per_second, bool repeat) : ImageGrabberBase (depth_image_files, frames_per_second, repeat), signal_ () { diff --git a/io/include/pcl/io/image_ir.h b/io/include/pcl/io/image_ir.h index ad6c2753..c0b59311 100644 --- a/io/include/pcl/io/image_ir.h +++ b/io/include/pcl/io/image_ir.h @@ -38,6 +38,7 @@ #include +#include #include #include diff --git a/io/include/pcl/io/image_metadata_wrapper.h b/io/include/pcl/io/image_metadata_wrapper.h index fcc290ff..d6417d32 100644 --- a/io/include/pcl/io/image_metadata_wrapper.h +++ b/io/include/pcl/io/image_metadata_wrapper.h @@ -1,17 +1,17 @@ /* * Software License Agreement (BSD License) - * + * * Point Cloud Library (PCL) - www.pointclouds.org * Copyright (c) 2009-2012, Willow Garage, Inc. * Copyright (c) 2012-, Open Perception, Inc. * Copyright (c) 2014, respective authors. - * + * * 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 @@ -21,7 +21,7 @@ * * 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 @@ -39,8 +39,8 @@ #pragma once +#include #include -#include namespace pcl { @@ -81,3 +81,4 @@ namespace pcl } // namespace } + diff --git a/io/include/pcl/io/impl/ascii_io.hpp b/io/include/pcl/io/impl/ascii_io.hpp index ba314145..b92960c2 100644 --- a/io/include/pcl/io/impl/ascii_io.hpp +++ b/io/include/pcl/io/impl/ascii_io.hpp @@ -35,11 +35,16 @@ * */ + #ifndef PCL_IO_ASCII_IO_HPP_ #define PCL_IO_ASCII_IO_HPP_ + +namespace pcl +{ + template void -pcl::ASCIIReader::setInputFields () +ASCIIReader::setInputFields () { fields_ = pcl::getFields (); @@ -55,5 +60,6 @@ pcl::ASCIIReader::setInputFields () } } +} // namespace pcl #endif //PCL_IO_ASCII_IO_HPP_ diff --git a/io/include/pcl/io/impl/buffers.hpp b/io/include/pcl/io/impl/buffers.hpp index a1645524..4a90249f 100644 --- a/io/include/pcl/io/impl/buffers.hpp +++ b/io/include/pcl/io/impl/buffers.hpp @@ -43,6 +43,7 @@ #include + template struct buffer_traits { @@ -64,38 +65,45 @@ struct buffer_traits static bool is_invalid (double value) { return std::isnan (value); }; }; + +namespace pcl +{ + +namespace io +{ + template -pcl::io::Buffer::Buffer (std::size_t size) +Buffer::Buffer (std::size_t size) : size_ (size) { } template -pcl::io::Buffer::~Buffer () +Buffer::~Buffer () { } template -pcl::io::SingleBuffer::SingleBuffer (std::size_t size) +SingleBuffer::SingleBuffer (std::size_t size) : Buffer (size) , data_ (size, buffer_traits::invalid ()) { } template -pcl::io::SingleBuffer::~SingleBuffer () +SingleBuffer::~SingleBuffer () { } template T -pcl::io::SingleBuffer::operator[] (std::size_t idx) const +SingleBuffer::operator[] (std::size_t idx) const { assert (idx < size_); return (data_[idx]); } template void -pcl::io::SingleBuffer::push (std::vector& data) +SingleBuffer::push (std::vector& data) { assert (data.size () == size_); std::lock_guard lock (data_mutex_); @@ -104,8 +112,8 @@ pcl::io::SingleBuffer::push (std::vector& data) } template -pcl::io::MedianBuffer::MedianBuffer (std::size_t size, - unsigned char window_size) +MedianBuffer::MedianBuffer (std::size_t size, + unsigned char window_size) : Buffer (size) , window_size_ (window_size) , midpoint_ (window_size_ / 2) @@ -130,12 +138,12 @@ pcl::io::MedianBuffer::MedianBuffer (std::size_t size, } template -pcl::io::MedianBuffer::~MedianBuffer () +MedianBuffer::~MedianBuffer () { } template T -pcl::io::MedianBuffer::operator[] (std::size_t idx) const +MedianBuffer::operator[] (std::size_t idx) const { assert (idx < size_); int midpoint = (window_size_ - data_invalid_count_[idx]) / 2; @@ -143,7 +151,7 @@ pcl::io::MedianBuffer::operator[] (std::size_t idx) const } template void -pcl::io::MedianBuffer::push (std::vector& data) +MedianBuffer::push (std::vector& data) { assert (data.size () == size_); std::lock_guard lock (data_mutex_); @@ -206,7 +214,7 @@ pcl::io::MedianBuffer::push (std::vector& data) } template int -pcl::io::MedianBuffer::compare (T a, T b) +MedianBuffer::compare (T a, T b) { bool a_is_invalid = buffer_traits::is_invalid (a); bool b_is_invalid = buffer_traits::is_invalid (b); @@ -222,8 +230,8 @@ pcl::io::MedianBuffer::compare (T a, T b) } template -pcl::io::AverageBuffer::AverageBuffer (std::size_t size, - unsigned char window_size) +AverageBuffer::AverageBuffer (std::size_t size, + unsigned char window_size) : Buffer (size) , window_size_ (window_size) , data_current_idx_ (window_size_ - 1) @@ -240,12 +248,12 @@ pcl::io::AverageBuffer::AverageBuffer (std::size_t size, } template -pcl::io::AverageBuffer::~AverageBuffer () +AverageBuffer::~AverageBuffer () { } template T -pcl::io::AverageBuffer::operator[] (std::size_t idx) const +AverageBuffer::operator[] (std::size_t idx) const { assert (idx < size_); if (data_invalid_count_[idx] == window_size_) @@ -254,7 +262,7 @@ pcl::io::AverageBuffer::operator[] (std::size_t idx) const } template void -pcl::io::AverageBuffer::push (std::vector& data) +AverageBuffer::push (std::vector& data) { assert (data.size () == size_); std::lock_guard lock (data_mutex_); @@ -288,5 +296,8 @@ pcl::io::AverageBuffer::push (std::vector& data) data.clear (); } +} // namespace io +} // namespace pcl + #endif /* PCL_IO_IMPL_BUFFERS_HPP */ diff --git a/io/include/pcl/io/impl/lzf_image_io.hpp b/io/include/pcl/io/impl/lzf_image_io.hpp index fe8360dc..0750eab8 100644 --- a/io/include/pcl/io/impl/lzf_image_io.hpp +++ b/io/include/pcl/io/impl/lzf_image_io.hpp @@ -49,9 +49,15 @@ #define CLIP_CHAR(c) static_cast ((c)>255?255:(c)<0?0:(c)) -////////////////////////////////////////////////////////////////////////////// + +namespace pcl +{ + +namespace io +{ + template bool -pcl::io::LZFDepth16ImageReader::read ( +LZFDepth16ImageReader::read ( const std::string &filename, pcl::PointCloud &cloud) { std::uint32_t uncompressed_size; @@ -100,9 +106,9 @@ pcl::io::LZFDepth16ImageReader::read ( } pt.z = static_cast (val * z_multiplication_factor_); - pt.x = (static_cast (u) - static_cast (parameters_.principal_point_x)) + pt.x = (static_cast (u) - static_cast (parameters_.principal_point_x)) * pt.z * static_cast (constant_x); - pt.y = (static_cast (v) - static_cast (parameters_.principal_point_y)) + pt.y = (static_cast (v) - static_cast (parameters_.principal_point_y)) * pt.z * static_cast (constant_y); } } @@ -113,12 +119,12 @@ pcl::io::LZFDepth16ImageReader::read ( cloud.sensor_orientation_.z () = 0.0f; return (true); } - -/////////////////////////////////////////////////////////////////////////////// + + template bool -pcl::io::LZFDepth16ImageReader::readOMP (const std::string &filename, - pcl::PointCloud &cloud, - unsigned int num_threads) +LZFDepth16ImageReader::readOMP (const std::string &filename, + pcl::PointCloud &cloud, + unsigned int num_threads) { std::uint32_t uncompressed_size; std::vector compressed_data; @@ -151,7 +157,10 @@ pcl::io::LZFDepth16ImageReader::readOMP (const std::string &filename, double constant_x = 1.0 / parameters_.focal_length_x, constant_y = 1.0 / parameters_.focal_length_y; #ifdef _OPENMP -#pragma omp parallel for num_threads (num_threads) +#pragma omp parallel for \ + default(none) \ + shared(cloud, constant_x, constant_y, uncompressed_data) \ + num_threads(num_threads) #else (void) num_threads; // suppress warning if OMP is not present #endif @@ -168,13 +177,11 @@ pcl::io::LZFDepth16ImageReader::readOMP (const std::string &filename, pt.x = pt.y = pt.z = std::numeric_limits::quiet_NaN (); if (cloud.is_dense) { -#ifdef _OPENMP #pragma omp critical -#endif - { - if (cloud.is_dense) - cloud.is_dense = false; - } + { + if (cloud.is_dense) + cloud.is_dense = false; + } } continue; } @@ -184,20 +191,19 @@ pcl::io::LZFDepth16ImageReader::readOMP (const std::string &filename, * pt.z * static_cast (constant_x); pt.y = (static_cast (v) - static_cast (parameters_.principal_point_y)) * pt.z * static_cast (constant_y); - } + cloud.sensor_origin_.setZero (); cloud.sensor_orientation_.w () = 1.0f; cloud.sensor_orientation_.x () = 0.0f; cloud.sensor_orientation_.y () = 0.0f; cloud.sensor_orientation_.z () = 0.0f; return (true); - } -////////////////////////////////////////////////////////////////////////////// + template bool -pcl::io::LZFRGB24ImageReader::read ( +LZFRGB24ImageReader::read ( const std::string &filename, pcl::PointCloud &cloud) { std::uint32_t uncompressed_size; @@ -244,9 +250,9 @@ pcl::io::LZFRGB24ImageReader::read ( return (true); } -////////////////////////////////////////////////////////////////////////////// + template bool -pcl::io::LZFRGB24ImageReader::readOMP ( +LZFRGB24ImageReader::readOMP ( const std::string &filename, pcl::PointCloud &cloud, unsigned int num_threads) { std::uint32_t uncompressed_size; @@ -282,7 +288,10 @@ pcl::io::LZFRGB24ImageReader::readOMP ( unsigned char *color_b = reinterpret_cast (&uncompressed_data[2 * getWidth () * getHeight ()]); #ifdef _OPENMP -#pragma omp parallel for num_threads (num_threads) +#pragma omp parallel for \ + default(none) \ + shared(cloud, color_b, color_g, color_r) \ + num_threads(num_threads) #else (void) num_threads; // suppress warning if OMP is not present #endif//_OPENMP @@ -297,9 +306,9 @@ pcl::io::LZFRGB24ImageReader::readOMP ( return (true); } -////////////////////////////////////////////////////////////////////////////// + template bool -pcl::io::LZFYUV422ImageReader::read ( +LZFYUV422ImageReader::read ( const std::string &filename, pcl::PointCloud &cloud) { std::uint32_t uncompressed_size; @@ -334,7 +343,7 @@ pcl::io::LZFYUV422ImageReader::read ( unsigned char *color_u = reinterpret_cast (&uncompressed_data[0]); unsigned char *color_y = reinterpret_cast (&uncompressed_data[wh2]); unsigned char *color_v = reinterpret_cast (&uncompressed_data[wh2 + getWidth () * getHeight ()]); - + int y_idx = 0; for (int i = 0; i < wh2; ++i, y_idx += 2) { @@ -355,9 +364,9 @@ pcl::io::LZFYUV422ImageReader::read ( return (true); } -////////////////////////////////////////////////////////////////////////////// + template bool -pcl::io::LZFYUV422ImageReader::readOMP ( +LZFYUV422ImageReader::readOMP ( const std::string &filename, pcl::PointCloud &cloud, unsigned int num_threads) { std::uint32_t uncompressed_size; @@ -392,9 +401,12 @@ pcl::io::LZFYUV422ImageReader::readOMP ( unsigned char *color_u = reinterpret_cast (&uncompressed_data[0]); unsigned char *color_y = reinterpret_cast (&uncompressed_data[wh2]); unsigned char *color_v = reinterpret_cast (&uncompressed_data[wh2 + getWidth () * getHeight ()]); - + #ifdef _OPENMP -#pragma omp parallel for num_threads (num_threads) +#pragma omp parallel for \ + default(none) \ + shared(cloud, color_u, color_v, color_y, wh2) \ + num_threads(num_threads) #else (void) num_threads; //suppress warning if OMP is not present #endif//_OPENMP @@ -418,9 +430,9 @@ pcl::io::LZFYUV422ImageReader::readOMP ( return (true); } -////////////////////////////////////////////////////////////////////////////// + template bool -pcl::io::LZFBayer8ImageReader::read ( +LZFBayer8ImageReader::read ( const std::string &filename, pcl::PointCloud &cloud) { std::uint32_t uncompressed_size; @@ -448,9 +460,9 @@ pcl::io::LZFBayer8ImageReader::read ( // Convert Bayer8 to RGB24 std::vector rgb_buffer (getWidth () * getHeight () * 3); - pcl::io::DeBayer i; - i.debayerEdgeAware (reinterpret_cast (&uncompressed_data[0]), - static_cast (&rgb_buffer[0]), + DeBayer i; + i.debayerEdgeAware (reinterpret_cast (&uncompressed_data[0]), + static_cast (&rgb_buffer[0]), getWidth (), getHeight ()); // Copy to PointT cloud.width = getWidth (); @@ -468,9 +480,9 @@ pcl::io::LZFBayer8ImageReader::read ( return (true); } -////////////////////////////////////////////////////////////////////////////// + template bool -pcl::io::LZFBayer8ImageReader::readOMP ( +LZFBayer8ImageReader::readOMP ( const std::string &filename, pcl::PointCloud &cloud, unsigned int num_threads) { std::uint32_t uncompressed_size; @@ -498,16 +510,18 @@ pcl::io::LZFBayer8ImageReader::readOMP ( // Convert Bayer8 to RGB24 std::vector rgb_buffer (getWidth () * getHeight () * 3); - pcl::io::DeBayer i; - i.debayerEdgeAware (reinterpret_cast (&uncompressed_data[0]), - static_cast (&rgb_buffer[0]), + DeBayer i; + i.debayerEdgeAware (reinterpret_cast (&uncompressed_data[0]), + static_cast (&rgb_buffer[0]), getWidth (), getHeight ()); // Copy to PointT cloud.width = getWidth (); cloud.height = getHeight (); cloud.resize (getWidth () * getHeight ()); #ifdef _OPENMP -#pragma omp parallel for num_threads (num_threads) +#pragma omp parallel for \ + default(none) \ + num_threads(num_threads) #else (void) num_threads; //suppress warning if OMP is not present #endif//_OPENMP @@ -522,5 +536,8 @@ pcl::io::LZFBayer8ImageReader::readOMP ( return (true); } +} // namespace io +} // namespace pcl + #endif //#ifndef PCL_LZF_IMAGE_IO_HPP_ diff --git a/io/include/pcl/io/impl/point_cloud_image_extractors.hpp b/io/include/pcl/io/impl/point_cloud_image_extractors.hpp index 526b3a8a..17e58384 100644 --- a/io/include/pcl/io/impl/point_cloud_image_extractors.hpp +++ b/io/include/pcl/io/impl/point_cloud_image_extractors.hpp @@ -35,8 +35,7 @@ * */ -#ifndef PCL_POINT_CLOUD_IMAGE_EXTRACTORS_IMPL_HPP_ -#define PCL_POINT_CLOUD_IMAGE_EXTRACTORS_IMPL_HPP_ +#pragma once #include #include @@ -45,6 +44,7 @@ #include #include +#include // for pcl::isFinite /////////////////////////////////////////////////////////////////////////////////////////// template bool @@ -296,5 +296,3 @@ pcl::io::PointCloudImageExtractorWithScaling::extractImpl (const PointCl return (true); } -#endif // PCL_POINT_CLOUD_IMAGE_EXTRACTORS_IMPL_HPP_ - diff --git a/io/include/pcl/io/impl/vtk_lib_io.hpp b/io/include/pcl/io/impl/vtk_lib_io.hpp index 19b7c88a..59beb8be 100644 --- a/io/include/pcl/io/impl/vtk_lib_io.hpp +++ b/io/include/pcl/io/impl/vtk_lib_io.hpp @@ -37,14 +37,14 @@ * */ -#ifndef PCL_IO_VTK_IO_IMPL_H_ -#define PCL_IO_VTK_IO_IMPL_H_ +#pragma once // PCL #include +#include // for pcl::isFinite #include #include -#include +#include // VTK // Ignore warnings in the above headers @@ -512,5 +512,3 @@ pcl::io::pointCloudTovtkStructuredGrid (const pcl::PointCloud& cloud, vt #undef GetTupleValue #endif -#endif //#ifndef PCL_IO_VTK_IO_H_ - diff --git a/io/include/pcl/io/obj_io.h b/io/include/pcl/io/obj_io.h index a9ca88c0..144a9832 100644 --- a/io/include/pcl/io/obj_io.h +++ b/io/include/pcl/io/obj_io.h @@ -36,6 +36,7 @@ #pragma once +#include #include #include #include diff --git a/io/include/pcl/io/oni_grabber.h b/io/include/pcl/io/oni_grabber.h index 877ff809..8ab4d59b 100644 --- a/io/include/pcl/io/oni_grabber.h +++ b/io/include/pcl/io/oni_grabber.h @@ -37,6 +37,7 @@ #pragma once +#include #include #include @@ -93,29 +94,29 @@ namespace pcl /** \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. */ - void + void start () override; /** \brief For devices that are streaming, the streams are stopped. * This method has no effect for triggered devices. */ - void + void stop () override; /** \brief returns the name of the concrete subclass. * \return the name of the concrete driver. */ - std::string + std::string getName () const override; /** \brief Indicates whether the grabber is streaming or not. This value is not defined for triggered devices. * \return true if grabber is running / streaming. False otherwise. */ - bool + bool isRunning () const override; /** \brief returns the frames pre second. 0 if it is trigger based. */ - float + float getFramesPerSecond () const override; /** \brief Check if there is any data left in the ONI file to process. */ diff --git a/io/include/pcl/io/openni2/openni2_device.h b/io/include/pcl/io/openni2/openni2_device.h index 0336500f..dd462785 100644 --- a/io/include/pcl/io/openni2/openni2_device.h +++ b/io/include/pcl/io/openni2/openni2_device.h @@ -30,18 +30,19 @@ #pragma once +#include #include -#include "openni.h" -#include "pcl/io/openni2/openni2_video_mode.h" -#include "pcl/io/io_exception.h" - -#include // Template frame wrappers #include #include #include +#include +#include + +#include "openni.h" + #include #include #include @@ -49,7 +50,6 @@ #include - namespace openni { class Device; diff --git a/io/include/pcl/io/openni2/openni2_device_manager.h b/io/include/pcl/io/openni2/openni2_device_manager.h index c0678e8b..18381008 100644 --- a/io/include/pcl/io/openni2/openni2_device_manager.h +++ b/io/include/pcl/io/openni2/openni2_device_manager.h @@ -31,7 +31,7 @@ #pragma once -#include +#include #include #include #include @@ -80,7 +80,7 @@ namespace pcl getDevice (const std::string& device_URI); OpenNI2Device::Ptr - getDeviceByIndex (int index); + getDeviceByIndex (int index) const; OpenNI2Device::Ptr getFileDevice (const std::string& path); diff --git a/io/include/pcl/io/openni2_grabber.h b/io/include/pcl/io/openni2_grabber.h index 83a2aab9..edb614e7 100644 --- a/io/include/pcl/io/openni2_grabber.h +++ b/io/include/pcl/io/openni2_grabber.h @@ -39,6 +39,7 @@ #pragma once +#include #include #include 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 66e54403..46a702df 100644 --- a/io/include/pcl/io/openni_camera/openni_depth_image.h +++ b/io/include/pcl/io/openni_camera/openni_depth_image.h @@ -39,7 +39,7 @@ #pragma once #include -#include +#include #ifdef HAVE_OPENNI #include "openni.h" diff --git a/io/include/pcl/io/openni_camera/openni_device.h b/io/include/pcl/io/openni_camera/openni_device.h index afdc35b2..b83017f5 100644 --- a/io/include/pcl/io/openni_camera/openni_device.h +++ b/io/include/pcl/io/openni_camera/openni_device.h @@ -38,7 +38,7 @@ #pragma once #include -#include +#include #ifdef HAVE_OPENNI #include "openni_exception.h" 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 c719c7ea..549fae20 100644 --- a/io/include/pcl/io/openni_camera/openni_device_kinect.h +++ b/io/include/pcl/io/openni_camera/openni_device_kinect.h @@ -37,7 +37,7 @@ #pragma once #include -#include +#include #ifdef HAVE_OPENNI #include "openni_device.h" 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 913ec8ef..99dbb9dc 100644 --- a/io/include/pcl/io/openni_camera/openni_device_oni.h +++ b/io/include/pcl/io/openni_camera/openni_device_oni.h @@ -37,7 +37,7 @@ #pragma once #include -#include +#include #ifdef HAVE_OPENNI #include "openni_device.h" 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 08b19df3..0ce07044 100644 --- a/io/include/pcl/io/openni_camera/openni_device_primesense.h +++ b/io/include/pcl/io/openni_camera/openni_device_primesense.h @@ -37,7 +37,7 @@ #pragma once #include -#include +#include #ifdef HAVE_OPENNI #include "openni_device.h" 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 ec210275..ea0735c4 100644 --- a/io/include/pcl/io/openni_camera/openni_device_xtion.h +++ b/io/include/pcl/io/openni_camera/openni_device_xtion.h @@ -37,7 +37,7 @@ #pragma once #include -#include +#include #ifdef HAVE_OPENNI #include "openni_device.h" diff --git a/io/include/pcl/io/openni_camera/openni_driver.h b/io/include/pcl/io/openni_camera/openni_driver.h index 6ece4940..747461e1 100644 --- a/io/include/pcl/io/openni_camera/openni_driver.h +++ b/io/include/pcl/io/openni_camera/openni_driver.h @@ -42,7 +42,8 @@ #include "openni.h" #include "openni_exception.h" #include "openni_device.h" -#include + +#include // for pcl::weak_ptr #include #include @@ -217,7 +218,7 @@ namespace openni_wrapper std::shared_ptr image_node; std::shared_ptr depth_node; std::shared_ptr ir_node; - boost::weak_ptr device; + pcl::weak_ptr device; } ; OpenNIDriver (); @@ -248,5 +249,8 @@ namespace openni_wrapper { return static_cast (device_context_.size ()); } -} // namespace + +} // namespace openni_wrapper + #endif + diff --git a/io/include/pcl/io/openni_camera/openni_image.h b/io/include/pcl/io/openni_camera/openni_image.h index df8c9318..17917229 100644 --- a/io/include/pcl/io/openni_camera/openni_image.h +++ b/io/include/pcl/io/openni_camera/openni_image.h @@ -37,7 +37,7 @@ #pragma once #include -#include +#include #ifdef HAVE_OPENNI #include 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 ca6803a5..ce613035 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 @@ -39,7 +39,7 @@ #pragma once #include -#include +#include #ifdef HAVE_OPENNI #include 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 0ee267ad..ad17143f 100644 --- a/io/include/pcl/io/openni_camera/openni_image_rgb24.h +++ b/io/include/pcl/io/openni_camera/openni_image_rgb24.h @@ -37,7 +37,7 @@ #pragma once #include -#include +#include #ifdef HAVE_OPENNI #include "openni_image.h" 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 f7d5208a..ea9e382e 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 @@ -37,7 +37,7 @@ #pragma once #include -#include +#include #ifdef HAVE_OPENNI #include 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 fac253d5..4f32f0e9 100644 --- a/io/include/pcl/io/openni_camera/openni_ir_image.h +++ b/io/include/pcl/io/openni_camera/openni_ir_image.h @@ -37,7 +37,7 @@ #define __OPENNI_IR_IMAGE__ #include -#include +#include #include "openni.h" #include "openni_exception.h" #include diff --git a/io/include/pcl/io/openni_grabber.h b/io/include/pcl/io/openni_grabber.h index bc16916b..4f8a8ca7 100644 --- a/io/include/pcl/io/openni_grabber.h +++ b/io/include/pcl/io/openni_grabber.h @@ -38,6 +38,7 @@ #pragma once +#include #include #include @@ -127,10 +128,10 @@ namespace pcl getName () const override; /** \brief Obtain the number of frames per second (FPS). */ - float + float getFramesPerSecond () const override; - /** \brief Get a boost shared pointer to the \ref pcl::openni_wrapper::OpenNIDevice object. */ + /** \brief Get a pcl::shared pointer to the openni_wrapper::OpenNIDevice object. */ inline openni_wrapper::OpenNIDevice::Ptr getDevice () const; @@ -151,8 +152,8 @@ namespace pcl * and the grabber will use the default values from the camera instead. */ inline void - setRGBCameraIntrinsics (const double rgb_focal_length_x, - const double rgb_focal_length_y, + setRGBCameraIntrinsics (const double rgb_focal_length_x, + const double rgb_focal_length_y, const double rgb_principal_point_x, const double rgb_principal_point_y) { @@ -161,7 +162,7 @@ namespace pcl rgb_principal_point_x_ = rgb_principal_point_x; rgb_principal_point_y_ = rgb_principal_point_y; } - + /** \brief Get the RGB camera parameters (fx, fy, cx, cy) * \param[out] rgb_focal_length_x the RGB focal length (fx) * \param[out] rgb_focal_length_y the RGB focal length (fy) @@ -169,8 +170,8 @@ namespace pcl * \param[out] rgb_principal_point_y the RGB principal point (cy) */ inline void - getRGBCameraIntrinsics (double &rgb_focal_length_x, - double &rgb_focal_length_y, + getRGBCameraIntrinsics (double &rgb_focal_length_x, + double &rgb_focal_length_y, double &rgb_principal_point_x, double &rgb_principal_point_y) const { @@ -217,7 +218,7 @@ namespace pcl rgb_focal_length_x = rgb_focal_length_x_; rgb_focal_length_y = rgb_focal_length_y_; } - + /** \brief Set the Depth camera parameters (fx, fy, cx, cy) * \param[in] depth_focal_length_x the Depth focal length (fx) * \param[in] depth_focal_length_y the Depth focal length (fy) @@ -227,8 +228,8 @@ namespace pcl * and the grabber will use the default values from the camera instead. */ inline void - setDepthCameraIntrinsics (const double depth_focal_length_x, - const double depth_focal_length_y, + setDepthCameraIntrinsics (const double depth_focal_length_x, + const double depth_focal_length_y, const double depth_principal_point_x, const double depth_principal_point_y) { @@ -237,7 +238,7 @@ namespace pcl depth_principal_point_x_ = depth_principal_point_x; depth_principal_point_y_ = depth_principal_point_y; } - + /** \brief Get the Depth camera parameters (fx, fy, cx, cy) * \param[out] depth_focal_length_x the Depth focal length (fx) * \param[out] depth_focal_length_y the Depth focal length (fy) @@ -245,8 +246,8 @@ namespace pcl * \param[out] depth_principal_point_y the Depth principal point (cy) */ inline void - getDepthCameraIntrinsics (double &depth_focal_length_x, - double &depth_focal_length_y, + getDepthCameraIntrinsics (double &depth_focal_length_x, + double &depth_focal_length_y, double &depth_principal_point_x, double &depth_principal_point_y) const { @@ -266,7 +267,7 @@ namespace pcl { depth_focal_length_x_ = depth_focal_length_y_ = depth_focal_length; } - + /** \brief Set the Depth image focal length * \param[in] depth_focal_length_x the Depth focal length (fx) @@ -427,7 +428,7 @@ namespace pcl unsigned image_height_; unsigned depth_width_; unsigned depth_height_; - + bool image_required_; bool depth_required_; bool ir_required_; diff --git a/io/include/pcl/io/pcd_grabber.h b/io/include/pcl/io/pcd_grabber.h index de546dac..872e1015 100644 --- a/io/include/pcl/io/pcd_grabber.h +++ b/io/include/pcl/io/pcd_grabber.h @@ -44,7 +44,7 @@ #include #include #include -#include +#include #ifdef HAVE_OPENNI #include @@ -62,7 +62,7 @@ namespace pcl */ class PCL_EXPORTS PCDGrabberBase : public Grabber { - public: + public: /** \brief Constructor taking just one PCD file or one TAR file containing multiple PCD files. * \param[in] pcd_file path to the PCD file * \param[in] frames_per_second frames per second. If 0, start() functions like a trigger, publishing the next PCD in the list. @@ -77,66 +77,48 @@ namespace pcl */ PCDGrabberBase (const std::vector& pcd_files, float frames_per_second, bool repeat); - /** \brief Copy constructor. - * \param[in] src the PCD Grabber base object to copy into this - */ - PCDGrabberBase (const PCDGrabberBase &src) : impl_ () - { - *this = src; - } - - /** \brief Copy operator. - * \param[in] src the PCD Grabber base object to copy into this - */ - PCDGrabberBase& - operator = (const PCDGrabberBase &src) - { - impl_ = src.impl_; - return (*this); - } - /** \brief Virtual destructor. */ ~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 + void start () override; - + /** \brief Stops playing the list of PCD files if frames_per_second is > 0. Otherwise the method has no effect. */ - void + void stop () override; - + /** \brief Triggers a callback with new data */ - virtual void + virtual void trigger (); /** \brief Indicates whether the grabber is streaming or not. * \return true if grabber is started and hasn't run out of PCD files. */ - bool + bool isRunning () const override; - + /** \return The name of the grabber */ - std::string + std::string getName () const override; - + /** \brief Rewinds to the first PCD file in the list.*/ - virtual void + virtual void rewind (); /** \brief Returns the frames_per_second. 0 if grabber is trigger-based */ - float + float getFramesPerSecond () const override; /** \brief Returns whether the repeat flag is on */ - bool + bool isRepeatOn () const; - + /** \brief Get cloud (in ROS form) at a particular location */ bool - getCloudAt (std::size_t idx, + getCloudAt (std::size_t idx, pcl::PCLPointCloud2 &blob, - Eigen::Vector4f &origin, + Eigen::Vector4f &origin, Eigen::Quaternionf &orientation) const; /** \brief Returns the size */ @@ -144,7 +126,7 @@ namespace pcl numFrames () const; private: - virtual void + virtual void publish (const pcl::PCLPointCloud2& blob, const Eigen::Vector4f& origin, const Eigen::Quaternionf& orientation, const std::string& file_name) const = 0; // to separate and hide the implementation from interface: PIMPL @@ -162,13 +144,13 @@ namespace pcl PCDGrabber (const std::string& pcd_path, float frames_per_second = 0, bool repeat = false); PCDGrabber (const std::vector& pcd_files, float frames_per_second = 0, bool repeat = false); - + /** \brief Virtual destructor. */ ~PCDGrabber () noexcept { stop (); } - + // Inherited from FileGrabber const typename pcl::PointCloud::ConstPtr operator[] (std::size_t idx) const override; @@ -178,9 +160,9 @@ namespace pcl size () const override; protected: - void + void publish (const pcl::PCLPointCloud2& blob, const Eigen::Vector4f& origin, const Eigen::Quaternionf& orientation, const std::string& file_name) const override; - + boost::signals2::signal::ConstPtr&)>* signal_; boost::signals2::signal* file_name_signal_; @@ -242,7 +224,7 @@ namespace pcl } //////////////////////////////////////////////////////////////////////////////////////////////////////////////// - template void + template void PCDGrabber::publish (const pcl::PCLPointCloud2& blob, const Eigen::Vector4f& origin, const Eigen::Quaternionf& orientation, const std::string& file_name) const { typename pcl::PointCloud::Ptr cloud (new pcl::PointCloud ()); @@ -304,7 +286,7 @@ namespace pcl openni_wrapper::Image::Ptr image (new openni_wrapper::ImageRGB24 (image_meta_data)); if (image_signal_->num_slots() > 0) image_signal_->operator()(image); - + if (image_depth_image_signal_->num_slots() > 0) image_depth_image_signal_->operator()(image, depth_image, 1.0f / 525.0f); } diff --git a/io/include/pcl/io/pcd_io.h b/io/include/pcl/io/pcd_io.h index 881542ba..e3b58fef 100644 --- a/io/include/pcl/io/pcd_io.h +++ b/io/include/pcl/io/pcd_io.h @@ -39,6 +39,7 @@ #pragma once +#include #include #include #include @@ -69,7 +70,7 @@ namespace pcl * - HEIGHT ... * - POINTS ... * - DATA ascii/binary - * + * * Everything that follows \b DATA is interpreted as data points and * will be read accordingly. * @@ -100,19 +101,19 @@ namespace pcl * \param[out] origin the sensor acquisition origin (only for > PCD_V7 - null if not present) * \param[out] orientation the sensor acquisition orientation (only for > PCD_V7 - identity if not present) * \param[out] pcd_version the PCD version of the file (i.e., PCD_V6, PCD_V7) - * \param[out] data_type the type of data (0 = ASCII, 1 = Binary, 2 = Binary compressed) + * \param[out] data_type the type of data (0 = ASCII, 1 = Binary, 2 = Binary compressed) * \param[out] data_idx the offset of cloud data within the file * * \return * * < 0 (-1) on error * * == 0 on success */ - int + int readHeader (std::istream &binary_istream, pcl::PCLPointCloud2 &cloud, Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int &pcd_version, int &data_type, unsigned int &data_idx); - /** \brief Read a point cloud data header from a PCD file. + /** \brief Read a point cloud data header from a PCD file. * * Load only the meta information (number of points, their types, etc), * and not the points themselves, from a given PCD file. Useful for fast @@ -128,7 +129,7 @@ namespace pcl * \param[out] origin the sensor acquisition origin (only for > PCD_V7 - null if not present) * \param[out] orientation the sensor acquisition orientation (only for > PCD_V7 - identity if not present) * \param[out] pcd_version the PCD version of the file (i.e., PCD_V6, PCD_V7) - * \param[out] data_type the type of data (0 = ASCII, 1 = Binary, 2 = Binary compressed) + * \param[out] data_type the type of data (0 = ASCII, 1 = Binary, 2 = Binary compressed) * \param[out] data_idx the offset of cloud data within the file * \param[in] offset the offset of where to expect the PCD Header in the * file (optional parameter). One usage example for setting the offset @@ -141,13 +142,13 @@ namespace pcl * * < 0 (-1) on error * * == 0 on success */ - int + int readHeader (const std::string &file_name, pcl::PCLPointCloud2 &cloud, Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int &pcd_version, int &data_type, unsigned int &data_idx, const int offset = 0) override; - /** \brief Read a point cloud data header from a PCD file. + /** \brief Read a point cloud data header from a PCD file. * * Load only the meta information (number of points, their types, etc), * and not the points themselves, from a given PCD file. Useful for fast @@ -171,10 +172,10 @@ namespace pcl * * < 0 (-1) on error * * == 0 on success */ - int + int readHeader (const std::string &file_name, pcl::PCLPointCloud2 &cloud, const int offset = 0); - /** \brief Read the point cloud data (body) from a PCD stream. + /** \brief Read the point cloud data (body) from a PCD stream. * * Reads the cloud points from a text-formatted stream. For use after * readHeader(), when the resulting data_type == 0. @@ -193,7 +194,7 @@ namespace pcl int readBodyASCII (std::istream &stream, pcl::PCLPointCloud2 &cloud, int pcd_version); - /** \brief Read the point cloud data (body) from a block of memory. + /** \brief Read the point cloud data (body) from a block of memory. * * Reads the cloud points from a binary-formatted memory block. For use * after readHeader(), when the resulting data_type is nonzero. @@ -230,16 +231,16 @@ namespace pcl * * < 0 (-1) on error * * == 0 on success */ - int + int read (const std::string &file_name, pcl::PCLPointCloud2 &cloud, Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int &pcd_version, const int offset = 0) override; /** \brief Read a point cloud data from a PCD (PCD_V6) and store it into a pcl/PCLPointCloud2. - * + * * \note This function is provided for backwards compatibility only and * it can only read PCD_V6 files correctly, as pcl::PCLPointCloud2 - * does not contain a sensor origin/orientation. Reading any file - * > PCD_V6 will generate a warning. + * does not contain a sensor origin/orientation. Reading any file + * > PCD_V6 will generate a warning. * * \param[in] file_name the name of the file containing the actual PointCloud data * \param[out] cloud the resultant PointCloud message read from disk @@ -254,7 +255,7 @@ namespace pcl * * < 0 (-1) on error * * == 0 on success */ - int + int read (const std::string &file_name, pcl::PCLPointCloud2 &cloud, const int offset = 0); /** \brief Read a point cloud data from any PCD file, and convert it to the given template format. @@ -276,7 +277,7 @@ namespace pcl { pcl::PCLPointCloud2 blob; int pcd_version; - int res = read (file_name, blob, cloud.sensor_origin_, cloud.sensor_orientation_, + int res = read (file_name, blob, cloud.sensor_origin_, cloud.sensor_orientation_, pcd_version, offset); // If no error, convert the data @@ -298,7 +299,7 @@ namespace pcl PCDWriter() : map_synchronization_(false) {} ~PCDWriter() {} - /** \brief Set whether mmap() synchornization via msync() is desired before munmap() calls. + /** \brief Set whether mmap() synchornization via msync() is desired before munmap() calls. * Setting this to true could prevent NFS data loss (see * http://www.pcl-developers.org/PCD-IO-consistency-on-NFS-msync-needed-td4885942.html). * Default: false @@ -319,7 +320,7 @@ namespace pcl */ std::string generateHeaderBinary (const pcl::PCLPointCloud2 &cloud, - const Eigen::Vector4f &origin, + const Eigen::Vector4f &origin, const Eigen::Quaternionf &orientation); /** \brief Generate the header of a BINARY_COMPRESSED PCD file format @@ -335,18 +336,17 @@ namespace pcl int generateHeaderBinaryCompressed (std::ostream &os, const pcl::PCLPointCloud2 &cloud, - const Eigen::Vector4f &origin, + const Eigen::Vector4f &origin, const Eigen::Quaternionf &orientation); /** \brief Generate the header of a BINARY_COMPRESSED PCD file format - * \param[out] os the stream into which to write the header * \param[in] cloud the point cloud data message * \param[in] origin the sensor acquisition origin * \param[in] orientation the sensor acquisition orientation */ std::string generateHeaderBinaryCompressed (const pcl::PCLPointCloud2 &cloud, - const Eigen::Vector4f &origin, + const Eigen::Vector4f &origin, const Eigen::Quaternionf &orientation); /** \brief Generate the header of a PCD file format @@ -356,7 +356,7 @@ namespace pcl */ std::string generateHeaderASCII (const pcl::PCLPointCloud2 &cloud, - const Eigen::Vector4f &origin, + const Eigen::Vector4f &origin, const Eigen::Quaternionf &orientation); /** \brief Generate the header of a PCD file format @@ -365,7 +365,7 @@ namespace pcl * By default, nr_points is set to INTMAX, and the data in the header is used instead. */ template static std::string - generateHeader (const pcl::PointCloud &cloud, + generateHeader (const pcl::PointCloud &cloud, const int nr_points = std::numeric_limits::max ()); /** \brief Save point cloud data to a PCD file containing n-D points, in ASCII format @@ -384,9 +384,9 @@ namespace pcl * * As an intermediary solution, precision 8 is used, which guarantees lossless storage for RGB. */ - int + int writeASCII (const std::string &file_name, const pcl::PCLPointCloud2 &cloud, - const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), + const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (), const int precision = 8); @@ -396,9 +396,9 @@ namespace pcl * \param[in] origin the sensor acquisition origin * \param[in] orientation the sensor acquisition orientation */ - int + int writeBinary (const std::string &file_name, const pcl::PCLPointCloud2 &cloud, - const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), + const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity ()); /** \brief Save point cloud data to a PCD file containing n-D points, in BINARY_COMPRESSED format @@ -411,9 +411,9 @@ namespace pcl * (-2) if the input cloud is too large for the file format * 0 on success */ - int + int writeBinaryCompressed (const std::string &file_name, const pcl::PCLPointCloud2 &cloud, - const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), + const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity ()); /** \brief Save point cloud data to a std::ostream containing n-D points, in BINARY_COMPRESSED format @@ -450,7 +450,7 @@ namespace pcl */ inline int write (const std::string &file_name, const pcl::PCLPointCloud2 &cloud, - const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), + const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (), const bool binary = false) override { @@ -462,7 +462,7 @@ namespace pcl /** \brief Save point cloud data to a PCD file containing n-D points * \param[in] file_name the output file name * \param[in] cloud the point cloud data message (boost shared pointer) - * \param[in] binary set to true if the file is to be written in a binary PCD format, + * \param[in] binary set to true if the file is to be written in a binary PCD format, * false (default) for ASCII * \param[in] origin the sensor acquisition origin * \param[in] orientation the sensor acquisition orientation @@ -476,7 +476,7 @@ namespace pcl */ inline int write (const std::string &file_name, const pcl::PCLPointCloud2::ConstPtr &cloud, - const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), + const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (), const bool binary = false) { @@ -487,8 +487,8 @@ namespace pcl * \param[in] file_name the output file name * \param[in] cloud the point cloud data message */ - template int - writeBinary (const std::string &file_name, + template int + writeBinary (const std::string &file_name, const pcl::PointCloud &cloud); /** \brief Save point cloud data to a binary comprssed PCD file @@ -499,8 +499,8 @@ namespace pcl * (-2) if the input cloud is too large for the file format * 0 on success */ - template int - writeBinaryCompressed (const std::string &file_name, + template int + writeBinaryCompressed (const std::string &file_name, const pcl::PointCloud &cloud); /** \brief Save point cloud data to a PCD file containing n-D points, in BINARY format @@ -508,9 +508,9 @@ namespace pcl * \param[in] cloud the point cloud data message * \param[in] indices the set of point indices that we want written to disk */ - template int - writeBinary (const std::string &file_name, - const pcl::PointCloud &cloud, + template int + writeBinary (const std::string &file_name, + const pcl::PointCloud &cloud, const std::vector &indices); /** \brief Save point cloud data to a PCD file containing n-D points, in ASCII format @@ -518,8 +518,8 @@ namespace pcl * \param[in] cloud the point cloud data message * \param[in] precision the specified output numeric stream precision (default: 8) */ - template int - writeASCII (const std::string &file_name, + template int + writeASCII (const std::string &file_name, const pcl::PointCloud &cloud, const int precision = 8); @@ -529,8 +529,8 @@ namespace pcl * \param[in] indices the set of point indices that we want written to disk * \param[in] precision the specified output numeric stream precision (default: 8) */ - template int - writeASCII (const std::string &file_name, + template int + writeASCII (const std::string &file_name, const pcl::PointCloud &cloud, const std::vector &indices, const int precision = 8); @@ -549,8 +549,8 @@ namespace pcl * future versions of PCL. */ template inline int - write (const std::string &file_name, - const pcl::PointCloud &cloud, + write (const std::string &file_name, + const pcl::PointCloud &cloud, const bool binary = false) { if (binary) @@ -573,8 +573,8 @@ namespace pcl * future versions of PCL. */ template inline int - write (const std::string &file_name, - const pcl::PointCloud &cloud, + write (const std::string &file_name, + const pcl::PointCloud &cloud, const std::vector &indices, bool binary = false) { @@ -608,7 +608,7 @@ namespace pcl namespace io { /** \brief Load a PCD v.6 file into a templated PointCloud type. - * + * * Any PCD files > v.6 will generate a warning as a * pcl/PCLPointCloud2 message cannot hold the sensor origin. * @@ -616,7 +616,7 @@ namespace pcl * \param[out] cloud the resultant templated point cloud * \ingroup io */ - inline int + inline int loadPCDFile (const std::string &file_name, pcl::PCLPointCloud2 &cloud) { pcl::PCDReader p; @@ -631,7 +631,7 @@ namespace pcl * PCD_V7 - identity if not present) * \ingroup io */ - inline int + inline int loadPCDFile (const std::string &file_name, pcl::PCLPointCloud2 &cloud, Eigen::Vector4f &origin, Eigen::Quaternionf &orientation) { @@ -667,9 +667,9 @@ namespace pcl * future versions of PCL. * \ingroup io */ - inline int + inline int savePCDFile (const std::string &file_name, const pcl::PCLPointCloud2 &cloud, - const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), + const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (), const bool binary_mode = false) { @@ -698,7 +698,7 @@ namespace pcl return (w.write (file_name, cloud, binary_mode)); } - /** + /** * \brief Templated version for saving point cloud data to a PCD file * containing a specific given cloud format. * @@ -721,7 +721,7 @@ namespace pcl return (w.write (file_name, cloud, false)); } - /** + /** * \brief Templated version for saving point cloud data to a PCD file * containing a specific given cloud format. The resulting file will be an uncompressed binary. * @@ -737,7 +737,7 @@ namespace pcl return (w.write (file_name, cloud, true)); } - /** + /** * \brief Templated version for saving point cloud data to a PCD file * containing a specific given cloud format * @@ -755,9 +755,9 @@ namespace pcl * \ingroup io */ template int - savePCDFile (const std::string &file_name, + savePCDFile (const std::string &file_name, const pcl::PointCloud &cloud, - const std::vector &indices, + const std::vector &indices, const bool binary_mode = false) { // Save the data diff --git a/io/include/pcl/io/ply/ply_parser.h b/io/include/pcl/io/ply/ply_parser.h index a689633f..12dd7043 100644 --- a/io/include/pcl/io/ply/ply_parser.h +++ b/io/include/pcl/io/ply/ply_parser.h @@ -40,14 +40,6 @@ #pragma once -#ifdef BUILD_Maintainer -# if defined __GNUC__ -# pragma GCC system_header -# elif defined _MSC_VER -# pragma warning(push, 1) -# endif -#endif - #include #include #include @@ -705,14 +697,3 @@ inline bool pcl::io::ply::ply_parser::parse_list_property (format_type format, s } return (true); } - -#ifdef BUILD_Maintainer -# if defined __GNUC__ -# if __GNUC__ == 4 && __GNUC_MINOR__ > 3 -# pragma GCC diagnostic warning "-Weffc++" -# pragma GCC diagnostic warning "-pedantic" -# endif -# elif defined _MSC_VER -# pragma warning(pop) -# endif -#endif diff --git a/io/include/pcl/io/ply_io.h b/io/include/pcl/io/ply_io.h index 918db541..ca0a618f 100644 --- a/io/include/pcl/io/ply_io.h +++ b/io/include/pcl/io/ply_io.h @@ -39,6 +39,7 @@ #pragma once +#include #include #include #include diff --git a/io/include/pcl/io/pxc_grabber.h b/io/include/pcl/io/pxc_grabber.h deleted file mode 100644 index 245e64f8..00000000 --- a/io/include/pcl/io/pxc_grabber.h +++ /dev/null @@ -1 +0,0 @@ -#error "PXCGrabber was deprecated and removed, please use DepthSenseGrabber instead" diff --git a/io/include/pcl/io/real_sense/real_sense_device_manager.h b/io/include/pcl/io/real_sense/real_sense_device_manager.h index 28d36c44..1691d879 100644 --- a/io/include/pcl/io/real_sense/real_sense_device_manager.h +++ b/io/include/pcl/io/real_sense/real_sense_device_manager.h @@ -37,15 +37,19 @@ #pragma once -#include - #include #include #include -#include +#include // for pcl::shared_ptr, pcl::weak_ptr +#include // for PCL_EXPORTS + +#include // for boost::noncopyable -#include +#include // for std::size_t +#include // for std::lock_guard, std::mutex +#include // for std::string +#include // for std::vector namespace pcl { @@ -104,7 +108,7 @@ namespace pcl pxcUID iuid; pxcI32 didx; std::string serial; - std::weak_ptr device_ptr; + weak_ptr device_ptr; inline bool isCaptured () { return (!device_ptr.expired ()); } }; @@ -132,8 +136,7 @@ namespace pcl { public: - - using Ptr = std::shared_ptr; + using Ptr = pcl::shared_ptr; inline const std::string& getSerialNumber () { return (device_id_); } diff --git a/io/include/pcl/io/real_sense_2_grabber.h b/io/include/pcl/io/real_sense_2_grabber.h index 5a197223..8fe9b1fe 100644 --- a/io/include/pcl/io/real_sense_2_grabber.h +++ b/io/include/pcl/io/real_sense_2_grabber.h @@ -160,7 +160,7 @@ namespace pcl /** \brief template function to convert realsense point cloud to PCL point cloud * \param[in] points - realsense point cloud array - * \param[in] dynamic function to convert individual point color or intensity values + * \param[in] mapColorFunc dynamic function to convert individual point color or intensity values */ template typename pcl::PointCloud::Ptr diff --git a/io/include/pcl/io/real_sense_grabber.h b/io/include/pcl/io/real_sense_grabber.h index 36ac096f..4c2c8d51 100644 --- a/io/include/pcl/io/real_sense_grabber.h +++ b/io/include/pcl/io/real_sense_grabber.h @@ -38,7 +38,7 @@ #pragma once #include -#include +#include #include #include #include diff --git a/io/include/pcl/io/robot_eye_grabber.h b/io/include/pcl/io/robot_eye_grabber.h index 92487f80..546b0d7d 100644 --- a/io/include/pcl/io/robot_eye_grabber.h +++ b/io/include/pcl/io/robot_eye_grabber.h @@ -43,7 +43,7 @@ #include #include #include -#include +#include #include #include diff --git a/io/src/ensenso_grabber.cpp b/io/src/ensenso_grabber.cpp index 6ba3f6b7..63133c38 100644 --- a/io/src/ensenso_grabber.cpp +++ b/io/src/ensenso_grabber.cpp @@ -290,7 +290,7 @@ pcl::EnsensoGrabber::configureCapture (const bool auto_exposure, ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// bool -pcl::EnsensoGrabber::grabSingleCloud (pcl::PointCloud &cloud) +pcl::EnsensoGrabber::grabSingleCloud (pcl::PointCloud &cloud) const { if (!device_open_) return (false); @@ -575,7 +575,7 @@ bool pcl::EnsensoGrabber::setExtrinsicCalibration (const double euler_angle, Eigen::Vector3d &rotation_axis, const Eigen::Vector3d &translation, - const std::string target) + const std::string target) const { if (!device_open_) return (false); diff --git a/io/src/hdl_grabber.cpp b/io/src/hdl_grabber.cpp index 2cd87b59..8e116e51 100644 --- a/io/src/hdl_grabber.cpp +++ b/io/src/hdl_grabber.cpp @@ -396,7 +396,7 @@ void pcl::HDLGrabber::computeXYZI (pcl::PointXYZI& point, std::uint16_t azimuth, HDLLaserReturn laserReturn, - HDLLaserCorrection correction) + HDLLaserCorrection correction) const { double cos_azimuth, sin_azimuth; @@ -507,7 +507,7 @@ pcl::HDLGrabber::start () } hdl_read_socket_ = new udp::socket (hdl_read_socket_service_, udp_listener_endpoint_); } - catch (const std::exception& bind) + catch (const std::exception&) { delete hdl_read_socket_; hdl_read_socket_ = new udp::socket (hdl_read_socket_service_, udp::endpoint (boost::asio::ip::address_v4::any (), udp_listener_endpoint_.port ())); @@ -550,11 +550,8 @@ pcl::HDLGrabber::stop () queue_consumer_thread_ = nullptr; } - if (hdl_read_socket_ != nullptr) - { - delete hdl_read_socket_; - hdl_read_socket_ = nullptr; - } + delete hdl_read_socket_; + hdl_read_socket_ = nullptr; } ///////////////////////////////////////////////////////////////////////////// diff --git a/io/src/image_grabber.cpp b/io/src/image_grabber.cpp index 680e2966..3d23fa1a 100644 --- a/io/src/image_grabber.cpp +++ b/io/src/image_grabber.cpp @@ -39,6 +39,7 @@ #include #include #include +#include #include #include #include @@ -60,33 +61,33 @@ struct pcl::ImageGrabberBase::ImageGrabberImpl { //! Implementation of ImageGrabber - ImageGrabberImpl (pcl::ImageGrabberBase& grabber, - const std::string& dir, - float frames_per_second, - bool repeat, + ImageGrabberImpl (pcl::ImageGrabberBase& grabber, + const std::string& dir, + float frames_per_second, + bool repeat, bool pclzf_mode=false); //! For now, split rgb / depth folders only makes sense for VTK images - ImageGrabberImpl (pcl::ImageGrabberBase& grabber, - const std::string& rgb_dir, - const std::string& depth_dir, - float frames_per_second, + ImageGrabberImpl (pcl::ImageGrabberBase& grabber, + const std::string& rgb_dir, + const std::string& depth_dir, + float frames_per_second, bool repeat); - ImageGrabberImpl (pcl::ImageGrabberBase& grabber, - const std::vector& depth_image_files, - float frames_per_second, + ImageGrabberImpl (pcl::ImageGrabberBase& grabber, + const std::vector& depth_image_files, + float frames_per_second, bool repeat); - - void + + void trigger (); //! Read ahead -- figure out whether we are in VTK image or PCLZF mode - void + void loadNextCloud (); - + //! Get cloud at a particular location bool getCloudAt (std::size_t idx, pcl::PCLPointCloud2 &blob, Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, double &fx, double &fy, double &cx, double &cy) const; - + //! Get cloud at a particular location bool getCloudVTK (std::size_t idx, pcl::PCLPointCloud2 &blob, Eigen::Vector4f &origin, Eigen::Quaternionf &orientation) const; @@ -110,7 +111,7 @@ struct pcl::ImageGrabberBase::ImageGrabberImpl //! True if it is an image we know how to read bool - isValidExtension (const std::string &extension); + isValidExtension (const std::string &extension) const; //! Convenience function to rewind to the last frame void @@ -124,13 +125,13 @@ struct pcl::ImageGrabberBase::ImageGrabberImpl std::size_t numFrames () const; - + #ifdef PCL_BUILT_WITH_VTK //! Load an image file, return the vtkImageReader2, return false if it couldn't be opened bool getVtkImage (const std::string &filename, vtkSmartPointer &image) const; #endif//PCL_BUILT_WITH_VTK - + pcl::ImageGrabberBase& grabber_; float frames_per_second_; bool repeat_; @@ -171,10 +172,10 @@ struct pcl::ImageGrabberBase::ImageGrabberImpl }; /////////////////////////////////////////////////////////////////////////////////////////// -pcl::ImageGrabberBase::ImageGrabberImpl::ImageGrabberImpl (pcl::ImageGrabberBase& grabber, - const std::string& dir, - float frames_per_second, - bool repeat, +pcl::ImageGrabberBase::ImageGrabberImpl::ImageGrabberImpl (pcl::ImageGrabberBase& grabber, + const std::string& dir, + float frames_per_second, + bool repeat, bool pclzf_mode) : grabber_ (grabber) , frames_per_second_ (frames_per_second) @@ -203,10 +204,10 @@ pcl::ImageGrabberBase::ImageGrabberImpl::ImageGrabberImpl (pcl::ImageGrabberBase } /////////////////////////////////////////////////////////////////////////////////////////// -pcl::ImageGrabberBase::ImageGrabberImpl::ImageGrabberImpl (pcl::ImageGrabberBase& grabber, - const std::string& depth_dir, - const std::string& rgb_dir, - float frames_per_second, +pcl::ImageGrabberBase::ImageGrabberImpl::ImageGrabberImpl (pcl::ImageGrabberBase& grabber, + const std::string& depth_dir, + const std::string& rgb_dir, + float frames_per_second, bool repeat) : grabber_ (grabber) , frames_per_second_ (frames_per_second) @@ -228,9 +229,9 @@ pcl::ImageGrabberBase::ImageGrabberImpl::ImageGrabberImpl (pcl::ImageGrabberBase } /////////////////////////////////////////////////////////////////////////////////////////// -pcl::ImageGrabberBase::ImageGrabberImpl::ImageGrabberImpl (pcl::ImageGrabberBase& grabber, - const std::vector& depth_image_files, - float frames_per_second, +pcl::ImageGrabberBase::ImageGrabberImpl::ImageGrabberImpl (pcl::ImageGrabberBase& grabber, + const std::vector& depth_image_files, + float frames_per_second, bool repeat) : grabber_ (grabber) , frames_per_second_ (frames_per_second) @@ -252,7 +253,7 @@ pcl::ImageGrabberBase::ImageGrabberImpl::ImageGrabberImpl (pcl::ImageGrabberBase } /////////////////////////////////////////////////////////////////////////////////////////// -void +void pcl::ImageGrabberBase::ImageGrabberImpl::loadNextCloud () { if (cur_frame_ >= numFrames ()) @@ -265,13 +266,13 @@ pcl::ImageGrabberBase::ImageGrabberImpl::loadNextCloud () return; } } - valid_ = getCloudAt (cur_frame_, next_cloud_, origin_, orientation_, + valid_ = getCloudAt (cur_frame_, next_cloud_, origin_, orientation_, focal_length_x_, focal_length_y_, principal_point_x_, principal_point_y_); cur_frame_++; } /////////////////////////////////////////////////////////////////////////////////////////// -void +void pcl::ImageGrabberBase::ImageGrabberImpl::trigger () { if (valid_) @@ -301,7 +302,7 @@ pcl::ImageGrabberBase::ImageGrabberImpl::loadDepthAndRGBFiles (const std::string extension = boost::algorithm::to_upper_copy (boost::filesystem::extension (itr->path ())); pathname = itr->path ().string (); basename = boost::filesystem::basename (itr->path ()); - if (!boost::filesystem::is_directory (itr->status ()) + if (!boost::filesystem::is_directory (itr->status ()) && isValidExtension (extension)) { if (basename.find ("rgb") < std::string::npos) @@ -400,7 +401,7 @@ pcl::ImageGrabberBase::ImageGrabberImpl::loadPCLZFFiles (const std::string &dir) extension = boost::algorithm::to_upper_copy (boost::filesystem::extension (itr->path ())); pathname = itr->path ().string (); basename = boost::filesystem::basename (itr->path ()); - if (!boost::filesystem::is_directory (itr->status ()) + if (!boost::filesystem::is_directory (itr->status ()) && isValidExtension (extension)) { if (basename.find ("rgb") < std::string::npos) @@ -429,7 +430,7 @@ pcl::ImageGrabberBase::ImageGrabberImpl::loadPCLZFFiles (const std::string &dir) } /////////////////////////////////////////////////////////////////////////////////////////// bool -pcl::ImageGrabberBase::ImageGrabberImpl::isValidExtension (const std::string &extension) +pcl::ImageGrabberBase::ImageGrabberImpl::isValidExtension (const std::string &extension) const { bool valid; if(pclzf_mode_) @@ -438,29 +439,29 @@ pcl::ImageGrabberBase::ImageGrabberImpl::isValidExtension (const std::string &ex } else { - valid = extension == ".TIFF" || extension == ".PNG" + valid = extension == ".TIFF" || extension == ".PNG" || extension == ".JPG" || extension == ".JPEG" || extension == ".PPM"; } return (valid); } - + void pcl::ImageGrabberBase::ImageGrabberImpl::rewindOnce () { if (cur_frame_ > 0) cur_frame_--; } - + ////////////////////////////////////////////////////////////////////////// bool pcl::ImageGrabberBase::ImageGrabberImpl::getTimestampFromFilepath ( - const std::string &filepath, + const std::string &filepath, std::uint64_t ×tamp) const { // For now, we assume the file is of the form frame_[22-char POSIX timestamp]_* char timestamp_str[256]; - int result = std::sscanf (boost::filesystem::basename (filepath).c_str (), + int result = std::sscanf (boost::filesystem::basename (filepath).c_str (), "frame_%22s_%*s", timestamp_str); if (result > 0) @@ -474,16 +475,16 @@ pcl::ImageGrabberBase::ImageGrabberImpl::getTimestampFromFilepath ( } return (false); } - + ///////////////////////////////////////////////////////////////////////////// bool -pcl::ImageGrabberBase::ImageGrabberImpl::getCloudAt (std::size_t idx, +pcl::ImageGrabberBase::ImageGrabberImpl::getCloudAt (std::size_t idx, pcl::PCLPointCloud2 &blob, - Eigen::Vector4f &origin, - Eigen::Quaternionf &orientation, - double &fx, - double &fy, - double &cx, + Eigen::Vector4f &origin, + Eigen::Quaternionf &orientation, + double &fx, + double &fy, + double &cx, double &cy) const { if (!depth_image_files_.empty ()) @@ -501,9 +502,9 @@ pcl::ImageGrabberBase::ImageGrabberImpl::getCloudAt (std::size_t idx, } bool -pcl::ImageGrabberBase::ImageGrabberImpl::getCloudVTK (std::size_t idx, +pcl::ImageGrabberBase::ImageGrabberImpl::getCloudVTK (std::size_t idx, pcl::PCLPointCloud2 &blob, - Eigen::Vector4f &origin, + Eigen::Vector4f &origin, Eigen::Quaternionf &orientation) const { #ifdef PCL_BUILT_WITH_VTK @@ -534,7 +535,7 @@ pcl::ImageGrabberBase::ImageGrabberImpl::getCloudVTK (std::size_t idx, // Fill in image data depth_pixel = static_cast(depth_image->GetScalarPointer ()); - + // Set up intrinsics float scaleFactorX, scaleFactorY; float centerX, centerY; @@ -567,12 +568,12 @@ pcl::ImageGrabberBase::ImageGrabberImpl::getCloudVTK (std::size_t idx, { pcl::PointXYZRGBA &pt = cloud_color.at (x,y); float depth = static_cast (*depth_pixel) * depth_image_units_; - if (depth == 0.0f) + if (depth == 0.0f) pt.x = pt.y = pt.z = std::numeric_limits::quiet_NaN (); else { pt.x = (static_cast (x) - centerX) * scaleFactorX * depth; - pt.y = (static_cast (y) - centerY) * scaleFactorY * depth; + pt.y = (static_cast (y) - centerY) * scaleFactorY * depth; pt.z = depth; } @@ -604,12 +605,12 @@ pcl::ImageGrabberBase::ImageGrabberImpl::getCloudVTK (std::size_t idx, { pcl::PointXYZ &pt = cloud.at (x,y); float depth = static_cast (*depth_pixel) * depth_image_units_; - if (depth == 0.0f) + if (depth == 0.0f) pt.x = pt.y = pt.z = std::numeric_limits::quiet_NaN (); else { pt.x = ((float)x - centerX) * scaleFactorX * depth; - pt.y = ((float)y - centerY) * scaleFactorY * depth; + pt.y = ((float)y - centerY) * scaleFactorY * depth; pt.z = depth; } } @@ -636,13 +637,13 @@ pcl::ImageGrabberBase::ImageGrabberImpl::getCloudVTK (std::size_t idx, } bool -pcl::ImageGrabberBase::ImageGrabberImpl::getCloudPCLZF (std::size_t idx, +pcl::ImageGrabberBase::ImageGrabberImpl::getCloudPCLZF (std::size_t idx, pcl::PCLPointCloud2 &blob, - Eigen::Vector4f &origin, - Eigen::Quaternionf &orientation, - double &fx, - double &fy, - double &cx, + Eigen::Vector4f &origin, + Eigen::Quaternionf &orientation, + double &fx, + double &fy, + double &cx, double &cy) const { if (idx > depth_pclzf_files_.size ()) @@ -671,10 +672,10 @@ pcl::ImageGrabberBase::ImageGrabberImpl::getCloudPCLZF (std::size_t idx, fy = focal_length_y_; cx = principal_point_x_; cy = principal_point_y_; - rgb.setParameters (manual_params); - yuv.setParameters (manual_params); - bayer.setParameters (manual_params); - depth.setParameters (manual_params); + rgb.setParameters (manual_params); + yuv.setParameters (manual_params); + bayer.setParameters (manual_params); + depth.setParameters (manual_params); } else { @@ -729,7 +730,7 @@ pcl::ImageGrabberBase::ImageGrabberImpl::getCloudPCLZF (std::size_t idx, fy = focal_length_y_; cx = principal_point_x_; cy = principal_point_y_; - depth.setParameters (manual_params); + depth.setParameters (manual_params); } else { @@ -759,14 +760,14 @@ pcl::ImageGrabberBase::ImageGrabberImpl::getCloudPCLZF (std::size_t idx, origin = Eigen::Vector4f::Zero (); orientation = Eigen::Quaternionf::Identity (); return (true); -} - +} + //////////////////////////////////////////////////////////////////////// // #ifdef PCL_BUILT_WITH_VTK bool pcl::ImageGrabberBase::ImageGrabberImpl::getVtkImage ( - const std::string &filename, + const std::string &filename, vtkSmartPointer &image) const { @@ -855,7 +856,7 @@ pcl::ImageGrabberBase::~ImageGrabberBase () noexcept } /////////////////////////////////////////////////////////////////////////////////////////// -void +void pcl::ImageGrabberBase::start () { if (impl_->frames_per_second_ > 0) @@ -868,7 +869,7 @@ pcl::ImageGrabberBase::start () } /////////////////////////////////////////////////////////////////////////////////////////// -void +void pcl::ImageGrabberBase::stop () { if (impl_->frames_per_second_ > 0) @@ -888,35 +889,35 @@ pcl::ImageGrabberBase::trigger () } /////////////////////////////////////////////////////////////////////////////////////////// -bool +bool pcl::ImageGrabberBase::isRunning () const { return (impl_->running_); } /////////////////////////////////////////////////////////////////////////////////////////// -std::string +std::string pcl::ImageGrabberBase::getName () const { return ("ImageGrabber"); } /////////////////////////////////////////////////////////////////////////////////////////// -void +void pcl::ImageGrabberBase::rewind () { impl_->cur_frame_ = 0; } /////////////////////////////////////////////////////////////////////////////////////////// -float +float pcl::ImageGrabberBase::getFramesPerSecond () const { return (impl_->frames_per_second_); } /////////////////////////////////////////////////////////////////////////////////////////// -bool +bool pcl::ImageGrabberBase::isRepeatOn () const { return (impl_->repeat_); @@ -924,7 +925,7 @@ pcl::ImageGrabberBase::isRepeatOn () const /////////////////////////////////////////////////////////////////////////////////////////// void -pcl::ImageGrabberBase::setRGBImageFiles (const std::vector& rgb_image_files) +pcl::ImageGrabberBase::setRGBImageFiles (const std::vector& rgb_image_files) { impl_->rgb_image_files_ = rgb_image_files; impl_->cur_frame_ = 0; @@ -933,9 +934,9 @@ pcl::ImageGrabberBase::setRGBImageFiles (const std::vector& rgb_ima /////////////////////////////////////////////////////// void -pcl::ImageGrabberBase::setCameraIntrinsics (const double focal_length_x, - const double focal_length_y, - const double principal_point_x, +pcl::ImageGrabberBase::setCameraIntrinsics (const double focal_length_x, + const double focal_length_y, + const double principal_point_x, const double principal_point_y) { impl_->focal_length_x_ = focal_length_x; @@ -952,9 +953,9 @@ pcl::ImageGrabberBase::setCameraIntrinsics (const double focal_length_x, } void -pcl::ImageGrabberBase::getCameraIntrinsics (double &focal_length_x, - double &focal_length_y, - double &principal_point_x, +pcl::ImageGrabberBase::getCameraIntrinsics (double &focal_length_x, + double &focal_length_y, + double &principal_point_x, double &principal_point_y) const { focal_length_x = impl_->focal_length_x_; @@ -982,7 +983,7 @@ pcl::ImageGrabberBase::numFrames () const bool pcl::ImageGrabberBase::getCloudAt (std::size_t idx, pcl::PCLPointCloud2 &blob, - Eigen::Vector4f &origin, + Eigen::Vector4f &origin, Eigen::Quaternionf &orientation) const { double fx, fy, cx, cy; @@ -1020,7 +1021,7 @@ pcl::ImageGrabberBase::getPrevDepthFileName () const std::string basename = boost::filesystem::basename (pathname); return (basename); } - + ///////////////////////////////////////////////////////////////////////////////////////// std::string pcl::ImageGrabberBase::getDepthFileNameAtIndex (std::size_t idx) const diff --git a/io/src/lzf.cpp b/io/src/lzf.cpp index 3069ec24..5f22614d 100644 --- a/io/src/lzf.cpp +++ b/io/src/lzf.cpp @@ -4,7 +4,7 @@ * Point Cloud Library (PCL) - www.pointclouds.org * Copyright (c) 2000-2010 Marc Alexander Lehmann * Copyright (c) 2010-2011, Willow Garage, Inc. - * + * * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -17,7 +17,7 @@ * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. - * + * * 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 @@ -35,11 +35,13 @@ * */ -#include -#include -#include #include +#include + #include +#include +#include +#include /* * Size of hashtable is (1 << HLOG) * sizeof (char *) @@ -114,7 +116,7 @@ pcl::lzfCompress (const void *const in_data, unsigned int in_len, hval = (hval << 8) | ip[2]; hslot = htab + IDX (hval); - const unsigned char *ref = *hslot + (static_cast (in_data)); + const unsigned char *ref = *hslot + (static_cast (in_data)); *hslot = static_cast (ip - (static_cast (in_data))); // off requires a type wide enough to hold a general pointer difference. @@ -125,14 +127,14 @@ pcl::lzfCompress (const void *const in_data, unsigned int in_len, // special workaround for it. #if defined (WIN32) && defined (_M_X64) && defined (_MSC_VER) // workaround for missing POSIX compliance - unsigned _int64 off; + unsigned _int64 off; #else unsigned long off; #endif if ( // The next test will actually take care of this, but this is faster if htab is initialized - ref < ip + ref < ip && (off = ip - ref - 1) < (1 << 13) && ref > static_cast (in_data) && ref[2] == ip[2] @@ -145,7 +147,7 @@ pcl::lzfCompress (const void *const in_data, unsigned int in_len, { // Match found at *ref++ unsigned int len = 2; - ptrdiff_t maxlen = in_end - ip - len; + std::ptrdiff_t maxlen = in_end - ip - len; maxlen = maxlen > ((1 << 8) + (1 << 3)) ? ((1 << 8) + (1 << 3)) : maxlen; // First a faster conservative test @@ -248,7 +250,7 @@ pcl::lzfCompress (const void *const in_data, unsigned int in_len, } } - // At most 3 bytes can be missing here + // At most 3 bytes can be missing here if (op + 3 > out_end) return (0); @@ -274,7 +276,7 @@ pcl::lzfCompress (const void *const in_data, unsigned int in_len, } /////////////////////////////////////////////////////////////////////////////////////////// -unsigned int +unsigned int pcl::lzfDecompress (const void *const in_data, unsigned int in_len, void *out_data, unsigned int out_len) { diff --git a/io/src/lzf_image_io.cpp b/io/src/lzf_image_io.cpp index 3ef73bf1..58e5f43c 100644 --- a/io/src/lzf_image_io.cpp +++ b/io/src/lzf_image_io.cpp @@ -187,7 +187,7 @@ pcl::io::LZFImageWriter::writeParameter (const double ¶meter, { boost::property_tree::xml_parser::read_xml (filename, pt, boost::property_tree::xml_parser::trim_whitespace); } - catch (std::exception& e) + catch (std::exception&) {} pt.put (tag, parameter); @@ -206,7 +206,7 @@ pcl::io::LZFDepth16ImageWriter::writeParameters (const pcl::io::CameraParameters { boost::property_tree::xml_parser::read_xml (filename, pt, boost::property_tree::xml_parser::trim_whitespace); } - catch (std::exception& e) + catch (std::exception&) {} pt.put ("depth.focal_length_x", parameters.focal_length_x); @@ -266,7 +266,7 @@ pcl::io::LZFRGB24ImageWriter::writeParameters (const pcl::io::CameraParameters & { boost::property_tree::xml_parser::read_xml (filename, pt, boost::property_tree::xml_parser::trim_whitespace); } - catch (std::exception& e) + catch (std::exception&) {} pt.put ("rgb.focal_length_x", parameters.focal_length_x); diff --git a/io/src/obj_io.cpp b/io/src/obj_io.cpp index 7c7c9cbe..223f7163 100644 --- a/io/src/obj_io.cpp +++ b/io/src/obj_io.cpp @@ -573,7 +573,7 @@ pcl::OBJReader::read (const std::string &file_name, pcl::PCLPointCloud2 &cloud, } ++point_idx; } - catch (const boost::bad_lexical_cast &e) + catch (const boost::bad_lexical_cast&) { PCL_ERROR ("Unable to convert %s to vertex coordinates!", line.c_str ()); return (-1); @@ -602,7 +602,7 @@ pcl::OBJReader::read (const std::string &file_name, pcl::PCLPointCloud2 &cloud, } ++normal_idx; } - catch (const boost::bad_lexical_cast &e) + catch (const boost::bad_lexical_cast&) { PCL_ERROR ("Unable to convert line %s to vertex normal!", line.c_str ()); return (-1); @@ -715,7 +715,7 @@ pcl::OBJReader::read (const std::string &file_name, pcl::TextureMesh &mesh, } ++v_idx; } - catch (const boost::bad_lexical_cast &e) + catch (const boost::bad_lexical_cast&) { PCL_ERROR ("Unable to convert %s to vertex coordinates!", line.c_str ()); return (-1); @@ -736,7 +736,7 @@ pcl::OBJReader::read (const std::string &file_name, pcl::TextureMesh &mesh, } ++vn_idx; } - catch (const boost::bad_lexical_cast &e) + catch (const boost::bad_lexical_cast&) { PCL_ERROR ("Unable to convert line %s to vertex normal!", line.c_str ()); return (-1); @@ -757,7 +757,7 @@ pcl::OBJReader::read (const std::string &file_name, pcl::TextureMesh &mesh, coordinates.emplace_back(c[0]/c[2], c[1]/c[2]); ++vt_idx; } - catch (const boost::bad_lexical_cast &e) + catch (const boost::bad_lexical_cast&) { PCL_ERROR ("Unable to convert line %s to texture coordinates!", line.c_str ()); return (-1); @@ -905,7 +905,7 @@ pcl::OBJReader::read (const std::string &file_name, pcl::PolygonMesh &mesh, } ++v_idx; } - catch (const boost::bad_lexical_cast &e) + catch (const boost::bad_lexical_cast&) { PCL_ERROR ("Unable to convert %s to vertex coordinates!", line.c_str ()); return (-1); @@ -927,7 +927,7 @@ pcl::OBJReader::read (const std::string &file_name, pcl::PolygonMesh &mesh, } ++vn_idx; } - catch (const boost::bad_lexical_cast &e) + catch (const boost::bad_lexical_cast&) { PCL_ERROR ("Unable to convert line %s to vertex normal!", line.c_str ()); return (-1); diff --git a/io/src/oni_grabber.cpp b/io/src/oni_grabber.cpp index 42dd2ae6..83789a45 100644 --- a/io/src/oni_grabber.cpp +++ b/io/src/oni_grabber.cpp @@ -42,8 +42,10 @@ #include #include #include -#include +#include // for boost::shared_array +#include // for dynamic_pointer_cast #include + #include namespace @@ -82,7 +84,7 @@ ONIGrabber::ONIGrabber (const std::string& file_name, bool repeat, bool stream) , point_cloud_rgba_signal_ () { openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance (); - device_ = boost::dynamic_pointer_cast< openni_wrapper::DeviceONI> (driver.createVirtualDevice (file_name, repeat, stream)); + device_ = dynamic_pointer_cast< openni_wrapper::DeviceONI> (driver.createVirtualDevice (file_name, repeat, stream)); if (!device_->hasDepthStream ()) PCL_THROW_EXCEPTION (pcl::IOException, "Device does not provide 3D information."); diff --git a/io/src/openni2/openni2_convert.cpp b/io/src/openni2/openni2_convert.cpp index 39f1e712..19416eb3 100644 --- a/io/src/openni2/openni2_convert.cpp +++ b/io/src/openni2/openni2_convert.cpp @@ -29,13 +29,13 @@ * Author: Julius Kammerl (jkammerl@willowgarage.com) */ -#include "pcl/io/openni2/openni2_convert.h" -#include "pcl/io/io_exception.h" - -#include +#include +#include +#include #include + namespace pcl { namespace io diff --git a/io/src/openni2/openni2_device_manager.cpp b/io/src/openni2/openni2_device_manager.cpp index a6dcb90e..f838e2f5 100644 --- a/io/src/openni2/openni2_device_manager.cpp +++ b/io/src/openni2/openni2_device_manager.cpp @@ -33,7 +33,7 @@ #include "pcl/io/openni2/openni2_convert.h" #include "pcl/io/openni2/openni2_device.h" #include "pcl/io/io_exception.h" -#include +#include #include #include @@ -226,7 +226,7 @@ pcl::io::openni2::OpenNI2DeviceManager::getDevice (const std::string& device_URI } OpenNI2Device::Ptr -pcl::io::openni2::OpenNI2DeviceManager::getDeviceByIndex (int index) +pcl::io::openni2::OpenNI2DeviceManager::getDeviceByIndex (int index) const { auto URIs = getConnectedDeviceURIs (); return pcl::make_shared( URIs->at (index) ); diff --git a/io/src/openni_camera/openni_device.cpp b/io/src/openni_camera/openni_device.cpp index a0a816e8..2a22248a 100644 --- a/io/src/openni_camera/openni_device.cpp +++ b/io/src/openni_camera/openni_device.cpp @@ -36,7 +36,7 @@ * */ #include -#include +#include #ifdef HAVE_OPENNI #ifdef __GNUC__ diff --git a/io/src/openni_camera/openni_device_kinect.cpp b/io/src/openni_camera/openni_device_kinect.cpp index 73dbfd08..fee348e6 100644 --- a/io/src/openni_camera/openni_device_kinect.cpp +++ b/io/src/openni_camera/openni_device_kinect.cpp @@ -36,7 +36,7 @@ * */ #include -#include +#include #ifdef HAVE_OPENNI #ifdef __GNUC__ diff --git a/io/src/openni_camera/openni_device_oni.cpp b/io/src/openni_camera/openni_device_oni.cpp index 170518a2..56641b26 100644 --- a/io/src/openni_camera/openni_device_oni.cpp +++ b/io/src/openni_camera/openni_device_oni.cpp @@ -36,7 +36,7 @@ * */ #include -#include +#include #ifdef HAVE_OPENNI #ifdef __GNUC__ diff --git a/io/src/openni_camera/openni_device_primesense.cpp b/io/src/openni_camera/openni_device_primesense.cpp index c0ae9eb6..aca1a8a0 100644 --- a/io/src/openni_camera/openni_device_primesense.cpp +++ b/io/src/openni_camera/openni_device_primesense.cpp @@ -36,7 +36,7 @@ * */ #include -#include +#include #ifdef HAVE_OPENNI #ifdef __GNUC__ diff --git a/io/src/openni_camera/openni_device_xtion.cpp b/io/src/openni_camera/openni_device_xtion.cpp index 638b6a43..cd06198c 100644 --- a/io/src/openni_camera/openni_device_xtion.cpp +++ b/io/src/openni_camera/openni_device_xtion.cpp @@ -36,7 +36,7 @@ * */ #include -#include +#include #ifdef HAVE_OPENNI #ifdef __GNUC__ diff --git a/io/src/openni_camera/openni_image_bayer_grbg.cpp b/io/src/openni_camera/openni_image_bayer_grbg.cpp index 1ac27ccb..2381999c 100644 --- a/io/src/openni_camera/openni_image_bayer_grbg.cpp +++ b/io/src/openni_camera/openni_image_bayer_grbg.cpp @@ -36,7 +36,7 @@ * */ #include -#include +#include #ifdef HAVE_OPENNI #include diff --git a/io/src/openni_camera/openni_image_rgb24.cpp b/io/src/openni_camera/openni_image_rgb24.cpp index 9d51d520..6a3935fd 100644 --- a/io/src/openni_camera/openni_image_rgb24.cpp +++ b/io/src/openni_camera/openni_image_rgb24.cpp @@ -1,5 +1,5 @@ #include -#include +#include #ifdef HAVE_OPENNI #include diff --git a/io/src/openni_camera/openni_image_yuv_422.cpp b/io/src/openni_camera/openni_image_yuv_422.cpp index fe2e7897..961d8a28 100644 --- a/io/src/openni_camera/openni_image_yuv_422.cpp +++ b/io/src/openni_camera/openni_image_yuv_422.cpp @@ -35,7 +35,7 @@ * */ #include -#include +#include #ifdef HAVE_OPENNI #include diff --git a/io/src/pcd_grabber.cpp b/io/src/pcd_grabber.cpp index 28a1f0c4..c6682975 100644 --- a/io/src/pcd_grabber.cpp +++ b/io/src/pcd_grabber.cpp @@ -39,6 +39,7 @@ #include #include #include +#include #include #include #include @@ -52,21 +53,21 @@ struct pcl::PCDGrabberBase::PCDGrabberImpl PCDGrabberImpl (pcl::PCDGrabberBase& grabber, const std::vector& pcd_files, float frames_per_second, bool repeat); void trigger (); void readAhead (); - + // TAR reading I/O int openTARFile (const std::string &file_name); void closeTARFile (); bool readTARHeader (); - + //! Initialize (find the locations of all clouds, if we haven't yet) void scrapeForClouds (bool force=false); //! Get cloud at a particular location bool - getCloudAt (std::size_t idx, + getCloudAt (std::size_t idx, pcl::PCLPointCloud2 &blob, - Eigen::Vector4f &origin, + Eigen::Vector4f &origin, Eigen::Quaternionf &orientation); //! Returns the size @@ -144,7 +145,7 @@ pcl::PCDGrabberBase::PCDGrabberImpl::PCDGrabberImpl (pcl::PCDGrabberBase& grabbe } /////////////////////////////////////////////////////////////////////////////////////////// -void +void pcl::PCDGrabberBase::PCDGrabberImpl::readAhead () { PCDReader reader; @@ -215,8 +216,8 @@ pcl::PCDGrabberBase::PCDGrabberImpl::readTARHeader () return (false); } - // We only support regular files for now. - // Additional file types in TAR include: hard links, symbolic links, device/special files, block devices, + // We only support regular files for now. + // Additional file types in TAR include: hard links, symbolic links, device/special files, block devices, // directories, and named pipes. if (tar_header_.file_type[0] != '0' && tar_header_.file_type[0] != '\0') { @@ -264,7 +265,7 @@ pcl::PCDGrabberBase::PCDGrabberImpl::openTARFile (const std::string &file_name) } /////////////////////////////////////////////////////////////////////////////////////////// -void +void pcl::PCDGrabberBase::PCDGrabberImpl::trigger () { std::lock_guard read_ahead_lock(read_ahead_mutex_); @@ -276,7 +277,7 @@ pcl::PCDGrabberBase::PCDGrabberImpl::trigger () } /////////////////////////////////////////////////////////////////////////////////////////// -void +void pcl::PCDGrabberBase::PCDGrabberImpl::scrapeForClouds (bool force) { // Do nothing if we've already scraped (unless force is set) @@ -328,16 +329,16 @@ pcl::PCDGrabberBase::PCDGrabberImpl::scrapeForClouds (bool force) } /////////////////////////////////////////////////////////////////////////////////////////// -bool -pcl::PCDGrabberBase::PCDGrabberImpl::getCloudAt (std::size_t idx, +bool +pcl::PCDGrabberBase::PCDGrabberImpl::getCloudAt (std::size_t idx, pcl::PCLPointCloud2 &blob, - Eigen::Vector4f &origin, + Eigen::Vector4f &origin, Eigen::Quaternionf &orientation) { scrapeForClouds (); // Make sure we've scraped if (idx >= numFrames ()) return false; - + PCDReader reader; int pcd_version; std::string filename = pcd_files_[cloud_idx_to_file_idx_[idx]]; @@ -372,7 +373,7 @@ pcl::PCDGrabberBase::~PCDGrabberBase () noexcept } /////////////////////////////////////////////////////////////////////////////////////////// -void +void pcl::PCDGrabberBase::start () { if (impl_->frames_per_second_ > 0) @@ -387,7 +388,7 @@ pcl::PCDGrabberBase::start () } /////////////////////////////////////////////////////////////////////////////////////////// -void +void pcl::PCDGrabberBase::stop () { if (impl_->frames_per_second_ > 0) @@ -409,35 +410,35 @@ pcl::PCDGrabberBase::trigger () } /////////////////////////////////////////////////////////////////////////////////////////// -bool +bool pcl::PCDGrabberBase::isRunning () const { return (impl_->running_ && (impl_->pcd_iterator_ != impl_->pcd_files_.end())); } /////////////////////////////////////////////////////////////////////////////////////////// -std::string +std::string pcl::PCDGrabberBase::getName () const { return ("PCDGrabber"); } /////////////////////////////////////////////////////////////////////////////////////////// -void +void pcl::PCDGrabberBase::rewind () { impl_->pcd_iterator_ = impl_->pcd_files_.begin (); } /////////////////////////////////////////////////////////////////////////////////////////// -float +float pcl::PCDGrabberBase::getFramesPerSecond () const { return (impl_->frames_per_second_); } /////////////////////////////////////////////////////////////////////////////////////////// -bool +bool pcl::PCDGrabberBase::isRepeatOn () const { return (impl_->repeat_); @@ -445,9 +446,9 @@ pcl::PCDGrabberBase::isRepeatOn () const /////////////////////////////////////////////////////////////////////////////////////////// bool -pcl::PCDGrabberBase::getCloudAt (std::size_t idx, +pcl::PCDGrabberBase::getCloudAt (std::size_t idx, pcl::PCLPointCloud2 &blob, - Eigen::Vector4f &origin, + Eigen::Vector4f &origin, Eigen::Quaternionf &orientation) const { return (impl_->getCloudAt (idx, blob, origin, orientation)); @@ -459,4 +460,3 @@ pcl::PCDGrabberBase::numFrames () const { return (impl_->numFrames ()); } - diff --git a/io/src/real_sense_2_grabber.cpp b/io/src/real_sense_2_grabber.cpp index d96aa35c..8ff9d61e 100644 --- a/io/src/real_sense_2_grabber.cpp +++ b/io/src/real_sense_2_grabber.cpp @@ -292,9 +292,9 @@ namespace pcl const auto cloud_vertices_ptr = points.get_vertices (); const auto cloud_texture_ptr = points.get_texture_coordinates (); -#ifdef _OPENMP -#pragma omp parallel for -#endif +#pragma omp parallel for \ + default(none) \ + shared(cloud, cloud_vertices_ptr, mapColorFunc) for (int index = 0; index < cloud->points.size (); ++index) { const auto ptr = cloud_vertices_ptr + index; diff --git a/io/src/robot_eye_grabber.cpp b/io/src/robot_eye_grabber.cpp index 7f7e71cf..ae654a27 100644 --- a/io/src/robot_eye_grabber.cpp +++ b/io/src/robot_eye_grabber.cpp @@ -36,6 +36,7 @@ */ #include +#include // for pcl::isFinite #include ///////////////////////////////////////////////////////////////////////////// diff --git a/io/tools/converter.cpp b/io/tools/converter.cpp index 39d73b56..dd155ec4 100644 --- a/io/tools/converter.cpp +++ b/io/tools/converter.cpp @@ -50,8 +50,10 @@ #include #include #include +#include // for pcl::make_shared -#include +#include // for boost::filesystem::path +#include // for boost::algorithm::ends_with #define ASCII 0 #define BINARY 1 @@ -169,7 +171,7 @@ saveMesh (pcl::PolygonMesh& input, { if (!input.polygons.empty ()) PCL_WARN ("PCD file format does not support meshes! Only points be saved.\n"); - pcl::PCLPointCloud2::Ptr cloud = boost::make_shared (input.cloud); + pcl::PCLPointCloud2::Ptr cloud = pcl::make_shared (input.cloud); if (!savePointCloud (cloud, output_file, output_type)) return (false); } diff --git a/io/tools/openni_grabber_example.cpp b/io/tools/openni_grabber_example.cpp index 90155263..28ddb5b5 100644 --- a/io/tools/openni_grabber_example.cpp +++ b/io/tools/openni_grabber_example.cpp @@ -50,7 +50,7 @@ class SimpleOpenNIProcessor SimpleOpenNIProcessor (openni_wrapper::OpenNIDevice::DepthMode depth_mode = openni_wrapper::OpenNIDevice::OpenNI_12_bit_depth) : mode (depth_mode) {} void - cloud_cb_ (const pcl::PointCloud::ConstPtr &cloud) + cloud_cb_ (const pcl::PointCloud::ConstPtr &cloud) const { static unsigned count = 0; static double last = pcl::getTime (); diff --git a/kdtree/include/pcl/kdtree/flann.h b/kdtree/include/pcl/kdtree/flann.h index cd944a2a..a49048d0 100644 --- a/kdtree/include/pcl/kdtree/flann.h +++ b/kdtree/include/pcl/kdtree/flann.h @@ -40,7 +40,7 @@ #include -PCL_PRAGMA_WARNING("This header is deprecated and will be removed in an upcoming release.") +PCL_DEPRECATED_HEADER(1, 12, "") #if defined _MSC_VER # pragma warning(disable: 4267 4244) diff --git a/kdtree/include/pcl/kdtree/kdtree.h b/kdtree/include/pcl/kdtree/kdtree.h index 91618731..df74eebc 100644 --- a/kdtree/include/pcl/kdtree/kdtree.h +++ b/kdtree/include/pcl/kdtree/kdtree.h @@ -39,6 +39,7 @@ #pragma once #include +#include #include #include #include diff --git a/keypoints/include/pcl/keypoints/agast_2d.h b/keypoints/include/pcl/keypoints/agast_2d.h index 159272d6..6c3d3e71 100644 --- a/keypoints/include/pcl/keypoints/agast_2d.h +++ b/keypoints/include/pcl/keypoints/agast_2d.h @@ -92,7 +92,7 @@ namespace pcl */ void detectKeypoints (const std::vector &intensity_data, - pcl::PointCloud &output); + pcl::PointCloud &output) const; /** \brief Detects corner points. * \param intensity_data @@ -100,7 +100,7 @@ namespace pcl */ void detectKeypoints (const std::vector &intensity_data, - pcl::PointCloud &output); + pcl::PointCloud &output) const; /** \brief Applies non-max-suppression. * \param[in] intensity_data the image data @@ -227,7 +227,7 @@ namespace pcl void computeCornerScores (const unsigned char* im, const std::vector > & corners_all, - std::vector & scores); + std::vector & scores) const; /** \brief Computes corner scores for the specified points. * \param im @@ -237,7 +237,7 @@ namespace pcl void computeCornerScores (const float* im, const std::vector > & corners_all, - std::vector & scores); + std::vector & scores) const; /** \brief Width of the image to process. */ std::size_t width_; diff --git a/keypoints/include/pcl/keypoints/brisk_2d.h b/keypoints/include/pcl/keypoints/brisk_2d.h index 0d4412aa..fdec9424 100644 --- a/keypoints/include/pcl/keypoints/brisk_2d.h +++ b/keypoints/include/pcl/keypoints/brisk_2d.h @@ -39,8 +39,10 @@ #pragma once +#include // for pcl::isFinite #include + namespace pcl { /** \brief Detects BRISK interest points based on the original code and paper diff --git a/keypoints/include/pcl/keypoints/impl/agast_2d.hpp b/keypoints/include/pcl/keypoints/impl/agast_2d.hpp index 71eaffc9..59bdc220 100644 --- a/keypoints/include/pcl/keypoints/impl/agast_2d.hpp +++ b/keypoints/include/pcl/keypoints/impl/agast_2d.hpp @@ -41,9 +41,12 @@ #include -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +namespace pcl +{ + template bool -pcl::AgastKeypoint2DBase::initCompute () +AgastKeypoint2DBase::initCompute () { if (!pcl::Keypoint::initCompute ()) { @@ -52,7 +55,7 @@ pcl::AgastKeypoint2DBase::initCompute () } if (!input_->isOrganized ()) - { + { PCL_ERROR ("[pcl::%s::initCompute] %s doesn't support non organized clouds!\n", name_.c_str ()); return (false); } @@ -60,9 +63,9 @@ pcl::AgastKeypoint2DBase::initCompute () return (true); } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::AgastKeypoint2D::detectKeypoints (PointCloudOut &output) +AgastKeypoint2D::detectKeypoints (PointCloudOut &output) { // image size const std::size_t width = input_->width; @@ -97,6 +100,9 @@ pcl::AgastKeypoint2D::detectKeypoints (PointCloudOut &outpu output.is_dense = true; } +} // namespace pcl #define AgastKeypoint2D(T,I) template class PCL_EXPORTS pcl::AgastKeypoint2D; -#endif + +#endif + diff --git a/keypoints/include/pcl/keypoints/impl/brisk_2d.hpp b/keypoints/include/pcl/keypoints/impl/brisk_2d.hpp index 1e000277..839a1913 100644 --- a/keypoints/include/pcl/keypoints/impl/brisk_2d.hpp +++ b/keypoints/include/pcl/keypoints/impl/brisk_2d.hpp @@ -42,9 +42,12 @@ #include -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +namespace pcl +{ + template bool -pcl::BriskKeypoint2D::initCompute () +BriskKeypoint2D::initCompute () { if (!pcl::Keypoint::initCompute ()) { @@ -53,7 +56,7 @@ pcl::BriskKeypoint2D::initCompute () } if (!input_->isOrganized ()) - { + { PCL_ERROR ("[pcl::%s::initCompute] %s doesn't support non organized clouds!\n", name_.c_str ()); return (false); } @@ -61,9 +64,9 @@ pcl::BriskKeypoint2D::initCompute () return (true); } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::BriskKeypoint2D::detectKeypoints (PointCloudOut &output) +BriskKeypoint2D::detectKeypoints (PointCloudOut &output) { // image size const int width = int (input_->width); @@ -105,4 +108,7 @@ pcl::BriskKeypoint2D::detectKeypoints (PointClo } } -#endif +} // namespace pcl + +#endif + diff --git a/keypoints/include/pcl/keypoints/impl/harris_2d.hpp b/keypoints/include/pcl/keypoints/impl/harris_2d.hpp index 3bf61115..79713f7e 100644 --- a/keypoints/include/pcl/keypoints/impl/harris_2d.hpp +++ b/keypoints/include/pcl/keypoints/impl/harris_2d.hpp @@ -37,72 +37,76 @@ * */ + #ifndef PCL_HARRIS_KEYPOINT_2D_IMPL_H_ #define PCL_HARRIS_KEYPOINT_2D_IMPL_H_ -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +namespace pcl +{ + template void -pcl::HarrisKeypoint2D::setMethod (ResponseMethod method) +HarrisKeypoint2D::setMethod (ResponseMethod method) { method_ = method; } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::HarrisKeypoint2D::setThreshold (float threshold) +HarrisKeypoint2D::setThreshold (float threshold) { threshold_= threshold; } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::HarrisKeypoint2D::setRefine (bool do_refine) +HarrisKeypoint2D::setRefine (bool do_refine) { refine_ = do_refine; } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::HarrisKeypoint2D::setNonMaxSupression (bool nonmax) +HarrisKeypoint2D::setNonMaxSupression (bool nonmax) { nonmax_ = nonmax; } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::HarrisKeypoint2D::setWindowWidth (int window_width) +HarrisKeypoint2D::setWindowWidth (int window_width) { window_width_= window_width; } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::HarrisKeypoint2D::setWindowHeight (int window_height) +HarrisKeypoint2D::setWindowHeight (int window_height) { window_height_= window_height; } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::HarrisKeypoint2D::setSkippedPixels (int skipped_pixels) +HarrisKeypoint2D::setSkippedPixels (int skipped_pixels) { skipped_pixels_= skipped_pixels; } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::HarrisKeypoint2D::setMinimalDistance (int min_distance) +HarrisKeypoint2D::setMinimalDistance (int min_distance) { min_distance_ = min_distance; } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::HarrisKeypoint2D::computeSecondMomentMatrix (std::size_t index, float* coefficients) const +HarrisKeypoint2D::computeSecondMomentMatrix (std::size_t index, float* coefficients) const { static const int width = static_cast (input_->width); static const int height = static_cast (input_->height); - + int x = static_cast (index % input_->width); int y = static_cast (index / input_->width); // indices 0 1 2 @@ -122,9 +126,9 @@ pcl::HarrisKeypoint2D::computeSecondMomentMatri } } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template bool -pcl::HarrisKeypoint2D::initCompute () +HarrisKeypoint2D::initCompute () { if (!pcl::Keypoint::initCompute ()) { @@ -133,17 +137,17 @@ pcl::HarrisKeypoint2D::initCompute () } if (!input_->isOrganized ()) - { + { PCL_ERROR ("[pcl::%s::initCompute] %s doesn't support non organized clouds!\n", name_.c_str ()); return (false); } - + if (indices_->size () != input_->size ()) { PCL_ERROR ("[pcl::%s::initCompute] %s doesn't support setting indices!\n", name_.c_str ()); return (false); } - + if ((window_height_%2) == 0) { PCL_ERROR ("[pcl::%s::initCompute] Window height must be odd!\n", name_.c_str ()); @@ -161,21 +165,21 @@ pcl::HarrisKeypoint2D::initCompute () PCL_ERROR ("[pcl::%s::initCompute] Window size must be >= 3x3!\n", name_.c_str ()); return (false); } - + half_window_width_ = window_width_ / 2; half_window_height_ = window_height_ / 2; return (true); } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::HarrisKeypoint2D::detectKeypoints (PointCloudOut &output) +HarrisKeypoint2D::detectKeypoints (PointCloudOut &output) { derivatives_cols_.resize (input_->width, input_->height); derivatives_rows_.resize (input_->width, input_->height); //Compute cloud intensities first derivatives along columns and rows - //!!! nsallem 20120220 : we don't test here for density so if one term in nan the result is nan + //!!! nsallem 20120220 : we don't test here for density so if one term is nan the result is nan int w = static_cast (input_->width) - 1; int h = static_cast (input_->height) - 1; // j = 0 --> j-1 out of range ; use 0 @@ -212,9 +216,9 @@ pcl::HarrisKeypoint2D::detectKeypoints (PointCl derivatives_rows_(0,h) = (intensity_ ((*input_) (1,h)) - intensity_ ((*input_) (0,h))) * 0.5; for(int i = 1; i < w; ++i) - { + { derivatives_cols_(i,h) = (intensity_ ((*input_) (i,h)) - intensity_ ((*input_) (i,h-1))) * 0.5; - } + } derivatives_rows_(w,h) = (intensity_ ((*input_) (w,h)) - intensity_ ((*input_) (w-1,h))) * 0.5; derivatives_cols_(w,h) = (intensity_ ((*input_) (w,h)) - intensity_ ((*input_) (w,h-1))) * 0.5; @@ -233,7 +237,7 @@ pcl::HarrisKeypoint2D::detectKeypoints (PointCl responseTomasi(*response_); break; } - + if (!nonmax_) { output = *response_; @@ -241,18 +245,28 @@ pcl::HarrisKeypoint2D::detectKeypoints (PointCl keypoints_indices_->indices.push_back (i); } else - { + { std::sort (indices_->begin (), indices_->end (), [this] (int p1, int p2) { return greaterIntensityAtIndices (p1, p2); }); - float threshold = threshold_ * response_->points[indices_->front ()].intensity; + const float threshold = threshold_ * response_->points[indices_->front ()].intensity; output.clear (); output.reserve (response_->size()); - std::vector occupency_map (response_->size (), false); + std::vector occupency_map (response_->size (), false); int width (response_->width); int height (response_->height); const int occupency_map_size (occupency_map.size ()); -#ifdef _OPENMP -#pragma omp parallel for shared (output, occupency_map) firstprivate (width, height) num_threads(threads_) +#if OPENMP_LEGACY_CONST_DATA_SHARING_RULE +#pragma omp parallel for \ + default(none) \ + shared(occupency_map, output) \ + firstprivate(width, height) \ + num_threads(threads_) +#else +#pragma omp parallel for \ + default(none) \ + shared(occupency_map, occupency_map_size, output, threshold) \ + firstprivate(width, height) \ + num_threads(threads_) #endif for (int i = 0; i < occupency_map_size; ++i) { @@ -260,17 +274,15 @@ pcl::HarrisKeypoint2D::detectKeypoints (PointCl const PointOutT& point_out = response_->points [idx]; if (occupency_map[idx] || point_out.intensity < threshold || !isFinite (point_out)) continue; - -#ifdef _OPENMP + #pragma omp critical -#endif { output.push_back (point_out); keypoints_indices_->indices.push_back (idx); } - - int u_end = std::min (width, idx % width + min_distance_); - int v_end = std::min (height, idx / width + min_distance_); + + int u_end = std::min (width, idx % width + min_distance_); + int v_end = std::min (height, idx / width + min_distance_); for(int u = std::max (0, idx % width - min_distance_); u < u_end; ++u) for(int v = std::max (0, idx / width - min_distance_); v < v_end; ++v) occupency_map[v*input_->width+u] = true; @@ -287,17 +299,27 @@ pcl::HarrisKeypoint2D::detectKeypoints (PointCl output.is_dense = input_->is_dense; } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::HarrisKeypoint2D::responseHarris (PointCloudOut &output) const +HarrisKeypoint2D::responseHarris (PointCloudOut &output) const { PCL_ALIGN (16) float covar [3]; output.clear (); output.resize (input_->size ()); const int output_size (output.size ()); -#ifdef _OPENMP -#pragma omp parallel for shared (output) private (covar) num_threads(threads_) +#if OPENMP_LEGACY_CONST_DATA_SHARING_RULE +#pragma omp parallel for \ + default(none) \ + shared(output) \ + private(covar) \ + num_threads(threads_) +#else +#pragma omp parallel for \ + default(none) \ + shared(output, output_size) \ + private(covar) \ + num_threads(threads_) #endif for (int index = 0; index < output_size; ++index) { @@ -323,17 +345,27 @@ pcl::HarrisKeypoint2D::responseHarris (PointClo output.width = input_->width; } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::HarrisKeypoint2D::responseNoble (PointCloudOut &output) const +HarrisKeypoint2D::responseNoble (PointCloudOut &output) const { PCL_ALIGN (16) float covar [3]; output.clear (); output.resize (input_->size ()); const int output_size (output.size ()); -#ifdef _OPENMP -#pragma omp parallel for shared (output) private (covar) num_threads(threads_) +#if OPENMP_LEGACY_CONST_DATA_SHARING_RULE +#pragma omp parallel for \ + default(none) \ + shared(output) \ + private(covar) \ + num_threads(threads_) +#else +#pragma omp parallel for \ + default(none) \ + shared(output, output_size) \ + private(covar) \ + num_threads(threads_) #endif for (int index = 0; index < output_size; ++index) { @@ -344,7 +376,7 @@ pcl::HarrisKeypoint2D::responseNoble (PointClou out_point.z = in_point.z; out_point.intensity = 0; if (isFinite (in_point)) - { + { computeSecondMomentMatrix (index, covar); float trace = covar [0] + covar [2]; if (trace != 0) @@ -352,26 +384,36 @@ pcl::HarrisKeypoint2D::responseNoble (PointClou float det = covar[0] * covar[2] - covar[1] * covar[1]; out_point.intensity = det / trace; } - } + } } - + output.height = input_->height; output.width = input_->width; } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::HarrisKeypoint2D::responseLowe (PointCloudOut &output) const +HarrisKeypoint2D::responseLowe (PointCloudOut &output) const { PCL_ALIGN (16) float covar [3]; output.clear (); output.resize (input_->size ()); const int output_size (output.size ()); -#ifdef _OPENMP -#pragma omp parallel for shared (output) private (covar) num_threads(threads_) +#if OPENMP_LEGACY_CONST_DATA_SHARING_RULE +#pragma omp parallel for \ + default(none) \ + shared(output) \ + private(covar) \ + num_threads(threads_) +#else +#pragma omp parallel for \ + default(none) \ + shared(output, output_size) \ + private(covar) \ + num_threads(threads_) #endif - for (int index = 0; index < output_size; ++index) + for (int index = 0; index < output_size; ++index) { PointOutT &out_point = output.points [index]; const PointInT &in_point = input_->points [index]; @@ -380,7 +422,7 @@ pcl::HarrisKeypoint2D::responseLowe (PointCloud out_point.z = in_point.z; out_point.intensity = 0; if (isFinite (in_point)) - { + { computeSecondMomentMatrix (index, covar); float trace = covar [0] + covar [2]; if (trace != 0) @@ -395,17 +437,27 @@ pcl::HarrisKeypoint2D::responseLowe (PointCloud output.width = input_->width; } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::HarrisKeypoint2D::responseTomasi (PointCloudOut &output) const +HarrisKeypoint2D::responseTomasi (PointCloudOut &output) const { PCL_ALIGN (16) float covar [3]; output.clear (); output.resize (input_->size ()); const int output_size (output.size ()); -#ifdef _OPENMP -#pragma omp parallel for shared (output) private (covar) num_threads(threads_) +#if OPENMP_LEGACY_CONST_DATA_SHARING_RULE +#pragma omp parallel for \ + default(none) \ + shared(output) \ + private(covar) \ + num_threads(threads_) +#else +#pragma omp parallel for \ + default(none) \ + shared(output, output_size) \ + private(covar) \ + num_threads(threads_) #endif for (int index = 0; index < output_size; ++index) { @@ -416,60 +468,20 @@ pcl::HarrisKeypoint2D::responseTomasi (PointClo out_point.z = in_point.z; out_point.intensity = 0; if (isFinite (in_point)) - { + { computeSecondMomentMatrix (index, covar); // min egenvalue out_point.intensity = ((covar[0] + covar[2] - sqrt((covar[0] - covar[2])*(covar[0] - covar[2]) + 4 * covar[1] * covar[1])) /2.0f); - } + } } - + output.height = input_->height; output.width = input_->width; } -// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -// template void -// pcl::HarrisKeypoint2D::refineCorners (PointCloudOut &corners) const -// { -// std::vector nn_indices; -// std::vector nn_dists; - -// Eigen::Matrix2f nnT; -// Eigen::Matrix2f NNT; -// Eigen::Matrix2f NNTInv; -// Eigen::Vector2f NNTp; -// float diff; -// const unsigned max_iterations = 10; -// #ifdef _OPENMP -// #pragma omp parallel for shared (corners) private (nnT, NNT, NNTInv, NNTp, diff, nn_indices, nn_dists) num_threads(threads_) -// #endif -// for (int cIdx = 0; cIdx < static_cast (corners.size ()); ++cIdx) -// { -// unsigned iterations = 0; -// do { -// NNT.setZero(); -// NNTp.setZero(); -// PointInT corner; -// corner.x = corners[cIdx].x; -// corner.y = corners[cIdx].y; -// corner.z = corners[cIdx].z; -// tree_->radiusSearch (corner, search_radius_, nn_indices, nn_dists); -// for (std::vector::const_iterator iIt = nn_indices.begin(); iIt != nn_indices.end(); ++iIt) -// { -// if (!std::isfinite (normals_->points[*iIt].normal_x)) -// continue; - -// nnT = normals_->points[*iIt].getNormalVector3fMap () * normals_->points[*iIt].getNormalVector3fMap ().transpose(); -// NNT += nnT; -// NNTp += nnT * surface_->points[*iIt].getVector3fMap (); -// } -// if (invert3x3SymMatrix (NNT, NNTInv) != 0) -// corners[cIdx].getVector3fMap () = NNTInv * NNTp; - -// diff = (corners[cIdx].getVector3fMap () - corner.getVector3fMap()).squaredNorm (); -// } while (diff > 1e-6 && ++iterations < max_iterations); -// } -// } +} // namespace pcl #define PCL_INSTANTIATE_HarrisKeypoint2D(T,U,I) template class PCL_EXPORTS pcl::HarrisKeypoint2D; + #endif // #ifndef PCL_HARRIS_KEYPOINT_2D_IMPL_H_ + diff --git a/keypoints/include/pcl/keypoints/impl/harris_3d.hpp b/keypoints/include/pcl/keypoints/impl/harris_3d.hpp index 14e2c2df..4cc061f1 100644 --- a/keypoints/include/pcl/keypoints/impl/harris_3d.hpp +++ b/keypoints/include/pcl/keypoints/impl/harris_3d.hpp @@ -276,9 +276,10 @@ pcl::HarrisKeypoint3D::detectKeypoints (PointCloud output.points.clear (); output.points.reserve (response->points.size()); -#ifdef _OPENMP -#pragma omp parallel for shared (output) num_threads(threads_) -#endif +#pragma omp parallel for \ + default(none) \ + shared(output, response) \ + num_threads(threads_) for (int idx = 0; idx < static_cast (response->points.size ()); ++idx) { if (!isFinite (response->points[idx]) || @@ -299,9 +300,7 @@ pcl::HarrisKeypoint3D::detectKeypoints (PointCloud } } if (is_maxima) -#ifdef _OPENMP #pragma omp critical -#endif { output.points.push_back (response->points[idx]); keypoints_indices_->indices.push_back (idx); @@ -323,9 +322,11 @@ pcl::HarrisKeypoint3D::responseHarris (PointCloudO { PCL_ALIGN (16) float covar [8]; output.resize (input_->size ()); -#ifdef _OPENMP - #pragma omp parallel for shared (output) private (covar) num_threads(threads_) -#endif +#pragma omp parallel for \ + default(none) \ + shared(output) \ + private(covar) \ + num_threads(threads_) for (int pIdx = 0; pIdx < static_cast (input_->size ()); ++pIdx) { const PointInT& pointIn = input_->points [pIdx]; @@ -362,9 +363,11 @@ pcl::HarrisKeypoint3D::responseNoble (PointCloudOu { PCL_ALIGN (16) float covar [8]; output.resize (input_->size ()); -#ifdef _OPENMP - #pragma omp parallel for shared (output) private (covar) num_threads(threads_) -#endif +#pragma omp parallel \ + for default(none) \ + shared(output) \ + private(covar) \ + num_threads(threads_) for (int pIdx = 0; pIdx < static_cast (input_->size ()); ++pIdx) { const PointInT& pointIn = input_->points [pIdx]; @@ -400,9 +403,11 @@ pcl::HarrisKeypoint3D::responseLowe (PointCloudOut { PCL_ALIGN (16) float covar [8]; output.resize (input_->size ()); -#ifdef _OPENMP - #pragma omp parallel for shared (output) private (covar) num_threads(threads_) -#endif +#pragma omp parallel for \ + default(none) \ + shared(output) \ + private(covar) \ + num_threads(threads_) for (int pIdx = 0; pIdx < static_cast (input_->size ()); ++pIdx) { const PointInT& pointIn = input_->points [pIdx]; @@ -457,9 +462,11 @@ pcl::HarrisKeypoint3D::responseTomasi (PointCloudO PCL_ALIGN (16) float covar [8]; Eigen::Matrix3f covariance_matrix; output.resize (input_->size ()); -#ifdef _OPENMP - #pragma omp parallel for shared (output) private (covar, covariance_matrix) num_threads(threads_) -#endif +#pragma omp parallel for \ + default(none) \ + shared(output) \ + private(covar, covariance_matrix) \ + num_threads(threads_) for (int pIdx = 0; pIdx < static_cast (input_->size ()); ++pIdx) { const PointInT& pointIn = input_->points [pIdx]; @@ -503,9 +510,11 @@ pcl::HarrisKeypoint3D::refineCorners (PointCloudOu Eigen::Vector3f NNTp; float diff; const unsigned max_iterations = 10; -#ifdef _OPENMP - #pragma omp parallel for shared (corners) private (nnT, NNT, NNTInv, NNTp, diff) num_threads(threads_) -#endif +#pragma omp parallel for \ + default(none) \ + shared(corners) \ + private(nnT, NNT, NNTInv, NNTp, diff) \ + num_threads(threads_) for (int cIdx = 0; cIdx < static_cast (corners.size ()); ++cIdx) { unsigned iterations = 0; diff --git a/keypoints/include/pcl/keypoints/impl/harris_6d.hpp b/keypoints/include/pcl/keypoints/impl/harris_6d.hpp index 4b05db49..47c56ca2 100644 --- a/keypoints/include/pcl/keypoints/impl/harris_6d.hpp +++ b/keypoints/include/pcl/keypoints/impl/harris_6d.hpp @@ -164,9 +164,9 @@ pcl::HarrisKeypoint6D::detectKeypoints (PointCloud pcl::PointCloud::Ptr cloud (new pcl::PointCloud); cloud->resize (surface_->size ()); -#ifdef _OPENMP - #pragma omp parallel for num_threads(threads_) default(shared) -#endif +#pragma omp parallel for \ + default(none) \ + num_threads(threads_) for (unsigned idx = 0; idx < surface_->size (); ++idx) { cloud->points [idx].x = surface_->points [idx].x; @@ -184,9 +184,9 @@ pcl::HarrisKeypoint6D::detectKeypoints (PointCloud grad_est.setRadiusSearch (search_radius_); grad_est.compute (*intensity_gradients_); -#ifdef _OPENMP - #pragma omp parallel for num_threads(threads_) default (shared) -#endif +#pragma omp parallel for \ + default(none) \ + num_threads(threads_) for (std::size_t idx = 0; idx < intensity_gradients_->size (); ++idx) { float len = intensity_gradients_->points [idx].gradient_x * intensity_gradients_->points [idx].gradient_x + @@ -227,9 +227,9 @@ pcl::HarrisKeypoint6D::detectKeypoints (PointCloud output.points.clear (); output.points.reserve (response->points.size()); -#ifdef _OPENMP - #pragma omp parallel for num_threads(threads_) default(shared) -#endif +#pragma omp parallel for \ + default(none) \ + num_threads(threads_) for (std::size_t idx = 0; idx < response->points.size (); ++idx) { if (!isFinite (response->points[idx]) || response->points[idx].intensity < threshold_) @@ -248,9 +248,7 @@ pcl::HarrisKeypoint6D::detectKeypoints (PointCloud } } if (is_maxima) -#ifdef _OPENMP #pragma omp critical -#endif { output.points.push_back (response->points[idx]); keypoints_indices_->indices.push_back (idx); @@ -275,9 +273,10 @@ pcl::HarrisKeypoint6D::responseTomasi (PointCloudO Eigen::SelfAdjointEigenSolver > solver; Eigen::Matrix covariance; -#ifdef _OPENMP - #pragma omp parallel for default (shared) private (pointOut, covar, covariance, solver) num_threads(threads_) -#endif +#pragma omp parallel for \ + default(none) \ + private(pointOut, covar, covariance, solver) \ + num_threads(threads_) for (unsigned pIdx = 0; pIdx < input_->size (); ++pIdx) { const PointInT& pointIn = input_->points [pIdx]; @@ -347,10 +346,8 @@ pcl::HarrisKeypoint6D::responseTomasi (PointCloudO pointOut.x = pointIn.x; pointOut.y = pointIn.y; pointOut.z = pointIn.z; -#ifdef _OPENMP - #pragma omp critical -#endif + #pragma omp critical output.points.push_back(pointOut); } output.height = input_->height; diff --git a/keypoints/include/pcl/keypoints/impl/iss_3d.hpp b/keypoints/include/pcl/keypoints/impl/iss_3d.hpp index 2ee9f0c0..5537ad4f 100644 --- a/keypoints/include/pcl/keypoints/impl/iss_3d.hpp +++ b/keypoints/include/pcl/keypoints/impl/iss_3d.hpp @@ -112,9 +112,11 @@ pcl::ISSKeypoint3D::getBoundaryPoints (PointCloudI pcl::BoundaryEstimation boundary_estimator; boundary_estimator.setInputCloud (input_); -#ifdef _OPENMP -#pragma omp parallel for private(u, v) num_threads(threads_) -#endif +#pragma omp parallel for \ + default(none) \ + shared(angle_threshold, boundary_estimator, border_radius, edge_points, input) \ + private(u, v) \ + num_threads(threads_) for (int index = 0; index < int (input.points.size ()); index++) { edge_points[index] = false; @@ -233,13 +235,11 @@ pcl::ISSKeypoint3D::initCompute () return (false); } - if (third_eigen_value_) delete[] third_eigen_value_; third_eigen_value_ = new double[input_->size ()]; memset(third_eigen_value_, 0, sizeof(double) * input_->size ()); - if (edge_points_) delete[] edge_points_; if (border_radius_ > 0.0) @@ -299,9 +299,10 @@ pcl::ISSKeypoint3D::detectKeypoints (PointCloudOut bool* borders = new bool [input_->size()]; -#ifdef _OPENMP - #pragma omp parallel for num_threads(threads_) -#endif +#pragma omp parallel for \ + default(none) \ + shared(borders) \ + num_threads(threads_) for (int index = 0; index < int (input_->size ()); index++) { borders[index] = false; @@ -342,9 +343,10 @@ pcl::ISSKeypoint3D::detectKeypoints (PointCloudOut for (std::size_t i = 0; i < input_->size (); i++) prg_mem[i] = prg_local_mem + 3 * i; -#ifdef _OPENMP - #pragma omp parallel for num_threads(threads_) -#endif +#pragma omp parallel for \ + default(none) \ + shared(borders, omp_mem, prg_mem) \ + num_threads(threads_) for (int index = 0; index < static_cast (input_->size ()); index++) { #ifdef _OPENMP @@ -395,11 +397,11 @@ pcl::ISSKeypoint3D::detectKeypoints (PointCloudOut } bool* feat_max = new bool [input_->size()]; - bool is_max; -#ifdef _OPENMP - #pragma omp parallel for private(is_max) num_threads(threads_) -#endif +#pragma omp parallel for \ + default(none) \ + shared(feat_max) \ + num_threads(threads_) for (int index = 0; index < int (input_->size ()); index++) { feat_max [index] = false; @@ -417,7 +419,7 @@ pcl::ISSKeypoint3D::detectKeypoints (PointCloudOut if (n_neighbors >= min_neighbors_) { - is_max = true; + bool is_max = true; for (int j = 0 ; j < n_neighbors; j++) if (third_eigen_value_[index] < third_eigen_value_[nn_indices[j]]) @@ -428,15 +430,14 @@ pcl::ISSKeypoint3D::detectKeypoints (PointCloudOut } } -#ifdef _OPENMP -#pragma omp parallel for shared (output) num_threads(threads_) -#endif +#pragma omp parallel for \ + default(none) \ + shared(feat_max, output) \ + num_threads(threads_) for (int index = 0; index < int (input_->size ()); index++) { if (feat_max[index]) -#ifdef _OPENMP #pragma omp critical -#endif { PointOutT p; p.getVector3fMap () = input_->points[index].getVector3fMap (); diff --git a/keypoints/include/pcl/keypoints/impl/keypoint.hpp b/keypoints/include/pcl/keypoints/impl/keypoint.hpp index 024534f3..5a29c444 100644 --- a/keypoints/include/pcl/keypoints/impl/keypoint.hpp +++ b/keypoints/include/pcl/keypoints/impl/keypoint.hpp @@ -35,12 +35,16 @@ * */ + #ifndef PCL_KEYPOINT_IMPL_H_ #define PCL_KEYPOINT_IMPL_H_ -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +namespace pcl +{ + template bool -pcl::Keypoint::initCompute () +Keypoint::initCompute () { if (!PCLBase::initCompute ()) return (false); @@ -124,9 +128,9 @@ pcl::Keypoint::initCompute () return (true); } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template inline void -pcl::Keypoint::compute (PointCloudOut &output) +Keypoint::compute (PointCloudOut &output) { if (!initCompute ()) { @@ -144,5 +148,7 @@ pcl::Keypoint::compute (PointCloudOut &output) surface_.reset (); } +} // namespace pcl + #endif //#ifndef PCL_KEYPOINT_IMPL_H_ diff --git a/keypoints/include/pcl/keypoints/impl/susan.hpp b/keypoints/include/pcl/keypoints/impl/susan.hpp index 11a220b7..8e95bb9d 100644 --- a/keypoints/include/pcl/keypoints/impl/susan.hpp +++ b/keypoints/include/pcl/keypoints/impl/susan.hpp @@ -309,9 +309,6 @@ pcl::SUSANKeypoint::detectKeypoints (P label_idx_ = pcl::getFieldIndex ("label", out_fields_); const int input_size = static_cast (input_->size ()); -//#ifdef _OPENMP -//#pragma omp parallel for shared (response) num_threads(threads_) -//#endif for (int point_index = 0; point_index < input_size; ++point_index) { const PointInT& point_in = input_->points [point_index]; @@ -362,9 +359,6 @@ pcl::SUSANKeypoint::detectKeypoints (P memcpy (reinterpret_cast (&point_out) + out_fields_[label_idx_].offset, &label, sizeof (std::uint32_t)); } -//#ifdef _OPENMP -//#pragma omp critical -//#endif response->push_back (point_out); } else @@ -398,9 +392,6 @@ pcl::SUSANKeypoint::detectKeypoints (P memcpy (reinterpret_cast (&point_out) + out_fields_[label_idx_].offset, &label, sizeof (std::uint32_t)); } -//#ifdef _OPENMP -//#pragma omp critical -//#endif response->push_back (point_out); } } @@ -424,9 +415,6 @@ pcl::SUSANKeypoint::detectKeypoints (P output.points.clear (); output.points.reserve (response->points.size()); -//#ifdef _OPENMP -//#pragma omp parallel for shared (output) num_threads(threads_) -//#endif for (int idx = 0; idx < static_cast (response->points.size ()); ++idx) { const PointOutT& point_in = response->points [idx]; @@ -449,9 +437,6 @@ pcl::SUSANKeypoint::detectKeypoints (P } } if (is_minima) -//#ifdef _OPENMP -//#pragma omp critical -//#endif { output.points.push_back (response->points[idx]); keypoints_indices_->indices.push_back (idx); diff --git a/keypoints/include/pcl/keypoints/impl/trajkovic_2d.hpp b/keypoints/include/pcl/keypoints/impl/trajkovic_2d.hpp index b9c83ed4..d5edfa20 100644 --- a/keypoints/include/pcl/keypoints/impl/trajkovic_2d.hpp +++ b/keypoints/include/pcl/keypoints/impl/trajkovic_2d.hpp @@ -35,11 +35,16 @@ * */ + #ifndef PCL_TRAJKOVIC_KEYPOINT_2D_IMPL_H_ #define PCL_TRAJKOVIC_KEYPOINT_2D_IMPL_H_ + +namespace pcl +{ + template bool -pcl::TrajkovicKeypoint2D::initCompute () +TrajkovicKeypoint2D::initCompute () { if (!PCLBase::initCompute ()) return (false); @@ -76,18 +81,25 @@ pcl::TrajkovicKeypoint2D::initCompute () return (true); } -///////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::TrajkovicKeypoint2D::detectKeypoints (PointCloudOut &output) +TrajkovicKeypoint2D::detectKeypoints (PointCloudOut &output) { response_.reset (new pcl::PointCloud (input_->width, input_->height)); - int w = static_cast (input_->width) - half_window_size_; - int h = static_cast (input_->height) - half_window_size_; + const int w = static_cast (input_->width) - half_window_size_; + const int h = static_cast (input_->height) - half_window_size_; if (method_ == pcl::TrajkovicKeypoint2D::FOUR_CORNERS) { -#ifdef _OPENMP -#pragma omp parallel for num_threads (threads_) +#if OPENMP_LEGACY_CONST_DATA_SHARING_RULE +#pragma omp parallel for \ + default(none) \ + num_threads(threads_) +#else +#pragma omp parallel for \ + default(none) \ + shared(h, w) \ + num_threads(threads_) #endif for(int j = half_window_size_; j < h; ++j) { @@ -127,8 +139,15 @@ pcl::TrajkovicKeypoint2D::detectKeypoints (Poin } else { -#ifdef _OPENMP -#pragma omp parallel for num_threads (threads_) +#if OPENMP_LEGACY_CONST_DATA_SHARING_RULE +#pragma omp parallel for \ + default(none) \ + num_threads(threads_) +#else +#pragma omp parallel for \ + default(none) \ + shared(h, w) \ + num_threads(threads_) #endif for(int j = half_window_size_; j < h; ++j) { @@ -216,8 +235,16 @@ pcl::TrajkovicKeypoint2D::detectKeypoints (Poin const int width (input_->width); const int height (input_->height); -#ifdef _OPENMP -#pragma omp parallel for shared (output) num_threads (threads_) +#if OPENMP_LEGACY_CONST_DATA_SHARING_RULE +#pragma omp parallel for \ + default(none) \ + shared(indices, occupency_map, output) \ + num_threads(threads_) +#else +#pragma omp parallel for \ + default(none) \ + shared(height, indices, occupency_map, output, width) \ + num_threads(threads_) #endif for (std::size_t i = 0; i < indices.size (); ++i) { @@ -229,9 +256,7 @@ pcl::TrajkovicKeypoint2D::detectKeypoints (Poin p.getVector3fMap () = input_->points[idx].getVector3fMap (); p.intensity = response_->points [idx]; -#ifdef _OPENMP #pragma omp critical -#endif { output.push_back (p); keypoints_indices_->indices.push_back (idx); @@ -252,5 +277,9 @@ pcl::TrajkovicKeypoint2D::detectKeypoints (Poin output.is_dense = input_->is_dense; } +} // namespace pcl + #define PCL_INSTANTIATE_TrajkovicKeypoint2D(T,U,I) template class PCL_EXPORTS pcl::TrajkovicKeypoint2D; + #endif + diff --git a/keypoints/include/pcl/keypoints/impl/trajkovic_3d.hpp b/keypoints/include/pcl/keypoints/impl/trajkovic_3d.hpp index a9a67973..d8b4eba1 100644 --- a/keypoints/include/pcl/keypoints/impl/trajkovic_3d.hpp +++ b/keypoints/include/pcl/keypoints/impl/trajkovic_3d.hpp @@ -40,8 +40,12 @@ #include + +namespace pcl +{ + template bool -pcl::TrajkovicKeypoint3D::initCompute () +TrajkovicKeypoint3D::initCompute () { if (!PCLBase::initCompute ()) return (false); @@ -89,9 +93,9 @@ pcl::TrajkovicKeypoint3D::initCompute () return (true); } -///////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::TrajkovicKeypoint3D::detectKeypoints (PointCloudOut &output) +TrajkovicKeypoint3D::detectKeypoints (PointCloudOut &output) { response_.reset (new pcl::PointCloud (input_->width, input_->height)); const Normals &normals = *normals_; @@ -102,8 +106,16 @@ pcl::TrajkovicKeypoint3D::detectKeypoints (PointCl if (method_ == FOUR_CORNERS) { -#ifdef _OPENMP -#pragma omp parallel for num_threads (threads_) +#if OPENMP_LEGACY_CONST_DATA_SHARING_RULE +#pragma omp parallel for \ + default(none) \ + shared(input, normals, response) \ + num_threads(threads_) +#else +#pragma omp parallel for \ + default(none) \ + shared(h, input, normals, response, w) \ + num_threads(threads_) #endif for(int j = half_window_size_; j < h; ++j) { @@ -144,8 +156,16 @@ pcl::TrajkovicKeypoint3D::detectKeypoints (PointCl } else { -#ifdef _OPENMP -#pragma omp parallel for num_threads (threads_) +#if OPENMP_LEGACY_CONST_DATA_SHARING_RULE +#pragma omp parallel for \ + default(none) \ + shared(input, normals, response) \ + num_threads(threads_) +#else +#pragma omp parallel for \ + default(none) \ + shared(h, input, normals, response, w) \ + num_threads(threads_) #endif for(int j = half_window_size_; j < h; ++j) { @@ -230,8 +250,16 @@ pcl::TrajkovicKeypoint3D::detectKeypoints (PointCl const int width (input_->width); const int height (input_->height); -#ifdef _OPENMP -#pragma omp parallel for shared (output) num_threads (threads_) +#if OPENMP_LEGACY_CONST_DATA_SHARING_RULE +#pragma omp parallel for \ + default(none) \ + shared(indices, occupency_map, output) \ + num_threads(threads_) +#else +#pragma omp parallel for \ + default(none) \ + shared(height, indices, occupency_map, output, width) \ + num_threads(threads_) #endif for (int i = 0; i < static_cast(indices.size ()); ++i) { @@ -243,9 +271,7 @@ pcl::TrajkovicKeypoint3D::detectKeypoints (PointCl p.getVector3fMap () = input_->points[idx].getVector3fMap (); p.intensity = response_->points [idx]; -#ifdef _OPENMP #pragma omp critical -#endif { output.push_back (p); keypoints_indices_->indices.push_back (idx); @@ -266,5 +292,9 @@ pcl::TrajkovicKeypoint3D::detectKeypoints (PointCl output.is_dense = true; } +} // namespace pcl + #define PCL_INSTANTIATE_TrajkovicKeypoint3D(T,U,N) template class PCL_EXPORTS pcl::TrajkovicKeypoint3D; + #endif + diff --git a/keypoints/include/pcl/keypoints/narf_keypoint.h b/keypoints/include/pcl/keypoints/narf_keypoint.h index 29c72f09..ef07ae6a 100644 --- a/keypoints/include/pcl/keypoints/narf_keypoint.h +++ b/keypoints/include/pcl/keypoints/narf_keypoint.h @@ -37,6 +37,7 @@ #pragma once +#include #include #include #include diff --git a/keypoints/include/pcl/keypoints/uniform_sampling.h b/keypoints/include/pcl/keypoints/uniform_sampling.h index 7266dcfe..4b9da135 100644 --- a/keypoints/include/pcl/keypoints/uniform_sampling.h +++ b/keypoints/include/pcl/keypoints/uniform_sampling.h @@ -40,7 +40,7 @@ #pragma once #ifdef __DEPRECATED -#warning UniformSampling is not a Keypoint anymore, use instead. +PCL_DEPRECATED_HEADER(1, 12, "UniformSampling is not a Keypoint anymore, use instead.") #endif #include diff --git a/keypoints/src/agast_2d.cpp b/keypoints/src/agast_2d.cpp index 82a00a97..c2884d07 100644 --- a/keypoints/src/agast_2d.cpp +++ b/keypoints/src/agast_2d.cpp @@ -79,7 +79,7 @@ pcl::AgastKeypoint2D::detectKeypoints (pcl::PointCl void pcl::keypoints::agast::AbstractAgastDetector::detectKeypoints ( const std::vector & intensity_data, - pcl::PointCloud &output) + pcl::PointCloud &output) const { detect (&(intensity_data[0]), output.points); @@ -91,7 +91,7 @@ pcl::keypoints::agast::AbstractAgastDetector::detectKeypoints ( void pcl::keypoints::agast::AbstractAgastDetector::detectKeypoints ( const std::vector & intensity_data, - pcl::PointCloud &output) + pcl::PointCloud &output) const { detect (&(intensity_data[0]), output.points); @@ -297,7 +297,7 @@ void pcl::keypoints::agast::AbstractAgastDetector::computeCornerScores ( const unsigned char* im, const std::vector > &corners_all, - std::vector &scores) + std::vector &scores) const { unsigned int num_corners = static_cast (corners_all.size ()); @@ -327,7 +327,7 @@ void pcl::keypoints::agast::AbstractAgastDetector::computeCornerScores ( const float* im, const std::vector > &corners_all, - std::vector &scores) + std::vector &scores) const { unsigned int num_corners = static_cast (corners_all.size ()); diff --git a/keypoints/src/narf_keypoint.cpp b/keypoints/src/narf_keypoint.cpp index 26717e9d..a23572cf 100644 --- a/keypoints/src/narf_keypoint.cpp +++ b/keypoints/src/narf_keypoint.cpp @@ -279,8 +279,13 @@ NarfKeypoint::calculateCompleteInterestImage () was_touched.resize (array_size, false); std::vector neighbors_to_check; -# pragma omp parallel for num_threads (parameters_.max_no_of_threads) default (shared) \ - firstprivate (was_touched, neighbors_to_check, angle_histogram) schedule (dynamic, 10) +#pragma omp parallel for \ + default(none) \ + shared(array_size, border_descriptions, interest_image, range_image, radius_reciprocal, radius_squared, scale_idx, \ + surface_change_directions, surface_change_scores, start_usage_range) \ + firstprivate(was_touched, neighbors_to_check, angle_histogram) \ + schedule(dynamic, 10) \ + num_threads(parameters_.max_no_of_threads) for (int index=0; index +#include #include #include #include -#include - namespace pcl { class PCL_EXPORTS DenseCrf { @@ -107,7 +107,7 @@ public: expAndNormalize(std::vector& out, const std::vector& in, float scale, - float relax = 1.0f); + float relax = 1.0f) const; void expAndNormalizeORI(float* out, diff --git a/ml/include/pcl/ml/dt/decision_forest.h b/ml/include/pcl/ml/dt/decision_forest.h index 83d8e799..02cc485a 100644 --- a/ml/include/pcl/ml/dt/decision_forest.h +++ b/ml/include/pcl/ml/dt/decision_forest.h @@ -38,7 +38,6 @@ #pragma once #include - #include #include diff --git a/ml/include/pcl/ml/dt/decision_forest_evaluator.h b/ml/include/pcl/ml/dt/decision_forest_evaluator.h index e4595ada..59652eb4 100644 --- a/ml/include/pcl/ml/dt/decision_forest_evaluator.h +++ b/ml/include/pcl/ml/dt/decision_forest_evaluator.h @@ -38,7 +38,6 @@ #pragma once #include - #include #include #include diff --git a/ml/include/pcl/ml/dt/decision_forest_trainer.h b/ml/include/pcl/ml/dt/decision_forest_trainer.h index 499a256f..b1771986 100644 --- a/ml/include/pcl/ml/dt/decision_forest_trainer.h +++ b/ml/include/pcl/ml/dt/decision_forest_trainer.h @@ -38,7 +38,6 @@ #pragma once #include - #include #include #include diff --git a/ml/include/pcl/ml/dt/decision_tree_data_provider.h b/ml/include/pcl/ml/dt/decision_tree_data_provider.h index fc13b283..96b7b55a 100644 --- a/ml/include/pcl/ml/dt/decision_tree_data_provider.h +++ b/ml/include/pcl/ml/dt/decision_tree_data_provider.h @@ -37,9 +37,9 @@ #pragma once -#include - #include +#include +#include // for PCL_EXPORTS namespace pcl { template - #include #include #include diff --git a/ml/include/pcl/ml/dt/decision_tree_trainer.h b/ml/include/pcl/ml/dt/decision_tree_trainer.h index 7513b669..3920182a 100644 --- a/ml/include/pcl/ml/dt/decision_tree_trainer.h +++ b/ml/include/pcl/ml/dt/decision_tree_trainer.h @@ -38,7 +38,6 @@ #pragma once #include - #include #include #include diff --git a/ml/include/pcl/ml/ferns/fern.h b/ml/include/pcl/ml/ferns/fern.h index 6477dde0..77fb46f5 100644 --- a/ml/include/pcl/ml/ferns/fern.h +++ b/ml/include/pcl/ml/ferns/fern.h @@ -142,7 +142,8 @@ public: * * \param node_index the index of the node to access */ - inline NodeType& operator[](const std::size_t node_index) + inline NodeType& + operator[](const std::size_t node_index) { return nodes_[node_index]; } @@ -151,7 +152,8 @@ public: * * \param node_index the index of the node to access */ - inline const NodeType& operator[](const std::size_t node_index) const + inline const NodeType& + operator[](const std::size_t node_index) const { return nodes_[node_index]; } diff --git a/ml/include/pcl/ml/ferns/fern_evaluator.h b/ml/include/pcl/ml/ferns/fern_evaluator.h index f869b44a..d3f3c461 100644 --- a/ml/include/pcl/ml/ferns/fern_evaluator.h +++ b/ml/include/pcl/ml/ferns/fern_evaluator.h @@ -38,7 +38,6 @@ #pragma once #include - #include #include #include diff --git a/ml/include/pcl/ml/ferns/fern_trainer.h b/ml/include/pcl/ml/ferns/fern_trainer.h index f07f60ea..cda71bc3 100644 --- a/ml/include/pcl/ml/ferns/fern_trainer.h +++ b/ml/include/pcl/ml/ferns/fern_trainer.h @@ -38,7 +38,6 @@ #pragma once #include - #include #include #include diff --git a/ml/include/pcl/ml/impl/dt/decision_forest_evaluator.hpp b/ml/include/pcl/ml/impl/dt/decision_forest_evaluator.hpp index b9cfee01..61243ccc 100644 --- a/ml/include/pcl/ml/impl/dt/decision_forest_evaluator.hpp +++ b/ml/include/pcl/ml/impl/dt/decision_forest_evaluator.hpp @@ -38,7 +38,6 @@ #pragma once #include - #include #include #include diff --git a/ml/include/pcl/ml/impl/dt/decision_forest_trainer.hpp b/ml/include/pcl/ml/impl/dt/decision_forest_trainer.hpp index 019454e6..23b37961 100644 --- a/ml/include/pcl/ml/impl/dt/decision_forest_trainer.hpp +++ b/ml/include/pcl/ml/impl/dt/decision_forest_trainer.hpp @@ -37,12 +37,14 @@ #pragma once +namespace pcl { + template -pcl::DecisionForestTrainer:: +DecisionForestTrainer:: DecisionForestTrainer() : num_of_trees_to_train_(1), decision_tree_trainer_() {} @@ -52,7 +54,7 @@ template -pcl::DecisionForestTrainer:: +DecisionForestTrainer:: ~DecisionForestTrainer() {} @@ -62,8 +64,8 @@ template void -pcl::DecisionForestTrainer:: - train(pcl::DecisionForest& forest) +DecisionForestTrainer::train( + pcl::DecisionForest& forest) { for (std::size_t tree_index = 0; tree_index < num_of_trees_to_train_; ++tree_index) { pcl::DecisionTree tree; @@ -72,3 +74,5 @@ pcl::DecisionForestTrainer - #include #include #include #include +namespace pcl { + template -pcl::DecisionTreeEvaluator:: +DecisionTreeEvaluator:: DecisionTreeEvaluator() {} @@ -59,7 +60,7 @@ template -pcl::DecisionTreeEvaluator:: +DecisionTreeEvaluator:: ~DecisionTreeEvaluator() {} @@ -69,7 +70,7 @@ template void -pcl::DecisionTreeEvaluator:: +DecisionTreeEvaluator:: evaluate(pcl::DecisionTree& tree, pcl::FeatureHandler& feature_handler, pcl::StatsEstimator& @@ -106,7 +107,7 @@ template void -pcl::DecisionTreeEvaluator:: +DecisionTreeEvaluator:: evaluateAndAdd( pcl::DecisionTree& tree, pcl::FeatureHandler& feature_handler, @@ -143,7 +144,7 @@ template void -pcl::DecisionTreeEvaluator:: +DecisionTreeEvaluator:: evaluate(pcl::DecisionTree& tree, pcl::FeatureHandler& feature_handler, pcl::StatsEstimator& @@ -152,7 +153,6 @@ pcl::DecisionTreeEvaluatorsub_nodes.empty()) { @@ -177,7 +177,7 @@ template void -pcl::DecisionTreeEvaluator:: +DecisionTreeEvaluator:: getNodes(pcl::DecisionTree& tree, pcl::FeatureHandler& feature_handler, pcl::StatsEstimator& @@ -206,3 +206,5 @@ pcl::DecisionTreeEvaluator -pcl::DecisionTreeTrainer:: +DecisionTreeTrainer:: DecisionTreeTrainer() : max_tree_depth_(15) , num_of_features_(1000) @@ -61,7 +63,7 @@ template -pcl::DecisionTreeTrainer:: +DecisionTreeTrainer:: ~DecisionTreeTrainer() {} @@ -71,8 +73,8 @@ template void -pcl::DecisionTreeTrainer:: - train(pcl::DecisionTree& tree) +DecisionTreeTrainer::train( + pcl::DecisionTree& tree) { // create random features std::vector features; @@ -107,7 +109,7 @@ template void -pcl::DecisionTreeTrainer:: +DecisionTreeTrainer:: trainDecisionTreeNode(std::vector& features, std::vector& examples, std::vector& label_data, @@ -272,7 +274,7 @@ template void -pcl::DecisionTreeTrainer:: +DecisionTreeTrainer:: createThresholdsUniform(const std::size_t num_of_thresholds, std::vector& values, std::vector& thresholds) @@ -303,3 +305,5 @@ pcl::DecisionTreeTrainer(threshold_index + 1)); } } + +} // namespace pcl diff --git a/ml/include/pcl/ml/impl/ferns/fern_evaluator.hpp b/ml/include/pcl/ml/impl/ferns/fern_evaluator.hpp index ade2cc83..28bb49a1 100644 --- a/ml/include/pcl/ml/impl/ferns/fern_evaluator.hpp +++ b/ml/include/pcl/ml/impl/ferns/fern_evaluator.hpp @@ -38,20 +38,20 @@ #pragma once #include - #include #include #include #include +namespace pcl { + template -pcl::FernEvaluator:: - FernEvaluator() +FernEvaluator::FernEvaluator() {} template -pcl::FernEvaluator:: - ~FernEvaluator() +FernEvaluator::~FernEvaluator() {} template void -pcl::FernEvaluator::evaluate( +FernEvaluator::evaluate( pcl::Fern& fern, pcl::FeatureHandler& feature_handler, pcl::StatsEstimator& stats_estimator, @@ -123,15 +122,13 @@ template void -pcl::FernEvaluator:: - evaluateAndAdd( - pcl::Fern& fern, - pcl::FeatureHandler& feature_handler, - pcl::StatsEstimator& - stats_estimator, - DataSet& data_set, - std::vector& examples, - std::vector& label_data) +FernEvaluator::evaluateAndAdd( + pcl::Fern& fern, + pcl::FeatureHandler& feature_handler, + pcl::StatsEstimator& stats_estimator, + DataSet& data_set, + std::vector& examples, + std::vector& label_data) { const std::size_t num_of_examples = examples.size(); const std::size_t num_of_branches = stats_estimator.getNumOfBranches(); @@ -177,7 +174,7 @@ template void -pcl::FernEvaluator::getNodes( +FernEvaluator::getNodes( pcl::Fern& fern, pcl::FeatureHandler& feature_handler, pcl::StatsEstimator& stats_estimator, @@ -224,3 +221,5 @@ pcl::FernEvaluator::get nodes.push_back(&(fern[node_index])); } } + +} // namespace pcl diff --git a/ml/include/pcl/ml/impl/ferns/fern_trainer.hpp b/ml/include/pcl/ml/impl/ferns/fern_trainer.hpp index 608c56ff..182869c2 100644 --- a/ml/include/pcl/ml/impl/ferns/fern_trainer.hpp +++ b/ml/include/pcl/ml/impl/ferns/fern_trainer.hpp @@ -37,12 +37,14 @@ #pragma once +namespace pcl { + template -pcl::FernTrainer::FernTrainer() +FernTrainer::FernTrainer() : fern_depth_(10) , num_of_features_(1000) , num_of_thresholds_(10) @@ -58,8 +60,7 @@ template -pcl::FernTrainer:: - ~FernTrainer() +FernTrainer::~FernTrainer() {} template void -pcl::FernTrainer::train( +FernTrainer::train( pcl::Fern& fern) { const std::size_t num_of_branches = stats_estimator_->getNumOfBranches(); @@ -295,7 +296,7 @@ template void -pcl::FernTrainer:: +FernTrainer:: createThresholdsUniform(const std::size_t num_of_thresholds, std::vector& values, std::vector& thresholds) @@ -325,3 +326,5 @@ pcl::FernTrainer:: thresholds[threshold_index] = min_value + step * (threshold_index + 1); } } + +} // namespace pcl diff --git a/ml/include/pcl/ml/impl/kmeans.hpp b/ml/include/pcl/ml/impl/kmeans.hpp index ee02c1ef..9933b50b 100644 --- a/ml/include/pcl/ml/impl/kmeans.hpp +++ b/ml/include/pcl/ml/impl/kmeans.hpp @@ -47,22 +47,18 @@ //#include //#include +namespace pcl { template -pcl::Kmeans::Kmeans() : cluster_field_name_("") +Kmeans::Kmeans() : cluster_field_name_("") {} template -pcl::Kmeans::~Kmeans() +Kmeans::~Kmeans() {} template void -pcl::Kmeans::k_means() -{} - -template -void -pcl::Kmeans::cluster(std::vector& clusters) +Kmeans::cluster(std::vector& clusters) { if (!initCompute() || (input_ != 0 && input_->points.empty()) || (indices_ != 0 && indices_->empty())) { @@ -162,5 +158,6 @@ pcl::Kmeans::cluster(std::vector& clusters) deinitCompute(); } +} // namespace pcl #define PCL_INSTANTIATE_Kmeans(T) template class PCL_EXPORTS pcl::Kmeans; diff --git a/ml/include/pcl/ml/kmeans.h b/ml/include/pcl/ml/kmeans.h index 1cac4e6d..af77608c 100644 --- a/ml/include/pcl/ml/kmeans.h +++ b/ml/include/pcl/ml/kmeans.h @@ -39,16 +39,16 @@ #pragma once -#include - #include #include #include +#include +#include +#include #include #include -#include -#include +#include namespace pcl { diff --git a/ml/include/pcl/ml/multi_channel_2d_comparison_feature_handler.h b/ml/include/pcl/ml/multi_channel_2d_comparison_feature_handler.h index 93d9372b..0c0991c9 100644 --- a/ml/include/pcl/ml/multi_channel_2d_comparison_feature_handler.h +++ b/ml/include/pcl/ml/multi_channel_2d_comparison_feature_handler.h @@ -38,7 +38,6 @@ #pragma once #include - #include #include #include diff --git a/ml/include/pcl/ml/pairwise_potential.h b/ml/include/pcl/ml/pairwise_potential.h index 9b16bdc5..c37beba0 100644 --- a/ml/include/pcl/ml/pairwise_potential.h +++ b/ml/include/pcl/ml/pairwise_potential.h @@ -39,10 +39,10 @@ #pragma once -#include - #include +#include + namespace pcl { class PairwisePotential { diff --git a/ml/include/pcl/ml/permutohedral.h b/ml/include/pcl/ml/permutohedral.h index bfee8267..c2ca9e2e 100644 --- a/ml/include/pcl/ml/permutohedral.h +++ b/ml/include/pcl/ml/permutohedral.h @@ -41,21 +41,22 @@ #pragma GCC system_header #endif -#include -#include #include -#include +#include +#include + +#include -// TODO: SWAP with Boost intrusive hash table #include #include #include #include #include - -#include +#include +#include namespace pcl { + /** Implementation of a high-dimensional gaussian filtering using the permutohedral * lattice. * diff --git a/ml/include/pcl/ml/svm_wrapper.h b/ml/include/pcl/ml/svm_wrapper.h index 0e7a63b6..6604d969 100644 --- a/ml/include/pcl/ml/svm_wrapper.h +++ b/ml/include/pcl/ml/svm_wrapper.h @@ -38,6 +38,9 @@ #pragma once +#include +#include + #include #include #include @@ -45,10 +48,7 @@ #include #include #include -#include #include - -#include #define Malloc(type, n) static_cast(malloc((n) * sizeof(type))) namespace pcl { @@ -163,7 +163,7 @@ protected: /** Convert the libSVM format (svm_problem) into a easier output format. */ void - adaptLibSVMToInput(std::vector& training_set, svm_problem prob); + adaptLibSVMToInput(std::vector& training_set, svm_problem prob) const; /** Load a problem from an extern file. */ bool diff --git a/ml/src/densecrf.cpp b/ml/src/densecrf.cpp index a152e078..effdf09d 100644 --- a/ml/src/densecrf.cpp +++ b/ml/src/densecrf.cpp @@ -235,7 +235,7 @@ void pcl::DenseCrf::expAndNormalize(std::vector& out, const std::vector& in, float scale, - float relax) + float relax) const { std::vector V(N_ + 10); for (int i = 0; i < N_; i++) { diff --git a/ml/src/svm.cpp b/ml/src/svm.cpp index 93d5df23..74cba05a 100644 --- a/ml/src/svm.cpp +++ b/ml/src/svm.cpp @@ -39,6 +39,8 @@ #ifndef _LIBSVM_HPP_ #define _LIBSVM_HPP_ +#include + #include #include #include @@ -48,7 +50,6 @@ #include #include #include -#include int libsvm_version = LIBSVM_VERSION; using Qfloat = float; using schar = signed char; @@ -3178,6 +3179,7 @@ svm_load_model(const char* model_file_name) free(model->rho); free(model->label); free(model->nSV); + free(model->scaling); free(model); return nullptr; } @@ -3198,6 +3200,7 @@ svm_load_model(const char* model_file_name) free(model->rho); free(model->label); free(model->nSV); + free(model->scaling); free(model); return nullptr; } @@ -3297,6 +3300,7 @@ svm_load_model(const char* model_file_name) free(model->rho); free(model->label); free(model->nSV); + free(model->scaling); free(model); return nullptr; } @@ -3385,8 +3389,14 @@ svm_load_model(const char* model_file_name) // printf("%d e %f\n",model->scaling[j-2].index,model->scaling[j-2].value); - if (ferror(fp) != 0 || fclose(fp) != 0) + if (ferror(fp) != 0 || fclose(fp) != 0) { + free(model->rho); + free(model->label); + free(model->nSV); + free(model->scaling); + free(model); return nullptr; + } model->free_sv = 1; // XXX diff --git a/ml/src/svm_wrapper.cpp b/ml/src/svm_wrapper.cpp index ae3d9aa5..29df338b 100644 --- a/ml/src/svm_wrapper.cpp +++ b/ml/src/svm_wrapper.cpp @@ -39,9 +39,10 @@ #ifndef PCL_SVM_WRAPPER_HPP_ #define PCL_SVM_WRAPPER_HPP_ +#include + #include #include -#include char* pcl::SVM::readline(FILE* input) @@ -145,7 +146,7 @@ pcl::SVMTrain::scaleFactors(std::vector training_set, svm_scaling& scal }; void -pcl::SVM::adaptLibSVMToInput(std::vector& training_set, svm_problem prob) +pcl::SVM::adaptLibSVMToInput(std::vector& training_set, svm_problem prob) const { training_set.clear(); // Reset input diff --git a/octree/include/pcl/octree/impl/octree_base.hpp b/octree/include/pcl/octree/impl/octree_base.hpp index d7521bc6..5cca0142 100644 --- a/octree/include/pcl/octree/impl/octree_base.hpp +++ b/octree/include/pcl/octree/impl/octree_base.hpp @@ -39,10 +39,10 @@ #ifndef PCL_OCTREE_BASE_HPP #define PCL_OCTREE_BASE_HPP -#include - #include +#include + namespace pcl { namespace octree { ////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/octree/include/pcl/octree/impl/octree_pointcloud.hpp b/octree/include/pcl/octree/impl/octree_pointcloud.hpp index d2ee3733..eb5a4991 100644 --- a/octree/include/pcl/octree/impl/octree_pointcloud.hpp +++ b/octree/include/pcl/octree/impl/octree_pointcloud.hpp @@ -36,14 +36,14 @@ * $Id$ */ -#ifndef PCL_OCTREE_POINTCLOUD_HPP_ -#define PCL_OCTREE_POINTCLOUD_HPP_ - -#include +#pragma once #include +#include // for pcl::isFinite #include +#include + ////////////////////////////////////////////////////////////////////////////////////////////// template 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 adf3ac5c..ba7cc8f5 100644 --- a/octree/include/pcl/octree/impl/octree_pointcloud_adjacency.hpp +++ b/octree/include/pcl/octree/impl/octree_pointcloud_adjacency.hpp @@ -35,11 +35,12 @@ * */ -#ifndef PCL_OCTREE_POINTCLOUD_ADJACENCY_HPP_ -#define PCL_OCTREE_POINTCLOUD_ADJACENCY_HPP_ +#pragma once #include +#include // for pcl::isFinite #include + /* * OctreePointCloudAdjacency is not precompiled, since it's used in other * parts of PCL with custom LeafContainers. So if PCL_NO_PRECOMPILE is NOT @@ -330,5 +331,3 @@ 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_search.hpp b/octree/include/pcl/octree/impl/octree_search.hpp index 9a72e3dc..16b98ce0 100644 --- a/octree/include/pcl/octree/impl/octree_search.hpp +++ b/octree/include/pcl/octree/impl/octree_search.hpp @@ -41,11 +41,14 @@ #include -////////////////////////////////////////////////////////////////////////////////////////////// +namespace pcl { + +namespace octree { + template bool -pcl::octree::OctreePointCloudSearch:: - voxelSearch(const PointT& point, std::vector& point_idx_data) +OctreePointCloudSearch::voxelSearch( + const PointT& point, std::vector& point_idx_data) { assert(isFinite(point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"); @@ -65,24 +68,22 @@ pcl::octree::OctreePointCloudSearch:: return (b_success); } -////////////////////////////////////////////////////////////////////////////////////////////// template bool -pcl::octree::OctreePointCloudSearch:: - voxelSearch(const int index, std::vector& point_idx_data) +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)); } -////////////////////////////////////////////////////////////////////////////////////////////// template int -pcl::octree::OctreePointCloudSearch:: - nearestKSearch(const PointT& p_q, - int k, - std::vector& k_indices, - std::vector& k_sqr_distances) +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) && @@ -119,24 +120,19 @@ pcl::octree::OctreePointCloudSearch:: 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) +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)); } -////////////////////////////////////////////////////////////////////////////////////////////// template void -pcl::octree::OctreePointCloudSearch:: - approxNearestSearch(const PointT& p_q, int& result_index, float& sqr_distance) +OctreePointCloudSearch::approxNearestSearch( + const PointT& p_q, int& result_index, float& sqr_distance) { assert(this->leaf_count_ > 0); assert(isFinite(p_q) && @@ -151,26 +147,24 @@ pcl::octree::OctreePointCloudSearch:: return; } -////////////////////////////////////////////////////////////////////////////////////////////// template void -pcl::octree::OctreePointCloudSearch:: - approxNearestSearch(int query_index, int& result_index, float& sqr_distance) +OctreePointCloudSearch::approxNearestSearch( + int query_index, int& result_index, float& sqr_distance) { const PointT search_point = this->getPointByIndex(query_index); 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 +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!"); @@ -192,28 +186,26 @@ pcl::octree::OctreePointCloudSearch:: 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 +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); 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 +OctreePointCloudSearch::boxSearch( + const Eigen::Vector3f& min_pt, + const Eigen::Vector3f& max_pt, + std::vector& k_indices) const { OctreeKey key; @@ -226,10 +218,9 @@ pcl::octree::OctreePointCloudSearch:: return (static_cast(k_indices.size())); } -////////////////////////////////////////////////////////////////////////////////////////////// template double -pcl::octree::OctreePointCloudSearch:: +OctreePointCloudSearch:: getKNearestNeighborRecursive( const PointT& point, unsigned int K, @@ -339,10 +330,9 @@ pcl::octree::OctreePointCloudSearch:: return (smallest_squared_dist); } -////////////////////////////////////////////////////////////////////////////////////////////// template void -pcl::octree::OctreePointCloudSearch:: +OctreePointCloudSearch:: getNeighborsWithinRadiusRecursive(const PointT& point, const double radiusSquared, const BranchNode* node, @@ -427,10 +417,9 @@ pcl::octree::OctreePointCloudSearch:: } } -////////////////////////////////////////////////////////////////////////////////////////////// template void -pcl::octree::OctreePointCloudSearch:: +OctreePointCloudSearch:: approxNearestSearchRecursive(const PointT& point, const BranchNode* node, const OctreeKey& key, @@ -517,25 +506,23 @@ pcl::octree::OctreePointCloudSearch:: } } -////////////////////////////////////////////////////////////////////////////////////////////// template float -pcl::octree::OctreePointCloudSearch:: - pointSquaredDist(const PointT& point_a, const PointT& point_b) const +OctreePointCloudSearch::pointSquaredDist( + const PointT& point_a, const PointT& point_b) const { 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 +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++) { @@ -602,10 +589,9 @@ pcl::octree::OctreePointCloudSearch:: } } -////////////////////////////////////////////////////////////////////////////////////////////// template int -pcl::octree::OctreePointCloudSearch:: +OctreePointCloudSearch:: getIntersectedVoxelCenters(Eigen::Vector3f origin, Eigen::Vector3f direction, AlignedPointTVector& voxel_center_list, @@ -639,10 +625,9 @@ pcl::octree::OctreePointCloudSearch:: return (0); } -////////////////////////////////////////////////////////////////////////////////////////////// template int -pcl::octree::OctreePointCloudSearch:: +OctreePointCloudSearch:: getIntersectedVoxelIndices(Eigen::Vector3f origin, Eigen::Vector3f direction, std::vector& k_indices, @@ -674,10 +659,9 @@ pcl::octree::OctreePointCloudSearch:: return (0); } -////////////////////////////////////////////////////////////////////////////////////////////// template int -pcl::octree::OctreePointCloudSearch:: +OctreePointCloudSearch:: getIntersectedVoxelCentersRecursive(double min_x, double min_y, double min_z, @@ -871,10 +855,9 @@ pcl::octree::OctreePointCloudSearch:: return (voxel_count); } -////////////////////////////////////////////////////////////////////////////////////////////// template int -pcl::octree::OctreePointCloudSearch:: +OctreePointCloudSearch:: getIntersectedVoxelIndicesRecursive(double min_x, double min_y, double min_z, @@ -1065,6 +1048,9 @@ pcl::octree::OctreePointCloudSearch:: return (voxel_count); } +} // namespace octree +} // namespace pcl + #define PCL_INSTANTIATE_OctreePointCloudSearch(T) \ template class PCL_EXPORTS pcl::octree::OctreePointCloudSearch; diff --git a/octree/include/pcl/octree/octree.h b/octree/include/pcl/octree/octree.h index 074ffbf9..2767a302 100644 --- a/octree/include/pcl/octree/octree.h +++ b/octree/include/pcl/octree/octree.h @@ -42,7 +42,6 @@ #include #include #include - #include #include #include @@ -50,5 +49,4 @@ #include #include #include - #include diff --git a/octree/include/pcl/octree/octree2buf_base.h b/octree/include/pcl/octree/octree2buf_base.h index 39157230..6d9e70ee 100644 --- a/octree/include/pcl/octree/octree2buf_base.h +++ b/octree/include/pcl/octree/octree2buf_base.h @@ -38,14 +38,14 @@ #pragma once -#include - #include #include #include #include #include +#include + namespace pcl { namespace octree { @@ -139,16 +139,32 @@ public: } /** \brief Get const pointer to container */ - const ContainerT* operator->() const { return &container_; } + const ContainerT* + operator->() const + { + return &container_; + } /** \brief Get pointer to container */ - ContainerT* operator->() { return &container_; } + ContainerT* + operator->() + { + return &container_; + } /** \brief Get const reference to container */ - const ContainerT& operator*() const { return container_; } + const ContainerT& + operator*() const + { + return container_; + } /** \brief Get reference to container */ - ContainerT& operator*() { return container_; } + ContainerT& + operator*() + { + return container_; + } /** \brief Get const reference to container */ const ContainerT& @@ -237,14 +253,16 @@ public: using LeafNodeIterator = OctreeLeafNodeDepthFirstIterator; using ConstLeafNodeIterator = const OctreeLeafNodeDepthFirstIterator; - PCL_DEPRECATED("use leaf_depth_begin() instead") + PCL_DEPRECATED(1, 12, "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() + PCL_DEPRECATED(1, 12, "use leaf_depth_end() instead") + const LeafNodeIterator + leaf_end() { return LeafNodeIterator(); }; @@ -938,7 +956,7 @@ protected: * \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) + PCL_DEPRECATED(1, 12, "use std::log2 instead") inline double Log2(double n_arg) { return std::log2(n_arg); } diff --git a/octree/include/pcl/octree/octree_base.h b/octree/include/pcl/octree/octree_base.h index 026d65d4..c487b924 100644 --- a/octree/include/pcl/octree/octree_base.h +++ b/octree/include/pcl/octree/octree_base.h @@ -38,16 +38,17 @@ #pragma once -#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). @@ -126,14 +127,16 @@ public: using LeafNodeIterator = OctreeLeafNodeDepthFirstIterator; using ConstLeafNodeIterator = const OctreeLeafNodeDepthFirstIterator; - PCL_DEPRECATED("use leaf_depth_begin() instead") + PCL_DEPRECATED(1, 12, "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() + PCL_DEPRECATED(1, 12, "use leaf_depth_end() instead") + const LeafNodeIterator + leaf_end() { return LeafNodeIterator(this, 0, nullptr); }; @@ -247,6 +250,8 @@ public: { leaf_count_ = source.leaf_count_; branch_count_ = source.branch_count_; + delete root_node_; + root_node_ = new (BranchNode)(*(source.root_node_)); depth_mask_ = source.depth_mask_; max_key_ = source.max_key_; @@ -396,7 +401,7 @@ protected: createLeaf(const OctreeKey& key_arg) { - LeafNode* leaf_node; + LeafNode* leaf_node = nullptr; BranchNode* leaf_node_parent; createLeafRecursive(key_arg, depth_mask_, root_node_, leaf_node, leaf_node_parent); @@ -686,7 +691,7 @@ protected: * \param n_arg: some value * \return binary logarithm (log2) of argument n_arg */ - PCL_DEPRECATED("use std::log2 instead") double Log2(double n_arg) + PCL_DEPRECATED(1, 12, "use std::log2 instead") double Log2(double n_arg) { return std::log2(n_arg); } diff --git a/octree/include/pcl/octree/octree_container.h b/octree/include/pcl/octree/octree_container.h index 83238004..c4655b7c 100644 --- a/octree/include/pcl/octree/octree_container.h +++ b/octree/include/pcl/octree/octree_container.h @@ -38,13 +38,13 @@ #pragma once +#include #include #include -#include - namespace pcl { namespace octree { + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** \brief @b Octree container class that can serve as a base to construct own leaf node * container classes. diff --git a/octree/include/pcl/octree/octree_impl.h b/octree/include/pcl/octree/octree_impl.h index c8fa320b..190c0111 100644 --- a/octree/include/pcl/octree/octree_impl.h +++ b/octree/include/pcl/octree/octree_impl.h @@ -38,10 +38,9 @@ #pragma once -#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 2d20cf2b..2156b327 100644 --- a/octree/include/pcl/octree/octree_iterator.h +++ b/octree/include/pcl/octree/octree_iterator.h @@ -39,14 +39,13 @@ #pragma once -#include -#include -#include - #include #include +#include +#include #include +#include // Ignore warnings in the above headers #ifdef __GNUC__ @@ -218,7 +217,8 @@ public: /** \brief *operator. * \return pointer to the current octree node */ - inline OctreeNode* operator*() const + inline OctreeNode* + operator*() const { // return designated object if (octree_ && current_state_) { return (current_state_->node_); @@ -728,7 +728,8 @@ public: /** \brief *operator. * \return pointer to the current octree leaf node */ - OctreeNode* operator*() const + OctreeNode* + operator*() const { // return designated object OctreeNode* ret = 0; diff --git a/octree/include/pcl/octree/octree_key.h b/octree/include/pcl/octree/octree_key.h index 6ba5196a..a103d5a3 100644 --- a/octree/include/pcl/octree/octree_key.h +++ b/octree/include/pcl/octree/octree_key.h @@ -38,6 +38,7 @@ #pragma once #include +#include // for memcpy namespace pcl { namespace octree { @@ -59,7 +60,7 @@ public: {} /** \brief Copy constructor. */ - OctreeKey(const OctreeKey& source) { memcpy(key_, source.key_, sizeof(key_)); } + OctreeKey(const OctreeKey& source) { std::memcpy(key_, source.key_, sizeof(key_)); } OctreeKey& operator=(const OctreeKey&) = default; diff --git a/octree/include/pcl/octree/octree_node_pool.h b/octree/include/pcl/octree/octree_node_pool.h index f261eb83..568e28bf 100644 --- a/octree/include/pcl/octree/octree_node_pool.h +++ b/octree/include/pcl/octree/octree_node_pool.h @@ -37,10 +37,10 @@ #pragma once -#include - #include +#include + namespace pcl { namespace octree { diff --git a/octree/include/pcl/octree/octree_nodes.h b/octree/include/pcl/octree/octree_nodes.h index 0277fc98..e00182df 100644 --- a/octree/include/pcl/octree/octree_nodes.h +++ b/octree/include/pcl/octree/octree_nodes.h @@ -38,15 +38,14 @@ #pragma once -#include - -#include +#include +#include +#include #include -#include - -#include "octree_container.h" +#include +#include namespace pcl { namespace octree { @@ -111,16 +110,32 @@ public: } /** \brief Get const pointer to container */ - const ContainerT* operator->() const { return &container_; } + const ContainerT* + operator->() const + { + return &container_; + } /** \brief Get pointer to container */ - ContainerT* operator->() { return &container_; } + ContainerT* + operator->() + { + return &container_; + } /** \brief Get const reference to container */ - const ContainerT& operator*() const { return container_; } + const ContainerT& + operator*() const + { + return container_; + } /** \brief Get reference to container */ - ContainerT& operator*() { return container_; } + ContainerT& + operator*() + { + return container_; + } /** \brief Get const reference to container */ const ContainerT& @@ -210,7 +225,8 @@ public: * \param child_idx_arg: index to child node * \return OctreeNode pointer * */ - inline OctreeNode*& operator[](unsigned char child_idx_arg) + inline OctreeNode*& + operator[](unsigned char child_idx_arg) { assert(child_idx_arg < 8); return child_node_array_[child_idx_arg]; @@ -287,16 +303,32 @@ public: } /** \brief Get const pointer to container */ - const ContainerT* operator->() const { return &container_; } + const ContainerT* + operator->() const + { + return &container_; + } /** \brief Get pointer to container */ - ContainerT* operator->() { return &container_; } + ContainerT* + operator->() + { + return &container_; + } /** \brief Get const reference to container */ - const ContainerT& operator*() const { return container_; } + const ContainerT& + operator*() const + { + return container_; + } /** \brief Get reference to container */ - ContainerT& operator*() { return container_; } + ContainerT& + operator*() + { + return container_; + } /** \brief Get const reference to container */ const ContainerT& diff --git a/octree/include/pcl/octree/octree_pointcloud.h b/octree/include/pcl/octree/octree_pointcloud.h index 929c3c4c..4f66675c 100644 --- a/octree/include/pcl/octree/octree_pointcloud.h +++ b/octree/include/pcl/octree/octree_pointcloud.h @@ -39,7 +39,6 @@ #pragma once #include - #include #include diff --git a/octree/include/pcl/octree/octree_pointcloud_changedetector.h b/octree/include/pcl/octree/octree_pointcloud_changedetector.h index 8d2a3b4a..902dfd2c 100644 --- a/octree/include/pcl/octree/octree_pointcloud_changedetector.h +++ b/octree/include/pcl/octree/octree_pointcloud_changedetector.h @@ -38,10 +38,9 @@ #pragma once -#include - #include #include +#include namespace pcl { namespace octree { diff --git a/octree/include/pcl/octree/octree_search.h b/octree/include/pcl/octree/octree_search.h index 47b78182..6d97a606 100644 --- a/octree/include/pcl/octree/octree_search.h +++ b/octree/include/pcl/octree/octree_search.h @@ -38,12 +38,12 @@ #pragma once -#include - #include +#include namespace pcl { namespace octree { + /** \brief @b Octree pointcloud search class * \note This class provides several methods for spatial neighbor search based on octree * structure @@ -339,8 +339,8 @@ protected: 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 + * \param[in] point_idx index for a dataset point 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) diff --git a/outofcore/include/pcl/outofcore/impl/octree_base.hpp b/outofcore/include/pcl/outofcore/impl/octree_base.hpp index 4ed6c3da..acf0b596 100644 --- a/outofcore/include/pcl/outofcore/impl/octree_base.hpp +++ b/outofcore/include/pcl/outofcore/impl/octree_base.hpp @@ -475,8 +475,11 @@ namespace pcl //////////////////////////////////////////////////////////////////////////////// template void - OutofcoreOctreeBase::DeAllocEmptyNodeCache (OutofcoreOctreeBaseNode* current) + OutofcoreOctreeBase::DeAllocEmptyNodeCache (OutofcoreOctreeBaseNode* current) { + if (current == nullptr) + current = root_node_; + if (current->size () == 0) { current->flush_DeAlloc_this_only (); @@ -486,7 +489,6 @@ namespace pcl { DeAllocEmptyNodeCache (current->children[i]); } - } //////////////////////////////////////////////////////////////////////////////// diff --git a/outofcore/include/pcl/outofcore/impl/octree_base_node.hpp b/outofcore/include/pcl/outofcore/impl/octree_base_node.hpp index ece9288c..8492c417 100644 --- a/outofcore/include/pcl/outofcore/impl/octree_base_node.hpp +++ b/outofcore/include/pcl/outofcore/impl/octree_base_node.hpp @@ -337,11 +337,7 @@ namespace pcl for (std::size_t i = 0; i < 8; i++) { - if (children_[i]) - { - OutofcoreOctreeBaseNode* current = children_[i]; - delete (current); - } + delete static_cast*>(children_[i]); } children_.resize (8, static_cast* > (nullptr)); num_children_ = 0; diff --git a/outofcore/include/pcl/outofcore/octree_base.h b/outofcore/include/pcl/outofcore/octree_base.h index 43cdbbc5..8152c9d3 100644 --- a/outofcore/include/pcl/outofcore/octree_base.h +++ b/outofcore/include/pcl/outofcore/octree_base.h @@ -611,22 +611,15 @@ namespace pcl bool checkExtension (const boost::filesystem::path& path_name); - - /** \brief DEPRECATED - Flush all nodes' cache - * \deprecated this was moved to the octree_node class - */ + /** \brief Flush all nodes' cache */ void flushToDisk (); - /** \brief DEPRECATED - Flush all non leaf nodes' cache - * \deprecated - */ + /** \brief Flush all non leaf nodes' cache */ void flushToDiskLazy (); - /** \brief DEPRECATED - Flush empty nodes only - * \deprecated - */ + /** \brief Flush empty nodes only */ void DeAllocEmptyNodeCache (); diff --git a/outofcore/include/pcl/outofcore/octree_base_node.h b/outofcore/include/pcl/outofcore/octree_base_node.h index 5c33e542..c65d8491 100644 --- a/outofcore/include/pcl/outofcore/octree_base_node.h +++ b/outofcore/include/pcl/outofcore/octree_base_node.h @@ -477,9 +477,7 @@ namespace pcl void loadFromFile (const boost::filesystem::path &path, OutofcoreOctreeBaseNode* super); - /** \brief Recursively converts data files to ascii XZY files - * \note This will be deprecated soon - */ + /** \brief Recursively converts data files to ascii XZY files */ void convertToXYZRecursive (); diff --git a/outofcore/include/pcl/outofcore/outofcore_base_data.h b/outofcore/include/pcl/outofcore/outofcore_base_data.h index 6b34b907..474c692f 100644 --- a/outofcore/include/pcl/outofcore/outofcore_base_data.h +++ b/outofcore/include/pcl/outofcore/outofcore_base_data.h @@ -38,6 +38,7 @@ #pragma once +#include #include #include #include diff --git a/outofcore/include/pcl/outofcore/outofcore_node_data.h b/outofcore/include/pcl/outofcore/outofcore_node_data.h index 4a95544b..54523791 100644 --- a/outofcore/include/pcl/outofcore/outofcore_node_data.h +++ b/outofcore/include/pcl/outofcore/outofcore_node_data.h @@ -38,6 +38,7 @@ #pragma once +#include #include #include #include diff --git a/outofcore/include/pcl/outofcore/visualization/camera.h b/outofcore/include/pcl/outofcore/visualization/camera.h index 608e5596..e2d57b13 100644 --- a/outofcore/include/pcl/outofcore/visualization/camera.h +++ b/outofcore/include/pcl/outofcore/visualization/camera.h @@ -7,6 +7,7 @@ // PCL #include #include +#include #include // VTK diff --git a/outofcore/include/pcl/outofcore/visualization/outofcore_cloud.h b/outofcore/include/pcl/outofcore/visualization/outofcore_cloud.h index dc282b70..fde2b177 100644 --- a/outofcore/include/pcl/outofcore/visualization/outofcore_cloud.h +++ b/outofcore/include/pcl/outofcore/visualization/outofcore_cloud.h @@ -7,6 +7,7 @@ #include // PCL +#include #include #include @@ -207,7 +208,7 @@ class OutofcoreCloud : public Object } int - getLodPixelThreshold () + getLodPixelThreshold () const { return lod_pixel_threshold_; } diff --git a/outofcore/tools/outofcore_viewer.cpp b/outofcore/tools/outofcore_viewer.cpp index bcced61c..5098aeb2 100644 --- a/outofcore/tools/outofcore_viewer.cpp +++ b/outofcore/tools/outofcore_viewer.cpp @@ -56,7 +56,9 @@ // PCL - visualziation //#include #include +#if VTK_RENDERING_BACKEND_OPENGL_VERSION < 2 #include +#endif //#include "vtkVBOPolyDataMapper.h" diff --git a/people/include/pcl/people/hog.h b/people/include/pcl/people/hog.h index 2c6d7536..5c55a57e 100644 --- a/people/include/pcl/people/hog.h +++ b/people/include/pcl/people/hog.h @@ -40,7 +40,7 @@ #pragma once -#include +#include // export macro namespace pcl { diff --git a/people/src/hog.cpp b/people/src/hog.cpp index fae44bc4..bc5cf63d 100644 --- a/people/src/hog.cpp +++ b/people/src/hog.cpp @@ -41,12 +41,13 @@ #include -#include +#include // for memcpy +#include // for std::min #if defined(__SSE2__) -#include + #include // sse methods #else -#include + #include #endif /** \brief Constructor. */ @@ -170,30 +171,32 @@ pcl::people::HOG::gradHist( float *M, float *O, int h, int w, int bin_size, int { const int hb=h/bin_size, wb=w/bin_size, h0=hb*bin_size, w0=wb*bin_size, nb=wb*hb; const float s=(float)bin_size, sInv=1/s, sInv2=1/s/s; - float *H0, *H1, *M0, *M1; int x, y; int *O0, *O1; + float *H0, *H1, *M0, *M1; int *O0, *O1; O0=(int*)alMalloc(h*sizeof(int),16); M0=(float*) alMalloc(h*sizeof(float),16); O1=(int*)alMalloc(h*sizeof(int),16); M1=(float*) alMalloc(h*sizeof(float),16); // main loop float xb = 0; float init = 0; - for( x=0; x=0; xb0 = hasLf?(int)xb:-1; hasRt = xb0 < wb-1; - xd=xb-xb0; xb+=sInv; yb=init; y=0; + xd=xb-xb0; xb+=sInv; yb=init; + int y=0; // lambda for code conciseness // @TODO: remove the very generic closure for specific variable one const auto GHinit = [&]() @@ -247,7 +251,7 @@ pcl::people::HOG::gradHist( float *M, float *O, int h, int w, int bin_size, int bool hasLf, hasRt; int xb0, yb0; if( x==0 ) { init=(0+.5f)*sInv-0.5f; xb=init; } hasLf = xb>=0; xb0 = hasLf?(int)xb:-1; hasRt = xb0 < wb-1; - xd=xb-xb0; xb+=sInv; yb=init; y=0; + xd=xb-xb0; xb+=sInv; yb=init; int y=0; // lambda for code conciseness const auto GHinit = [&]() { diff --git a/recognition/include/pcl/recognition/3rdparty/metslib/termination-criteria.hh b/recognition/include/pcl/recognition/3rdparty/metslib/termination-criteria.hh index 608eaea6..63bb6eec 100644 --- a/recognition/include/pcl/recognition/3rdparty/metslib/termination-criteria.hh +++ b/recognition/include/pcl/recognition/3rdparty/metslib/termination-criteria.hh @@ -163,9 +163,9 @@ namespace mets { termination_criteria_chain::reset(); } - int second_guess() { return second_guess_m; } - int iteration() { return total_iterations_m; } - int resets() { return resets_m; } + int second_guess() const { return second_guess_m; } + int iteration() const { return total_iterations_m; } + int resets() const { return resets_m; } protected: gol_type best_cost_m; diff --git a/recognition/include/pcl/recognition/auxiliary.h b/recognition/include/pcl/recognition/auxiliary.h deleted file mode 100644 index 8825cc75..00000000 --- a/recognition/include/pcl/recognition/auxiliary.h +++ /dev/null @@ -1,2 +0,0 @@ -#include -#error "Using pcl/recognition/auxiliary.h is deprecated, please use pcl/recognition/ransac_based/auxiliary.h instead." \ No newline at end of file diff --git a/recognition/include/pcl/recognition/bvh.h b/recognition/include/pcl/recognition/bvh.h deleted file mode 100644 index a4a69528..00000000 --- a/recognition/include/pcl/recognition/bvh.h +++ /dev/null @@ -1,2 +0,0 @@ -#include -#error "Using pcl/recognition/bvh.h is deprecated, please use pcl/recognition/ransac_based/bvh.h instead." \ No newline at end of file diff --git a/recognition/include/pcl/recognition/cg/hough_3d.h b/recognition/include/pcl/recognition/cg/hough_3d.h index 8d4d88d0..883f8dc8 100644 --- a/recognition/include/pcl/recognition/cg/hough_3d.h +++ b/recognition/include/pcl/recognition/cg/hough_3d.h @@ -41,6 +41,7 @@ #include #include +#include #include #include diff --git a/recognition/include/pcl/recognition/face_detection/face_common.h b/recognition/include/pcl/recognition/face_detection/face_common.h index fda2962a..f32ba9d5 100644 --- a/recognition/include/pcl/recognition/face_detection/face_common.h +++ b/recognition/include/pcl/recognition/face_detection/face_common.h @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include diff --git a/recognition/include/pcl/recognition/face_detection/face_detector_data_provider.h b/recognition/include/pcl/recognition/face_detection/face_detector_data_provider.h index 669dbbcf..22363af3 100644 --- a/recognition/include/pcl/recognition/face_detection/face_detector_data_provider.h +++ b/recognition/include/pcl/recognition/face_detection/face_detector_data_provider.h @@ -7,17 +7,18 @@ #pragma once +#include +#include +#include +#include + +#include +#include + #include #include #include -#include -#include -#include - -#include -#include -#include namespace bf = boost::filesystem; diff --git a/recognition/include/pcl/recognition/impl/cg/correspondence_grouping.hpp b/recognition/include/pcl/recognition/impl/cg/correspondence_grouping.hpp index 8df7502e..7c6e92e2 100644 --- a/recognition/include/pcl/recognition/impl/cg/correspondence_grouping.hpp +++ b/recognition/include/pcl/recognition/impl/cg/correspondence_grouping.hpp @@ -3,7 +3,7 @@ * * Point Cloud Library (PCL) - www.pointclouds.org * Copyright (c) 2010-2012, Willow Garage, Inc. - * + * * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -37,11 +37,16 @@ * */ + #ifndef PCL_RECOGNITION_CORRESPONDENCE_GROUPING_IMPL_H_ #define PCL_RECOGNITION_CORRESPONDENCE_GROUPING_IMPL_H_ + +namespace pcl +{ + template void -pcl::CorrespondenceGrouping::cluster (std::vector &clustered_corrs) +CorrespondenceGrouping::cluster (std::vector &clustered_corrs) { clustered_corrs.clear (); corr_group_scale_.clear (); @@ -57,4 +62,7 @@ pcl::CorrespondenceGrouping::cluster (std::vector::initialize() { pcl::ScopeTime tcues ("Computing clutter cues"); -#pragma omp parallel for schedule(dynamic, 4) num_threads(omp_get_num_procs()) +#pragma omp parallel for \ + default(none) \ + schedule(dynamic, 4) \ + num_threads(omp_get_num_procs()) for (int j = 0; j < static_cast (recognition_models_.size ()); j++) computeClutterCue (recognition_models_[j]); } diff --git a/recognition/include/pcl/recognition/impl/implicit_shape_model.hpp b/recognition/include/pcl/recognition/impl/implicit_shape_model.hpp index 8a06e689..9f74622f 100644 --- a/recognition/include/pcl/recognition/impl/implicit_shape_model.hpp +++ b/recognition/include/pcl/recognition/impl/implicit_shape_model.hpp @@ -43,6 +43,8 @@ #include "../implicit_shape_model.h" +#include // for dynamic_pointer_cast + ////////////////////////////////////////////////////////////////////////////////////////////// template pcl::features::ISMVoteList::ISMVoteList () : @@ -1243,11 +1245,11 @@ pcl::ism::ImplicitShapeModelEstimation::estimateFe feature_estimator_->setSearchMethod (tree); // typename pcl::SpinImageEstimation >::Ptr feat_est_norm = -// boost::dynamic_pointer_cast > > (feature_estimator_); +// dynamic_pointer_cast > > (feature_estimator_); // feat_est_norm->setInputNormals (normal_cloud); typename pcl::FeatureFromNormals >::Ptr feat_est_norm = - boost::dynamic_pointer_cast > > (feature_estimator_); + dynamic_pointer_cast > > (feature_estimator_); feat_est_norm->setInputNormals (normal_cloud); feature_estimator_->compute (*feature_cloud); diff --git a/recognition/include/pcl/recognition/impl/linemod/line_rgbd.hpp b/recognition/include/pcl/recognition/impl/linemod/line_rgbd.hpp index b89f3a30..77d08cb3 100644 --- a/recognition/include/pcl/recognition/impl/linemod/line_rgbd.hpp +++ b/recognition/include/pcl/recognition/impl/linemod/line_rgbd.hpp @@ -4,7 +4,7 @@ * Point Cloud Library (PCL) - www.pointclouds.org * Copyright (c) 2010-2012, 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 @@ -44,17 +44,20 @@ #include #include -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +namespace pcl +{ + template bool -pcl::LineRGBD::readLTMHeader (int fd, pcl::io::TARHeader &header) +LineRGBD::readLTMHeader (int fd, pcl::io::TARHeader &header) { // Read in the header - int result = static_cast (::read (fd, reinterpret_cast (&header.file_name[0]), 512)); + int result = static_cast (io::raw_read (fd, reinterpret_cast (&header.file_name[0]), 512)); if (result == -1) return (false); - // We only support regular files for now. - // Additional file types in TAR include: hard links, symbolic links, device/special files, block devices, + // We only support regular files for now. + // Additional file types in TAR include: hard links, symbolic links, device/special files, block devices, // directories, and named pipes. if (header.file_type[0] != '0' && header.file_type[0] != '\0') return (false); @@ -69,15 +72,15 @@ pcl::LineRGBD::readLTMHeader (int fd, pcl::io::TARHeader & return (true); } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template bool -pcl::LineRGBD::loadTemplates (const std::string &file_name, const std::size_t object_id) +LineRGBD::loadTemplates (const std::string &file_name, const std::size_t object_id) { // Open the file int ltm_fd = io::raw_open(file_name.c_str (), O_RDONLY); if (ltm_fd == -1) return (false); - + int ltm_offset = 0; pcl::io::TARHeader ltm_header; @@ -115,7 +118,7 @@ pcl::LineRGBD::loadTemplates (const std::string &file_name unsigned int fsize = ltm_header.getFileSize (); char *buffer = new char[fsize]; - int result = static_cast (::read (ltm_fd, reinterpret_cast (&buffer[0]), fsize)); + int result = static_cast (io::raw_read (ltm_fd, reinterpret_cast (&buffer[0]), fsize)); if (result == -1) { delete [] buffer; @@ -215,9 +218,9 @@ pcl::LineRGBD::loadTemplates (const std::string &file_name return (true); } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template int -pcl::LineRGBD::createAndAddTemplate ( +LineRGBD::createAndAddTemplate ( pcl::PointCloud & cloud, const std::size_t object_id, const MaskMap & mask_xyz, @@ -311,9 +314,8 @@ pcl::LineRGBD::createAndAddTemplate ( } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template bool -pcl::LineRGBD::addTemplate (const SparseQuantizedMultiModTemplate & sqmmt, pcl::PointCloud & cloud, std::size_t object_id) +LineRGBD::addTemplate (const SparseQuantizedMultiModTemplate & sqmmt, pcl::PointCloud & cloud, std::size_t object_id) { // add point cloud template_point_clouds_.resize (template_point_clouds_.size () + 1); @@ -394,9 +396,9 @@ pcl::LineRGBD::addTemplate (const SparseQuantizedMultiModT return (true); } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::LineRGBD::detect ( + +template void +LineRGBD::detect ( std::vector::Detection> & detections) { std::vector modalities; @@ -421,11 +423,11 @@ pcl::LineRGBD::detect ( detection.response = linemod_detection.score; // compute bounding box: - // we assume that the bounding boxes of the templates are relative to the center of mass + // we assume that the bounding boxes of the templates are relative to the center of mass // of the template points; so we also compute the center of mass of the points - // covered by the + // covered by the - const pcl::SparseQuantizedMultiModTemplate & linemod_template = + const pcl::SparseQuantizedMultiModTemplate & linemod_template = linemod_.getTemplate (linemod_detection.template_id); const std::size_t start_x = std::max (linemod_detection.x, 0); @@ -492,9 +494,9 @@ pcl::LineRGBD::detect ( } } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::LineRGBD::detectSemiScaleInvariant ( + +template void +LineRGBD::detectSemiScaleInvariant ( std::vector::Detection> & detections, const float min_scale, const float max_scale, @@ -522,11 +524,11 @@ pcl::LineRGBD::detectSemiScaleInvariant ( detection.response = linemod_detection.score; // compute bounding box: - // we assume that the bounding boxes of the templates are relative to the center of mass + // we assume that the bounding boxes of the templates are relative to the center of mass // of the template points; so we also compute the center of mass of the points - // covered by the + // covered by the - const pcl::SparseQuantizedMultiModTemplate & linemod_template = + const pcl::SparseQuantizedMultiModTemplate & linemod_template = linemod_.getTemplate (linemod_detection.template_id); const std::size_t start_x = std::max (linemod_detection.x, 0); @@ -593,9 +595,9 @@ pcl::LineRGBD::detectSemiScaleInvariant ( } } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::LineRGBD::computeTransformedTemplatePoints ( + +template void +LineRGBD::computeTransformedTemplatePoints ( const std::size_t detection_id, pcl::PointCloud &cloud) { if (detection_id >= detections_.size ()) @@ -607,11 +609,11 @@ pcl::LineRGBD::computeTransformedTemplatePoints ( const pcl::BoundingBoxXYZ & template_bounding_box = bounding_boxes_[template_id]; const pcl::BoundingBoxXYZ & detection_bounding_box = detections_[detection_id].bounding_box; - //std::cerr << "detection: " + //std::cerr << "detection: " // << detection_bounding_box.x << ", " // << detection_bounding_box.y << ", " // << detection_bounding_box.z << std::endl; - //std::cerr << "template: " + //std::cerr << "template: " // << template_bounding_box.x << ", " // << template_bounding_box.y << ", " // << template_bounding_box.z << std::endl; @@ -619,7 +621,7 @@ pcl::LineRGBD::computeTransformedTemplatePoints ( const float translation_y = detection_bounding_box.y - template_bounding_box.y; const float translation_z = detection_bounding_box.z - template_bounding_box.z; - //std::cerr << "translation: " + //std::cerr << "translation: " // << translation_x << ", " // << translation_y << ", " // << translation_z << std::endl; @@ -640,9 +642,9 @@ pcl::LineRGBD::computeTransformedTemplatePoints ( } } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::LineRGBD::refineDetectionsAlongDepth () + +template void +LineRGBD::refineDetectionsAlongDepth () { const std::size_t nr_detections = detections_.size (); for (std::size_t detection_index = 0; detection_index < nr_detections; ++detection_index) @@ -692,7 +694,7 @@ pcl::LineRGBD::refineDetectionsAlongDepth () } std::vector integral_depth_bins (nr_bins, 0); - + integral_depth_bins[0] = depth_bins[0]; for (std::size_t bin_index = 1; bin_index < nr_bins; ++bin_index) { @@ -720,9 +722,9 @@ pcl::LineRGBD::refineDetectionsAlongDepth () } } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::LineRGBD::applyProjectiveDepthICPOnDetections () + +template void +LineRGBD::applyProjectiveDepthICPOnDetections () { const std::size_t nr_detections = detections_.size (); for (std::size_t detection_index = 0; detection_index < nr_detections; ++detection_index) @@ -736,7 +738,7 @@ pcl::LineRGBD::applyProjectiveDepthICPOnDetections () const std::size_t start_y = detection.region.y; const std::size_t pc_width = point_cloud.width; const std::size_t pc_height = point_cloud.height; - + std::vector > depth_matches; for (std::size_t row_index = 0; row_index < pc_height; ++row_index) { @@ -801,9 +803,9 @@ pcl::LineRGBD::applyProjectiveDepthICPOnDetections () } } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::LineRGBD::removeOverlappingDetections () + +template void +LineRGBD::removeOverlappingDetections () { // compute overlap between each detection const std::size_t nr_detections = detections_.size (); @@ -820,7 +822,7 @@ pcl::LineRGBD::removeOverlappingDetections () overlaps (detection_index_1, detection_index_2) = 0.0f; else overlaps (detection_index_1, detection_index_2) = computeBoundingBoxIntersectionVolume ( - detections_[detection_index_1].bounding_box, + detections_[detection_index_1].bounding_box, detections_[detection_index_2].bounding_box) / bounding_box_volume; } } @@ -867,7 +869,7 @@ pcl::LineRGBD::removeOverlappingDetections () for (std::size_t cluster_id = 0; cluster_id < nr_clusters; ++cluster_id) { std::vector & cluster = clusters[cluster_id]; - + float average_center_x = 0.0f; float average_center_y = 0.0f; float average_center_z = 0.0f; @@ -935,9 +937,9 @@ pcl::LineRGBD::removeOverlappingDetections () detections_ = clustered_detections; } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -template float -pcl::LineRGBD::computeBoundingBoxIntersectionVolume ( + +template float +LineRGBD::computeBoundingBoxIntersectionVolume ( const BoundingBoxXYZ &box1, const BoundingBoxXYZ &box2) { const float x1_min = box1.x; @@ -953,7 +955,7 @@ pcl::LineRGBD::computeBoundingBoxIntersectionVolume ( const float x2_max = box2.x + box2.width; const float y2_max = box2.y + box2.height; const float z2_max = box2.z + box2.depth; - + const float xi_min = std::max (x1_min, x2_min); const float yi_min = std::max (y1_min, y2_min); const float zi_min = std::max (z1_min, z2_min); @@ -974,6 +976,7 @@ pcl::LineRGBD::computeBoundingBoxIntersectionVolume ( return (intersection_volume); } +} // namespace pcl -#endif // PCL_RECOGNITION_LINEMOD_LINE_RGBD_IMPL_HPP_ +#endif // PCL_RECOGNITION_LINEMOD_LINE_RGBD_IMPL_HPP_ 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 89f83867..08fef7f8 100644 --- a/recognition/include/pcl/recognition/impl/ransac_based/simple_octree.hpp +++ b/recognition/include/pcl/recognition/impl/ransac_based/simple_octree.hpp @@ -11,38 +11,40 @@ #include #include -//=============================================================================================================================== + +namespace pcl +{ + +namespace recognition +{ template inline -pcl::recognition::SimpleOctree::Node::Node () +SimpleOctree::Node::Node () : data_ (nullptr), parent_ (nullptr), children_(nullptr) {} -//=============================================================================================================================== template inline -pcl::recognition::SimpleOctree::Node::~Node () +SimpleOctree::Node::~Node () { this->deleteChildren (); this->deleteData (); } -//=============================================================================================================================== template inline void -pcl::recognition::SimpleOctree::Node::setCenter (const Scalar *c) +SimpleOctree::Node::setCenter (const Scalar *c) { center_[0] = c[0]; center_[1] = c[1]; center_[2] = c[2]; } -//=============================================================================================================================== template inline void -pcl::recognition::SimpleOctree::Node::setBounds (const Scalar *b) +SimpleOctree::Node::setBounds (const Scalar *b) { bounds_[0] = b[0]; bounds_[1] = b[1]; @@ -52,10 +54,9 @@ pcl::recognition::SimpleOctree::Node::setBoun bounds_[5] = b[5]; } -//=============================================================================================================================== template inline void -pcl::recognition::SimpleOctree::Node::computeRadius () +SimpleOctree::Node::computeRadius () { Scalar v[3] = {static_cast (0.5)*(bounds_[1]-bounds_[0]), static_cast (0.5)*(bounds_[3]-bounds_[2]), @@ -64,10 +65,9 @@ pcl::recognition::SimpleOctree::Node::compute radius_ = static_cast (std::sqrt (v[0]*v[0] + v[1]*v[1] + v[2]*v[2])); } -//=============================================================================================================================== template inline bool -pcl::recognition::SimpleOctree::Node::createChildren () +SimpleOctree::Node::createChildren () { if ( children_ ) return (false); @@ -152,34 +152,25 @@ pcl::recognition::SimpleOctree::Node::createC return (true); } -//=============================================================================================================================== template inline void -pcl::recognition::SimpleOctree::Node::deleteChildren () +SimpleOctree::Node::deleteChildren () { - if ( children_ ) - { - delete[] children_; - children_ = nullptr; - } + delete[] children_; + children_ = nullptr; } -//=============================================================================================================================== template inline void -pcl::recognition::SimpleOctree::Node::deleteData () +SimpleOctree::Node::deleteData () { - if ( data_ ) - { - delete data_; - data_ = nullptr; - } + delete data_; + data_ = nullptr; } -//=============================================================================================================================== template inline void -pcl::recognition::SimpleOctree::Node::makeNeighbors (Node* node) +SimpleOctree::Node::makeNeighbors (Node* node) { if ( !this->hasData () || !node->hasData () ) return; @@ -188,41 +179,34 @@ pcl::recognition::SimpleOctree::Node::makeNei node->full_leaf_neighbors_.insert (this); } -//=============================================================================================================================== template inline -pcl::recognition::SimpleOctree::SimpleOctree () +SimpleOctree::SimpleOctree () : tree_levels_ (0), root_ (nullptr) { } -//=============================================================================================================================== template inline -pcl::recognition::SimpleOctree::~SimpleOctree () +SimpleOctree::~SimpleOctree () { this->clear (); } -//=============================================================================================================================== template inline void -pcl::recognition::SimpleOctree::clear () +SimpleOctree::clear () { - if ( root_ ) - { - delete root_; - root_ = nullptr; - } + delete root_; + root_ = nullptr; full_leaves_.clear(); } -//=============================================================================================================================== template inline void -pcl::recognition::SimpleOctree::build (const Scalar* bounds, Scalar voxel_size, +SimpleOctree::build (const Scalar* bounds, Scalar voxel_size, NodeDataCreator* node_data_creator) { if ( voxel_size <= 0 ) @@ -265,11 +249,10 @@ pcl::recognition::SimpleOctree::build (const root_->computeRadius (); } -//=============================================================================================================================== template inline -typename pcl::recognition::SimpleOctree::Node* -pcl::recognition::SimpleOctree::createLeaf (Scalar x, Scalar y, Scalar z) +typename SimpleOctree::Node* +SimpleOctree::createLeaf (Scalar x, Scalar y, Scalar z) { // Make sure that the input point is within the octree bounds if ( x < bounds_[0] || x > bounds_[1] || @@ -305,11 +288,10 @@ pcl::recognition::SimpleOctree::createLeaf (S return (node); } -//=============================================================================================================================== template inline -typename pcl::recognition::SimpleOctree::Node* -pcl::recognition::SimpleOctree::getFullLeaf (int i, int j, int k) +typename SimpleOctree::Node* +SimpleOctree::getFullLeaf (int i, int j, int k) { Scalar offset = 0.5f*voxel_size_; Scalar p[3] = {bounds_[0] + offset + static_cast (i)*voxel_size_, @@ -319,11 +301,10 @@ pcl::recognition::SimpleOctree::getFullLeaf ( return (this->getFullLeaf (p[0], p[1], p[2])); } -//=============================================================================================================================== template inline -typename pcl::recognition::SimpleOctree::Node* -pcl::recognition::SimpleOctree::getFullLeaf (Scalar x, Scalar y, Scalar z) +typename SimpleOctree::Node* +SimpleOctree::getFullLeaf (Scalar x, Scalar y, Scalar z) { // Make sure that the input point is within the octree bounds if ( x < bounds_[0] || x > bounds_[1] || @@ -357,10 +338,9 @@ pcl::recognition::SimpleOctree::getFullLeaf ( return (node); } -//=============================================================================================================================== template inline void -pcl::recognition::SimpleOctree::insertNeighbors (Node* node) +SimpleOctree::insertNeighbors (Node* node) { const Scalar* c = node->getCenter (); Scalar s = static_cast (0.5)*voxel_size_; @@ -397,6 +377,8 @@ pcl::recognition::SimpleOctree::insertNeighbo neigh = this->getFullLeaf (c[0]-s, c[1]-s, c[2]-s); if ( neigh ) node->makeNeighbors (neigh); } -//=============================================================================================================================== +} // namespace recognition +} // namespace pcl #endif /* SIMPLE_OCTREE_HPP_ */ + diff --git a/recognition/include/pcl/recognition/impl/ransac_based/voxel_structure.hpp b/recognition/include/pcl/recognition/impl/ransac_based/voxel_structure.hpp index 93a00fce..35e3eabf 100644 --- a/recognition/include/pcl/recognition/impl/ransac_based/voxel_structure.hpp +++ b/recognition/include/pcl/recognition/impl/ransac_based/voxel_structure.hpp @@ -36,11 +36,19 @@ * */ + #ifndef PCL_RECOGNITION_VOXEL_STRUCTURE_HPP_ #define PCL_RECOGNITION_VOXEL_STRUCTURE_HPP_ + +namespace pcl +{ + +namespace recognition +{ + template inline void -pcl::recognition::VoxelStructure::build (const REAL bounds[6], int num_of_voxels[3]) +VoxelStructure::build (const REAL bounds[6], int num_of_voxels[3]) { this->clear(); @@ -72,10 +80,9 @@ pcl::recognition::VoxelStructure::build (const REAL bounds[6], int num_o min_center_[2] = bounds_[4] + static_cast (0.5)*spacing_[2]; } -//================================================================================================================================ template inline T* -pcl::recognition::VoxelStructure::getVoxel (const REAL p[3]) +VoxelStructure::getVoxel (const REAL p[3]) { if ( p[0] < bounds_[0] || p[0] >= bounds_[1] || p[1] < bounds_[2] || p[1] >= bounds_[3] || p[2] < bounds_[4] || p[2] >= bounds_[5] ) return nullptr; @@ -87,10 +94,9 @@ pcl::recognition::VoxelStructure::getVoxel (const REAL p[3]) return &voxels_[z*num_of_voxels_xy_plane_ + y*num_of_voxels_[0] + x]; } -//================================================================================================================================ template inline T* -pcl::recognition::VoxelStructure::getVoxel (int x, int y, int z) const +VoxelStructure::getVoxel (int x, int y, int z) const { if ( x < 0 || x >= num_of_voxels_[0] ) return nullptr; if ( y < 0 || y >= num_of_voxels_[1] ) return nullptr; @@ -99,10 +105,9 @@ pcl::recognition::VoxelStructure::getVoxel (int x, int y, int z) const return &voxels_[z*num_of_voxels_xy_plane_ + y*num_of_voxels_[0] + x]; } -//================================================================================================================================ template inline int -pcl::recognition::VoxelStructure::getNeighbors (const REAL* p, T **neighs) const +VoxelStructure::getNeighbors (const REAL* p, T **neighs) const { if ( p[0] < bounds_[0] || p[0] >= bounds_[1] || p[1] < bounds_[2] || p[1] >= bounds_[3] || p[2] < bounds_[4] || p[2] >= bounds_[5] ) return 0; @@ -151,6 +156,8 @@ pcl::recognition::VoxelStructure::getNeighbors (const REAL* p, T **neigh return num_neighs; } -//================================================================================================================================ +} // namespace recognition +} // namespace pcl #endif // PCL_RECOGNITION_VOXEL_STRUCTURE_HPP_ + diff --git a/recognition/include/pcl/recognition/implicit_shape_model.h b/recognition/include/pcl/recognition/implicit_shape_model.h index 55723ae5..44e08583 100644 --- a/recognition/include/pcl/recognition/implicit_shape_model.h +++ b/recognition/include/pcl/recognition/implicit_shape_model.h @@ -39,6 +39,7 @@ #include #include #include +#include #include #include #include @@ -153,7 +154,7 @@ namespace pcl /** \brief Stores square distances to the corresponding neighbours. */ std::vector k_sqr_dist_; }; - + /** \brief The assignment of this structure is to store the statistical/learned weights and other information * of the trained Implict Shape Model algorithm. */ @@ -553,7 +554,7 @@ namespace pcl int flags, Eigen::MatrixXf& cluster_centers); - /** \brief Generates centers for clusters as described in + /** \brief Generates centers for clusters as described in * Arthur, David and Sergei Vassilvitski (2007) k-means++: The Advantages of Careful Seeding. * \param[in] data points to cluster * \param[out] out_centers it will contain generated centers diff --git a/recognition/include/pcl/recognition/mask_map.h b/recognition/include/pcl/recognition/mask_map.h index 54074614..afef8eeb 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()); } - PCL_DEPRECATED("Use new version diff getDifferenceMask(mask0, mask1)") + PCL_DEPRECATED(1, 12, "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/model_library.h b/recognition/include/pcl/recognition/model_library.h deleted file mode 100644 index 4e3fcc31..00000000 --- a/recognition/include/pcl/recognition/model_library.h +++ /dev/null @@ -1,2 +0,0 @@ -#include -#error "Using pcl/recognition/model_library.h is deprecated, please use pcl/recognition/ransac_based/model_library.h instead." \ No newline at end of file diff --git a/recognition/include/pcl/recognition/obj_rec_ransac.h b/recognition/include/pcl/recognition/obj_rec_ransac.h deleted file mode 100644 index 1cacff83..00000000 --- a/recognition/include/pcl/recognition/obj_rec_ransac.h +++ /dev/null @@ -1,2 +0,0 @@ -#include -#error "Using pcl/recognition/obj_rec_ransac.h is deprecated, please use pcl/recognition/ransac_based/obj_rec_ransac.h instead." \ No newline at end of file diff --git a/recognition/include/pcl/recognition/orr_graph.h b/recognition/include/pcl/recognition/orr_graph.h deleted file mode 100644 index 436b3e8b..00000000 --- a/recognition/include/pcl/recognition/orr_graph.h +++ /dev/null @@ -1,2 +0,0 @@ -#include -#error "Using pcl/recognition/orr_graph.h is deprecated, please use pcl/recognition/ransac_based/orr_graph.h instead." \ No newline at end of file diff --git a/recognition/include/pcl/recognition/orr_octree.h b/recognition/include/pcl/recognition/orr_octree.h deleted file mode 100644 index 38b850c8..00000000 --- a/recognition/include/pcl/recognition/orr_octree.h +++ /dev/null @@ -1,2 +0,0 @@ -#include -#error "Using pcl/recognition/orr_octree.h is deprecated, please use pcl/recognition/ransac_based/orr_octree.h instead." \ No newline at end of file diff --git a/recognition/include/pcl/recognition/orr_octree_zprojection.h b/recognition/include/pcl/recognition/orr_octree_zprojection.h deleted file mode 100644 index edb05809..00000000 --- a/recognition/include/pcl/recognition/orr_octree_zprojection.h +++ /dev/null @@ -1,2 +0,0 @@ -#include -#error "Using pcl/recognition/orr_octree_zprojection.h is deprecated, please use pcl/recognition/ransac_based/orr_octree_zprojection.h instead." \ No newline at end of file diff --git a/recognition/include/pcl/recognition/point_types.h b/recognition/include/pcl/recognition/point_types.h index 7e37a17b..af71d6fc 100644 --- a/recognition/include/pcl/recognition/point_types.h +++ b/recognition/include/pcl/recognition/point_types.h @@ -37,6 +37,7 @@ #pragma once +#include #include #include #include @@ -63,7 +64,7 @@ namespace pcl }; PCL_MAKE_ALIGNED_OPERATOR_NEW - inline bool operator< (const GradientXY & rhs) + inline bool operator< (const GradientXY & rhs) const { return (magnitude > rhs.magnitude); } diff --git a/recognition/include/pcl/recognition/ransac_based/bvh.h b/recognition/include/pcl/recognition/ransac_based/bvh.h index fd9e054e..540e6b47 100644 --- a/recognition/include/pcl/recognition/ransac_based/bvh.h +++ b/recognition/include/pcl/recognition/ransac_based/bvh.h @@ -150,13 +150,10 @@ namespace pcl } virtual ~Node () - { - if ( children_[0] ) { delete children_[0]; delete children_[1]; } - } bool hasChildren () const @@ -249,13 +246,10 @@ namespace pcl /** \brief Frees the memory allocated by this object. After that, you have to call build to use the tree again. */ void clear() - { - if ( root_ ) { delete root_; root_ = nullptr; } - } inline const std::vector* getInputObjects () const diff --git a/recognition/include/pcl/recognition/ransac_based/orr_octree.h b/recognition/include/pcl/recognition/ransac_based/orr_octree.h index 06550641..d43aabd2 100644 --- a/recognition/include/pcl/recognition/ransac_based/orr_octree.h +++ b/recognition/include/pcl/recognition/ransac_based/orr_octree.h @@ -234,30 +234,24 @@ namespace pcl /** \brief Computes the "radius" of the node which is half the diagonal length. */ inline float - getRadius (){ return radius_;} + getRadius () const{ return radius_;} bool createChildren (); inline void deleteChildren () - { - if ( children_ ) { delete[] children_; children_ = nullptr; } - } inline void deleteData () - { - if ( data_ ) { delete data_; data_ = nullptr; } - } /** \brief Make this and 'node' neighbors by inserting each node in the others node neighbor set. Nothing happens * of either of the nodes has no data. */ diff --git a/recognition/include/pcl/recognition/ransac_based/voxel_structure.h b/recognition/include/pcl/recognition/ransac_based/voxel_structure.h index 026a59b8..78622a77 100644 --- a/recognition/include/pcl/recognition/ransac_based/voxel_structure.h +++ b/recognition/include/pcl/recognition/ransac_based/voxel_structure.h @@ -58,7 +58,7 @@ namespace pcl /** \brief Release the memory allocated for the voxels. */ inline void - clear (){ if ( voxels_ ){ delete[] voxels_; voxels_ = nullptr;}} + clear (){ delete[] voxels_; voxels_ = nullptr;} /** \brief Returns a pointer to the voxel which contains p or NULL if p is not inside the structure. */ inline T* diff --git a/recognition/include/pcl/recognition/rigid_transform_space.h b/recognition/include/pcl/recognition/rigid_transform_space.h deleted file mode 100644 index a62e3dfe..00000000 --- a/recognition/include/pcl/recognition/rigid_transform_space.h +++ /dev/null @@ -1,2 +0,0 @@ -#include -#error "Using pcl/recognition/rigid_transform_space.h is deprecated, please use pcl/recognition/ransac_based/rigid_transform_space.h instead." \ No newline at end of file diff --git a/recognition/include/pcl/recognition/simple_octree.h b/recognition/include/pcl/recognition/simple_octree.h deleted file mode 100644 index 19f2a2ba..00000000 --- a/recognition/include/pcl/recognition/simple_octree.h +++ /dev/null @@ -1,2 +0,0 @@ -#include -#error "Using pcl/recognition/simple_octree.h is deprecated, please use pcl/recognition/ransac_based/simple_octree.h instead." \ No newline at end of file diff --git a/recognition/include/pcl/recognition/sparse_quantized_multi_mod_template.h b/recognition/include/pcl/recognition/sparse_quantized_multi_mod_template.h index d8b298ee..9a38fff5 100644 --- a/recognition/include/pcl/recognition/sparse_quantized_multi_mod_template.h +++ b/recognition/include/pcl/recognition/sparse_quantized_multi_mod_template.h @@ -65,7 +65,7 @@ namespace pcl * \param[in] base the feature to compare to. */ bool - compareForEquality (const QuantizedMultiModFeature & base) + compareForEquality (const QuantizedMultiModFeature & base) const { if (base.x != x) return false; diff --git a/recognition/include/pcl/recognition/trimmed_icp.h b/recognition/include/pcl/recognition/trimmed_icp.h deleted file mode 100644 index 23fd0304..00000000 --- a/recognition/include/pcl/recognition/trimmed_icp.h +++ /dev/null @@ -1,2 +0,0 @@ -#include -#error "Using pcl/recognition/trimmed_icp.h is deprecated, please use pcl/recognition/ransac_based/trimmed_icp.h instead." \ No newline at end of file diff --git a/recognition/include/pcl/recognition/voxel_structure.h b/recognition/include/pcl/recognition/voxel_structure.h deleted file mode 100644 index 09ab880b..00000000 --- a/recognition/include/pcl/recognition/voxel_structure.h +++ /dev/null @@ -1,2 +0,0 @@ -#include -#error "Using pcl/recognition/voxel_structure.h is deprecated, please use pcl/recognition/ransac_based/voxel_structure.h instead." \ No newline at end of file diff --git a/recognition/src/face_detection/face_detector_data_provider.cpp b/recognition/src/face_detection/face_detector_data_provider.cpp index 23369dd0..e645de34 100644 --- a/recognition/src/face_detection/face_detector_data_provider.cpp +++ b/recognition/src/face_detection/face_detector_data_provider.cpp @@ -1,5 +1,6 @@ #include "pcl/recognition/face_detection/face_detector_data_provider.h" #include "pcl/recognition/face_detection/face_common.h" +#include // for pcl::isFinite #include #include #include diff --git a/recognition/src/face_detection/rf_face_detector_trainer.cpp b/recognition/src/face_detection/rf_face_detector_trainer.cpp index be8f8346..35b5fda9 100644 --- a/recognition/src/face_detection/rf_face_detector_trainer.cpp +++ b/recognition/src/face_detection/rf_face_detector_trainer.cpp @@ -8,6 +8,7 @@ #include "pcl/recognition/face_detection/rf_face_detector_trainer.h" #include "pcl/recognition/face_detection/face_common.h" #include "pcl/io/pcd_io.h" +#include // for dynamic_pointer_cast #include "pcl/ml/dt/decision_tree_trainer.h" #include "pcl/ml/dt/decision_tree_evaluator.h" #include "pcl/ml/dt/decision_forest_trainer.h" @@ -22,6 +23,7 @@ #include #include + void pcl::RFFaceDetectorTrainer::trainWithDataProvider() { @@ -58,7 +60,7 @@ void pcl::RFFaceDetectorTrainer::trainWithDataProvider() dtdp->initialize (directory_); - auto cast_dtdp = boost::dynamic_pointer_cast, float, int, NodeType>> (dtdp); + auto cast_dtdp = dynamic_pointer_cast, float, int, NodeType>> (dtdp); dft.setDecisionTreeDataProvider (cast_dtdp); pcl::DecisionForest forest; diff --git a/recognition/src/mask_map.cpp b/recognition/src/mask_map.cpp index 120bbed7..bf492833 100644 --- a/recognition/src/mask_map.cpp +++ b/recognition/src/mask_map.cpp @@ -37,6 +37,7 @@ #include +#include // for std::transform #include ////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/recognition/src/ransac_based/orr_octree.cpp b/recognition/src/ransac_based/orr_octree.cpp index b7b5a47e..e755a520 100644 --- a/recognition/src/ransac_based/orr_octree.cpp +++ b/recognition/src/ransac_based/orr_octree.cpp @@ -60,11 +60,8 @@ pcl::recognition::ORROctree::ORROctree () void pcl::recognition::ORROctree::clear () { - if ( root_ ) - { - delete root_; - root_ = nullptr; - } + delete root_; + root_ = nullptr; full_leaves_.clear(); } diff --git a/recognition/src/ransac_based/orr_octree_zprojection.cpp b/recognition/src/ransac_based/orr_octree_zprojection.cpp index a539f597..2056dbae 100644 --- a/recognition/src/ransac_based/orr_octree_zprojection.cpp +++ b/recognition/src/ransac_based/orr_octree_zprojection.cpp @@ -60,7 +60,6 @@ pcl::recognition::ORROctreeZProjection::clear () { // Delete pixel by pixel for ( int j = 0 ; j < num_pixels_y_ ; ++j ) - if ( pixels_[i][j] ) delete pixels_[i][j]; // Delete the whole row @@ -76,7 +75,6 @@ pcl::recognition::ORROctreeZProjection::clear () for ( int i = 0 ; i < num_pixels_x_ ; ++i ) { for ( int j = 0 ; j < num_pixels_y_ ; ++j ) - if ( sets_[i][j] ) delete sets_[i][j]; delete[] sets_[i]; diff --git a/registration/include/pcl/registration/bfgs.h b/registration/include/pcl/registration/bfgs.h index d6b30748..cf302061 100644 --- a/registration/include/pcl/registration/bfgs.h +++ b/registration/include/pcl/registration/bfgs.h @@ -69,6 +69,16 @@ namespace Eigen }; } +namespace BFGSSpace { + enum Status { + NegativeGradientEpsilon = -3, + NotStarted = -2, + Running = -1, + Success = 0, + NoProgress = 1 + }; +} + template struct BFGSDummyFunctor { @@ -87,18 +97,9 @@ struct BFGSDummyFunctor virtual double operator() (const VectorType &x) = 0; virtual void df(const VectorType &x, VectorType &df) = 0; virtual void fdf(const VectorType &x, Scalar &f, VectorType &df) = 0; + virtual BFGSSpace::Status checkGradient(const VectorType& g) { return BFGSSpace::NotStarted; }; }; -namespace BFGSSpace { - enum Status { - NegativeGradientEpsilon = -3, - NotStarted = -2, - Running = -1, - Success = 0, - NoProgress = 1 - }; -} - /** * BFGS stands for Broyden–Fletcher–Goldfarb–Shanno (BFGS) method for solving * unconstrained nonlinear optimization problems. @@ -148,7 +149,9 @@ public: BFGSSpace::Status minimize(FVectorType &x); BFGSSpace::Status minimizeInit(FVectorType &x); BFGSSpace::Status minimizeOneStep(FVectorType &x); - BFGSSpace::Status testGradient(Scalar epsilon); + BFGSSpace::Status testGradient(); + PCL_DEPRECATED(1, 13, "Use `testGradient()` instead") + BFGSSpace::Status testGradient(Scalar) { return testGradient(); } void resetParameters(void) { parameters = Parameters(); } Parameters parameters; @@ -407,18 +410,11 @@ BFGS::minimizeOneStep(FVectorType &x) return BFGSSpace::Success; } -template typename BFGSSpace::Status -BFGS::testGradient(Scalar epsilon) +template +typename BFGSSpace::Status +BFGS::testGradient() { - if(epsilon < 0) - return BFGSSpace::NegativeGradientEpsilon; - else - { - if(gradient.norm () < epsilon) - return BFGSSpace::Success; - else - return BFGSSpace::Running; - } + return functor.checkGradient(gradient); } template typename BFGS::Scalar diff --git a/registration/include/pcl/registration/boost.h b/registration/include/pcl/registration/boost.h index 962e5888..ae726dcc 100644 --- a/registration/include/pcl/registration/boost.h +++ b/registration/include/pcl/registration/boost.h @@ -49,4 +49,3 @@ #include #include -#include diff --git a/registration/include/pcl/registration/convergence_criteria.h b/registration/include/pcl/registration/convergence_criteria.h index efe05323..ab64c576 100644 --- a/registration/include/pcl/registration/convergence_criteria.h +++ b/registration/include/pcl/registration/convergence_criteria.h @@ -39,6 +39,7 @@ #pragma once +#include #include namespace pcl diff --git a/registration/include/pcl/registration/correspondence_estimation.h b/registration/include/pcl/registration/correspondence_estimation.h index 696804ca..f016f298 100644 --- a/registration/include/pcl/registration/correspondence_estimation.h +++ b/registration/include/pcl/registration/correspondence_estimation.h @@ -45,6 +45,7 @@ #include #include #include +#include #include #include diff --git a/registration/include/pcl/registration/correspondence_estimation_organized_projection.h b/registration/include/pcl/registration/correspondence_estimation_organized_projection.h index 220ca647..d8458183 100644 --- a/registration/include/pcl/registration/correspondence_estimation_organized_projection.h +++ b/registration/include/pcl/registration/correspondence_estimation_organized_projection.h @@ -39,6 +39,7 @@ #pragma once +#include #include #include diff --git a/registration/include/pcl/registration/correspondence_rejection.h b/registration/include/pcl/registration/correspondence_rejection.h index ca9371fb..88023d6a 100644 --- a/registration/include/pcl/registration/correspondence_rejection.h +++ b/registration/include/pcl/registration/correspondence_rejection.h @@ -69,7 +69,7 @@ namespace pcl virtual ~CorrespondenceRejector () {} /** \brief Provide a pointer to the vector of the input correspondences. - * \param[in] correspondences the const boost shared pointer to a correspondence vector + * \param[in] correspondences the const shared pointer to a correspondence vector */ virtual inline void setInputCorrespondences (const CorrespondencesConstPtr &correspondences) @@ -78,7 +78,7 @@ namespace pcl }; /** \brief Get a pointer to the vector of the input correspondences. - * \return correspondences the const boost shared pointer to a correspondence vector + * \return correspondences the const shared pointer to a correspondence vector */ inline CorrespondencesConstPtr getInputCorrespondences () { return input_correspondences_; }; diff --git a/registration/include/pcl/registration/correspondence_rejection_distance.h b/registration/include/pcl/registration/correspondence_rejection_distance.h index 26552446..f02721a7 100644 --- a/registration/include/pcl/registration/correspondence_rejection_distance.h +++ b/registration/include/pcl/registration/correspondence_rejection_distance.h @@ -41,6 +41,8 @@ #pragma once #include +#include // for static_pointer_cast + namespace pcl { @@ -95,19 +97,20 @@ namespace pcl /** \brief Get the maximum distance used for thresholding in correspondence rejection. */ inline float - getMaximumDistance () { return std::sqrt (max_distance_); }; + getMaximumDistance () const { return std::sqrt (max_distance_); }; /** \brief Provide a source point cloud dataset (must contain XYZ * data!), used to compute the correspondence distance. * \param[in] cloud a cloud containing XYZ data */ - template inline void + template + PCL_DEPRECATED(1, 12, "pcl::registration::CorrespondenceRejectorDistance::setInputCloud is deprecated. Please use setInputSource instead") + inline void setInputCloud (const typename pcl::PointCloud::ConstPtr &cloud) { - PCL_WARN ("[pcl::registration::%s::setInputCloud] setInputCloud is deprecated. Please use setInputSource instead.\n", getClassName ().c_str ()); if (!data_container_) data_container_.reset (new DataContainer); - boost::static_pointer_cast > (data_container_)->setInputSource (cloud); + static_pointer_cast > (data_container_)->setInputSource (cloud); } /** \brief Provide a source point cloud dataset (must contain XYZ @@ -119,7 +122,7 @@ namespace pcl { if (!data_container_) data_container_.reset (new DataContainer); - boost::static_pointer_cast > (data_container_)->setInputSource (cloud); + static_pointer_cast > (data_container_)->setInputSource (cloud); } /** \brief Provide a target point cloud dataset (must contain XYZ @@ -131,7 +134,7 @@ namespace pcl { if (!data_container_) data_container_.reset (new DataContainer); - boost::static_pointer_cast > (data_container_)->setInputTarget (target); + static_pointer_cast > (data_container_)->setInputTarget (target); } @@ -174,7 +177,7 @@ namespace pcl setSearchMethodTarget (const typename pcl::search::KdTree::Ptr &tree, bool force_no_recompute = false) { - boost::static_pointer_cast< DataContainer > + static_pointer_cast< DataContainer > (data_container_)->setSearchMethodTarget (tree, force_no_recompute ); } diff --git a/registration/include/pcl/registration/correspondence_rejection_median_distance.h b/registration/include/pcl/registration/correspondence_rejection_median_distance.h index 08104c71..8cece9d2 100644 --- a/registration/include/pcl/registration/correspondence_rejection_median_distance.h +++ b/registration/include/pcl/registration/correspondence_rejection_median_distance.h @@ -41,8 +41,10 @@ #pragma once #include +#include // for static_pointer_cast #include + namespace pcl { namespace registration @@ -97,20 +99,21 @@ namespace pcl { if (!data_container_) data_container_.reset (new DataContainer); - boost::static_pointer_cast > (data_container_)->setInputSource (cloud); + static_pointer_cast > (data_container_)->setInputSource (cloud); } /** \brief Provide a source point cloud dataset (must contain XYZ * data!), used to compute the correspondence distance. * \param[in] cloud a cloud containing XYZ data */ - template inline void + template + PCL_DEPRECATED(1, 12, "pcl::registration::CorrespondenceRejectorMedianDistance::setInputCloud is deprecated. Please use setInputSource instead") + inline void setInputCloud (const typename pcl::PointCloud::ConstPtr &cloud) { - PCL_WARN ("[pcl::registration::%s::setInputCloud] setInputCloud is deprecated. Please use setInputSource instead.\n", getClassName ().c_str ()); if (!data_container_) data_container_.reset (new DataContainer); - boost::static_pointer_cast > (data_container_)->setInputSource (cloud); + static_pointer_cast > (data_container_)->setInputSource (cloud); } /** \brief Provide a target point cloud dataset (must contain XYZ @@ -122,7 +125,7 @@ namespace pcl { if (!data_container_) data_container_.reset (new DataContainer); - boost::static_pointer_cast > (data_container_)->setInputTarget (target); + static_pointer_cast > (data_container_)->setInputTarget (target); } /** \brief See if this rejector requires source points */ @@ -164,7 +167,7 @@ namespace pcl setSearchMethodTarget (const typename pcl::search::KdTree::Ptr &tree, bool force_no_recompute = false) { - boost::static_pointer_cast< DataContainer > + static_pointer_cast< DataContainer > (data_container_)->setSearchMethodTarget (tree, force_no_recompute ); } diff --git a/registration/include/pcl/registration/correspondence_rejection_organized_boundary.h b/registration/include/pcl/registration/correspondence_rejection_organized_boundary.h index cfa20ec8..49e6d263 100644 --- a/registration/include/pcl/registration/correspondence_rejection_organized_boundary.h +++ b/registration/include/pcl/registration/correspondence_rejection_organized_boundary.h @@ -39,7 +39,7 @@ #pragma once #include - +#include // for static_pointer_cast namespace pcl { @@ -79,7 +79,7 @@ namespace pcl { if (!data_container_) data_container_.reset (new pcl::registration::DataContainer); - boost::static_pointer_cast > (data_container_)->setInputSource (cloud); + static_pointer_cast > (data_container_)->setInputSource (cloud); } template inline void @@ -87,7 +87,7 @@ namespace pcl { if (!data_container_) data_container_.reset (new pcl::registration::DataContainer); - boost::static_pointer_cast > (data_container_)->setInputTarget (cloud); + static_pointer_cast > (data_container_)->setInputTarget (cloud); } /** \brief See if this rejector requires source points */ diff --git a/registration/include/pcl/registration/correspondence_rejection_poly.h b/registration/include/pcl/registration/correspondence_rejection_poly.h index 593bd304..44fde05c 100644 --- a/registration/include/pcl/registration/correspondence_rejection_poly.h +++ b/registration/include/pcl/registration/correspondence_rejection_poly.h @@ -109,11 +109,10 @@ namespace pcl /** \brief Provide a source point cloud dataset (must contain XYZ data!), used to compute the correspondence distance. * \param[in] cloud a cloud containing XYZ data */ + PCL_DEPRECATED(1, 12, "pcl::registration::CorrespondenceRejectorPoly::setInputCloud is deprecated. Please use setInputSource instead") inline void setInputCloud (const PointCloudSourceConstPtr &cloud) { - PCL_WARN ("[pcl::registration::%s::setInputCloud] setInputCloud is deprecated. Please use setInputSource instead.\n", - getClassName ().c_str ()); input_ = cloud; } diff --git a/registration/include/pcl/registration/correspondence_rejection_sample_consensus.h b/registration/include/pcl/registration/correspondence_rejection_sample_consensus.h index 41dcf508..3a2fa585 100644 --- a/registration/include/pcl/registration/correspondence_rejection_sample_consensus.h +++ b/registration/include/pcl/registration/correspondence_rejection_sample_consensus.h @@ -41,6 +41,7 @@ #pragma once +#include #include #include diff --git a/registration/include/pcl/registration/correspondence_rejection_sample_consensus_2d.h b/registration/include/pcl/registration/correspondence_rejection_sample_consensus_2d.h index d415cd43..9698b5ae 100644 --- a/registration/include/pcl/registration/correspondence_rejection_sample_consensus_2d.h +++ b/registration/include/pcl/registration/correspondence_rejection_sample_consensus_2d.h @@ -38,6 +38,7 @@ #pragma once +#include #include #include diff --git a/registration/include/pcl/registration/correspondence_rejection_surface_normal.h b/registration/include/pcl/registration/correspondence_rejection_surface_normal.h index 0c9fba39..510b8a64 100644 --- a/registration/include/pcl/registration/correspondence_rejection_surface_normal.h +++ b/registration/include/pcl/registration/correspondence_rejection_surface_normal.h @@ -40,8 +40,10 @@ #pragma once #include +#include // for static_pointer_cast #include + namespace pcl { namespace registration @@ -102,16 +104,17 @@ namespace pcl /** \brief Provide a source point cloud dataset (must contain XYZ data!), used to compute the correspondence distance. * \param[in] input a cloud containing XYZ data */ - template inline void + template + PCL_DEPRECATED(1, 12, "pcl::registration::CorrespondenceRejectorSurfaceNormal::setInputCloud is deprecated. Please use setInputSource instead") + inline void setInputCloud (const typename pcl::PointCloud::ConstPtr &input) { - PCL_WARN ("[pcl::registration::%s::setInputCloud] setInputCloud is deprecated. Please use setInputSource instead.\n", getClassName ().c_str ()); if (!data_container_) { PCL_ERROR ("[pcl::registration::%s::setInputCloud] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ()); return; } - boost::static_pointer_cast > (data_container_)->setInputSource (input); + static_pointer_cast > (data_container_)->setInputSource (input); } /** \brief Provide a source point cloud dataset (must contain XYZ data!), used to compute the correspondence distance. @@ -125,7 +128,7 @@ namespace pcl PCL_ERROR ("[pcl::registration::%s::setInputCloud] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ()); return; } - boost::static_pointer_cast > (data_container_)->setInputSource (input); + static_pointer_cast > (data_container_)->setInputSource (input); } /** \brief Get the target input point cloud */ @@ -137,7 +140,7 @@ namespace pcl PCL_ERROR ("[pcl::registration::%s::getInputSource] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ()); return; } - return (boost::static_pointer_cast > (data_container_)->getInputSource ()); + return (static_pointer_cast > (data_container_)->getInputSource ()); } /** \brief Provide a target point cloud dataset (must contain XYZ data!), used to compute the correspondence distance. @@ -151,7 +154,7 @@ namespace pcl PCL_ERROR ("[pcl::registration::%s::setInputTarget] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ()); return; } - boost::static_pointer_cast > (data_container_)->setInputTarget (target); + static_pointer_cast > (data_container_)->setInputTarget (target); } /** \brief Provide a pointer to the search object used to find correspondences in @@ -165,7 +168,7 @@ namespace pcl setSearchMethodTarget (const typename pcl::search::KdTree::Ptr &tree, bool force_no_recompute = false) { - boost::static_pointer_cast< DataContainer > + static_pointer_cast< DataContainer > (data_container_)->setSearchMethodTarget (tree, force_no_recompute ); } @@ -178,7 +181,7 @@ namespace pcl PCL_ERROR ("[pcl::registration::%s::getInputTarget] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ()); return; } - return (boost::static_pointer_cast > (data_container_)->getInputTarget ()); + return (static_pointer_cast > (data_container_)->getInputTarget ()); } /** \brief Set the normals computed on the input point cloud @@ -192,7 +195,7 @@ namespace pcl PCL_ERROR ("[pcl::registration::%s::setInputNormals] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ()); return; } - boost::static_pointer_cast > (data_container_)->setInputNormals (normals); + static_pointer_cast > (data_container_)->setInputNormals (normals); } /** \brief Get the normals computed on the input point cloud */ @@ -204,7 +207,7 @@ namespace pcl PCL_ERROR ("[pcl::registration::%s::getInputNormals] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ()); return; } - return (boost::static_pointer_cast > (data_container_)->getInputNormals ()); + return (static_pointer_cast > (data_container_)->getInputNormals ()); } /** \brief Set the normals computed on the target point cloud @@ -218,7 +221,7 @@ namespace pcl PCL_ERROR ("[pcl::registration::%s::setTargetNormals] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ()); return; } - boost::static_pointer_cast > (data_container_)->setTargetNormals (normals); + static_pointer_cast > (data_container_)->setTargetNormals (normals); } /** \brief Get the normals computed on the target point cloud */ @@ -230,7 +233,7 @@ namespace pcl PCL_ERROR ("[pcl::registration::%s::getTargetNormals] Initialize the data container object by calling intializeDataContainer () before using this function.\n", getClassName ().c_str ()); return; } - return (boost::static_pointer_cast > (data_container_)->getTargetNormals ()); + return (static_pointer_cast > (data_container_)->getTargetNormals ()); } diff --git a/registration/include/pcl/registration/correspondence_rejection_trimmed.h b/registration/include/pcl/registration/correspondence_rejection_trimmed.h index a88e5227..e645161c 100644 --- a/registration/include/pcl/registration/correspondence_rejection_trimmed.h +++ b/registration/include/pcl/registration/correspondence_rejection_trimmed.h @@ -91,7 +91,7 @@ namespace pcl /** \brief Get the maximum distance used for thresholding in correspondence rejection. */ inline float - getOverlapRatio () { return overlap_ratio_; }; + getOverlapRatio () const { return overlap_ratio_; }; /** \brief Set a minimum number of correspondences. If the specified overlap ratio causes to have * less correspondences, \a CorrespondenceRejectorTrimmed will try to return at least @@ -104,7 +104,7 @@ namespace pcl /** \brief Get the minimum number of correspondences. */ inline unsigned int - getMinCorrespondences () { return nr_min_correspondences_; }; + getMinCorrespondences () const { return nr_min_correspondences_; }; /** \brief Get a list of valid correspondences after rejection from the original set of correspondences. diff --git a/registration/include/pcl/registration/correspondence_rejection_var_trimmed.h b/registration/include/pcl/registration/correspondence_rejection_var_trimmed.h index c3750c27..93753a48 100644 --- a/registration/include/pcl/registration/correspondence_rejection_var_trimmed.h +++ b/registration/include/pcl/registration/correspondence_rejection_var_trimmed.h @@ -40,6 +40,7 @@ #pragma once #include +#include // for static_pointer_cast #include #include @@ -103,20 +104,21 @@ namespace pcl { if (!data_container_) data_container_.reset (new DataContainer); - boost::static_pointer_cast > (data_container_)->setInputSource (cloud); + static_pointer_cast > (data_container_)->setInputSource (cloud); } /** \brief Provide a source point cloud dataset (must contain XYZ * data!), used to compute the correspondence distance. * \param[in] cloud a cloud containing XYZ data */ - template inline void + template + PCL_DEPRECATED(1, 12, "pcl::registration::CorrespondenceRejectorVarTrimmed::setInputCloud is deprecated. Please use setInputSource instead") + inline void setInputCloud (const typename pcl::PointCloud::ConstPtr &cloud) { - PCL_WARN ("[pcl::registration::%s::setInputCloud] setInputCloud is deprecated. Please use setInputSource instead.\n", getClassName ().c_str ()); if (!data_container_) data_container_.reset (new DataContainer); - boost::static_pointer_cast > (data_container_)->setInputSource (cloud); + static_pointer_cast > (data_container_)->setInputSource (cloud); } /** \brief Provide a target point cloud dataset (must contain XYZ @@ -128,7 +130,7 @@ namespace pcl { if (!data_container_) data_container_.reset (new DataContainer); - boost::static_pointer_cast > (data_container_)->setInputTarget (target); + static_pointer_cast > (data_container_)->setInputTarget (target); } @@ -172,7 +174,7 @@ namespace pcl setSearchMethodTarget (const typename pcl::search::KdTree::Ptr &tree, bool force_no_recompute = false) { - boost::static_pointer_cast< DataContainer > + static_pointer_cast< DataContainer > (data_container_)->setSearchMethodTarget (tree, force_no_recompute ); } @@ -244,7 +246,7 @@ namespace pcl /** \brief finds the optimal inlier ratio. This is based on the paper 'Outlier Robust ICP for minimizing Fractional RMSD, J. M. Philips et al' */ - inline float optimizeInlierRatio (std::vector &dists); + inline float optimizeInlierRatio (std::vector &dists) const; }; } } diff --git a/registration/include/pcl/registration/default_convergence_criteria.h b/registration/include/pcl/registration/default_convergence_criteria.h index 64c09325..aa02d272 100644 --- a/registration/include/pcl/registration/default_convergence_criteria.h +++ b/registration/include/pcl/registration/default_convergence_criteria.h @@ -39,6 +39,7 @@ #pragma once +#include #include #include #include diff --git a/registration/include/pcl/registration/edge_measurements.h b/registration/include/pcl/registration/edge_measurements.h index 73c47a3d..d3667bc7 100644 --- a/registration/include/pcl/registration/edge_measurements.h +++ b/registration/include/pcl/registration/edge_measurements.h @@ -40,6 +40,7 @@ #pragma once +#include #include namespace pcl diff --git a/registration/include/pcl/registration/elch.h b/registration/include/pcl/registration/elch.h index 67f61ecb..08d9ba2d 100644 --- a/registration/include/pcl/registration/elch.h +++ b/registration/include/pcl/registration/elch.h @@ -40,6 +40,7 @@ #pragma once +#include #include #include #include diff --git a/registration/include/pcl/registration/gicp.h b/registration/include/pcl/registration/gicp.h index 887b004f..e3b8ea6f 100644 --- a/registration/include/pcl/registration/gicp.h +++ b/registration/include/pcl/registration/gicp.h @@ -45,12 +45,12 @@ namespace pcl { - /** \brief GeneralizedIterativeClosestPoint is an ICP variant that implements the - * generalized iterative closest point algorithm as described by Alex Segal et al. in + /** \brief GeneralizedIterativeClosestPoint is an ICP variant that implements the + * generalized iterative closest point algorithm as described by Alex Segal et al. in * http://www.robots.ox.ac.uk/~avsegal/resources/papers/Generalized_ICP.pdf - * The approach is based on using anistropic cost functions to optimize the alignment + * The approach is based on using anistropic cost functions to optimize the alignment * after closest point assignments have been made. - * The original code uses GSL and ANN while in ours we use an eigen mapped BFGS and + * The original code uses GSL and ANN while in ours we use an eigen mapped BFGS and * FLANN. * \author Nizar Sallem * \ingroup registration @@ -92,7 +92,7 @@ namespace pcl using MatricesVector = std::vector< Eigen::Matrix3d, Eigen::aligned_allocator >; using MatricesVectorPtr = shared_ptr; using MatricesVectorConstPtr = shared_ptr; - + using InputKdTree = typename Registration::KdTree; using InputKdTreePtr = typename Registration::KdTreePtr; @@ -103,12 +103,14 @@ namespace pcl using Vector6d = Eigen::Matrix; /** \brief Empty constructor. */ - GeneralizedIterativeClosestPoint () + GeneralizedIterativeClosestPoint () : k_correspondences_(20) , gicp_epsilon_(0.001) , rotation_epsilon_(2e-3) , mahalanobis_(0) , max_inner_iterations_(20) + ,translation_gradient_tolerance_(1e-2) + ,rotation_gradient_tolerance_(1e-2) { min_number_correspondences_ = 4; reg_name_ = "GeneralizedIterativeClosestPoint"; @@ -141,43 +143,43 @@ namespace pcl // Set all the point.data[3] values to 1 to aid the rigid transformation for (std::size_t i = 0; i < input.size (); ++i) input[i].data[3] = 1.0; - + pcl::IterativeClosestPoint::setInputSource (cloud); input_covariances_.reset (); } - /** \brief Provide a pointer to the covariances of the input source (if computed externally!). + /** \brief Provide a pointer to the covariances of the input source (if computed externally!). * If not set, GeneralizedIterativeClosestPoint will compute the covariances itself. * Make sure to set the covariances AFTER setting the input source point cloud (setting the input source point cloud will reset the covariances). - * \param[in] target the input point cloud target + * \param[in] covariances the input source covariances */ - inline void + inline void setSourceCovariances (const MatricesVectorPtr& covariances) { input_covariances_ = covariances; } - + /** \brief Provide a pointer to the input target (e.g., the point cloud that we want to align the input source to) * \param[in] target the input point cloud target */ - inline void + inline void setInputTarget (const PointCloudTargetConstPtr &target) override { pcl::IterativeClosestPoint::setInputTarget(target); target_covariances_.reset (); } - /** \brief Provide a pointer to the covariances of the input target (if computed externally!). + /** \brief Provide a pointer to the covariances of the input target (if computed externally!). * If not set, GeneralizedIterativeClosestPoint will compute the covariances itself. * Make sure to set the covariances AFTER setting the input source point cloud (setting the input source point cloud will reset the covariances). - * \param[in] target the input point cloud target + * \param[in] covariances the input target covariances */ - inline void + inline void setTargetCovariances (const MatricesVectorPtr& covariances) { target_covariances_ = covariances; } - + /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using an iterative * non-linear Levenberg-Marquardt approach. * \param[in] cloud_src the source point cloud dataset @@ -192,7 +194,7 @@ namespace pcl const PointCloudTarget &cloud_tgt, const std::vector &indices_tgt, Eigen::Matrix4f &transformation_matrix); - + /** \brief \return Mahalanobis distance matrix for the given point index */ inline const Eigen::Matrix3d& mahalanobis(std::size_t index) const { @@ -210,59 +212,82 @@ namespace pcl void computeRDerivative(const Vector6d &x, const Eigen::Matrix3d &R, Vector6d &g) const; - /** \brief Set the rotation epsilon (maximum allowable difference between two - * consecutive rotations) in order for an optimization to be considered as having + /** \brief Set the rotation epsilon (maximum allowable difference between two + * consecutive rotations) in order for an optimization to be considered as having * converged to the final solution. * \param epsilon the rotation epsilon */ - inline void + inline void setRotationEpsilon (double epsilon) { rotation_epsilon_ = epsilon; } - /** \brief Get the rotation epsilon (maximum allowable difference between two + /** \brief Get the rotation epsilon (maximum allowable difference between two * consecutive rotations) as set by the user. */ - inline double - getRotationEpsilon () { return (rotation_epsilon_); } + inline double + getRotationEpsilon () const { return rotation_epsilon_; } /** \brief Set the number of neighbors used when selecting a point neighbourhood - * to compute covariances. - * A higher value will bring more accurate covariance matrix but will make + * to compute covariances. + * A higher value will bring more accurate covariance matrix but will make * covariances computation slower. * \param k the number of neighbors to use when computing covariances */ void setCorrespondenceRandomness (int k) { k_correspondences_ = k; } - /** \brief Get the number of neighbors used when computing covariances as set by - * the user + /** \brief Get the number of neighbors used when computing covariances as set by + * the user */ int - getCorrespondenceRandomness () { return (k_correspondences_); } + getCorrespondenceRandomness () const { return k_correspondences_; } - /** set maximum number of iterations at the optimization step + /** \brief Set maximum number of iterations at the optimization step * \param[in] max maximum number of iterations for the optimizer */ void setMaximumOptimizerIterations (int max) { max_inner_iterations_ = max; } - ///\return maximum number of iterations at the optimization step + /** \brief Return maximum number of iterations at the optimization step + */ int - getMaximumOptimizerIterations () { return (max_inner_iterations_); } + getMaximumOptimizerIterations () const { return max_inner_iterations_; } + + /** \brief Set the minimal translation gradient threshold for early optimization stop + * \param[in] translation gradient threshold in meters + */ + void + setTranslationGradientTolerance (double tolerance) { translation_gradient_tolerance_ = tolerance; } + + /** \brief Return the minimal translation gradient threshold for early optimization stop + */ + double + getTranslationGradientTolerance () const { return translation_gradient_tolerance_; } + + /** \brief Set the minimal rotation gradient threshold for early optimization stop + * \param[in] rotation gradient threshold in radians + */ + void + setRotationGradientTolerance (double tolerance) { rotation_gradient_tolerance_ = tolerance; } + + /** \brief Return the minimal rotation gradient threshold for early optimization stop + */ + double + getRotationGradientTolerance () const { return rotation_gradient_tolerance_; } protected: - /** \brief The number of neighbors used for covariances computation. + /** \brief The number of neighbors used for covariances computation. * default: 20 */ int k_correspondences_; - /** \brief The epsilon constant for gicp paper; this is NOT the convergence - * tolerance + /** \brief The epsilon constant for gicp paper; this is NOT the convergence + * tolerance * default: 0.001 */ double gicp_epsilon_; - /** The epsilon constant for rotation error. (In GICP the transformation epsilon + /** The epsilon constant for rotation error. (In GICP the transformation epsilon * is split in rotation part and translation part). * default: 2e-3 */ @@ -283,7 +308,6 @@ namespace pcl /** \brief Temporary pointer to the target dataset indices. */ const std::vector *tmp_idx_tgt_; - /** \brief Input cloud points covariances. */ MatricesVectorPtr input_covariances_; @@ -292,26 +316,32 @@ namespace pcl /** \brief Mahalanobis matrices holder. */ std::vector mahalanobis_; - + /** \brief maximum number of optimizations */ int max_inner_iterations_; - /** \brief compute points covariances matrices according to the K nearest + /** \brief minimal translation gradient for early optimization stop */ + double translation_gradient_tolerance_; + + /** \brief minimal rotation gradient for early optimization stop */ + double rotation_gradient_tolerance_; + + /** \brief compute points covariances matrices according to the K nearest * neighbors. K is set via setCorrespondenceRandomness() method. * \param cloud pointer to point cloud * \param tree KD tree performer for nearest neighbors search * \param[out] cloud_covariances covariances matrices for each point in the cloud */ template - void computeCovariances(typename pcl::PointCloud::ConstPtr cloud, + void computeCovariances(typename pcl::PointCloud::ConstPtr cloud, const typename pcl::search::KdTree::Ptr tree, MatricesVector& cloud_covariances); - /** \return trace of mat1^t . mat2 + /** \return trace of mat1^t . mat2 * \param mat1 matrix of dimension nxm * \param mat2 matrix of dimension nxp */ - inline double + inline double matricesInnerProd(const Eigen::MatrixXd& mat1, const Eigen::MatrixXd& mat2) const { double r = 0.; @@ -327,7 +357,7 @@ namespace pcl * \param output the transformed input point cloud dataset using the rigid transformation found * \param guess the initial guess of the transformation to compute */ - void + void computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess) override; /** \brief Search for the closest nearest neighbor of a given point. @@ -335,7 +365,7 @@ namespace pcl * \param index vector of size 1 to store the index of the nearest neighbour found * \param distance vector of size 1 to store the distance to nearest neighbour found */ - inline bool + inline bool searchForNeighbors (const PointSource &query, std::vector& index, std::vector& distance) { int k = tree_->nearestKSearch (query, 1, index, distance); @@ -346,7 +376,7 @@ namespace pcl /// \brief compute transformation matrix from transformation matrix void applyState(Eigen::Matrix4f &t, const Vector6d& x) const; - + /// \brief optimization functor structure struct OptimizationFunctorWithIndices : public BFGSDummyFunctor { @@ -355,10 +385,11 @@ namespace pcl double operator() (const Vector6d& x) override; void df(const Vector6d &x, Vector6d &df) override; void fdf(const Vector6d &x, double &f, Vector6d &df) override; + BFGSSpace::Status checkGradient(const Vector6d& g) override; const GeneralizedIterativeClosestPoint *gicp_; }; - + std::function &cloud_src, const std::vector &src_indices, const pcl::PointCloud &cloud_tgt, diff --git a/registration/include/pcl/registration/gicp6d.h b/registration/include/pcl/registration/gicp6d.h index 590d8a64..c69f72f7 100644 --- a/registration/include/pcl/registration/gicp6d.h +++ b/registration/include/pcl/registration/gicp6d.h @@ -38,6 +38,7 @@ #pragma once +#include #include #include #include diff --git a/registration/include/pcl/registration/ia_ransac.h b/registration/include/pcl/registration/ia_ransac.h index b920fdef..b9d64402 100644 --- a/registration/include/pcl/registration/ia_ransac.h +++ b/registration/include/pcl/registration/ia_ransac.h @@ -40,6 +40,7 @@ #pragma once +#include #include #include #include diff --git a/registration/include/pcl/registration/icp.h b/registration/include/pcl/registration/icp.h index 7114cc31..e3671c14 100644 --- a/registration/include/pcl/registration/icp.h +++ b/registration/include/pcl/registration/icp.h @@ -41,7 +41,7 @@ #pragma once // PCL includes -#include +#include // for dynamic_pointer_cast, pcl::make_shared, shared_ptr #include #include #include @@ -51,6 +51,7 @@ #include #include + namespace pcl { /** \brief @b IterativeClosestPoint provides a base implementation of the Iterative Closest Point algorithm. @@ -368,7 +369,7 @@ namespace pcl setEnforceSameDirectionNormals (bool enforce_same_direction_normals) { enforce_same_direction_normals_ = enforce_same_direction_normals; - auto symmetric_transformation_estimation = boost::dynamic_pointer_cast >(transformation_estimation_); + auto symmetric_transformation_estimation = dynamic_pointer_cast >(transformation_estimation_); if (symmetric_transformation_estimation) symmetric_transformation_estimation->setEnforceSameDirectionNormals (enforce_same_direction_normals_); } diff --git a/registration/include/pcl/registration/impl/correspondence_estimation.hpp b/registration/include/pcl/registration/impl/correspondence_estimation.hpp index 3eae7d1f..0b695a65 100644 --- a/registration/include/pcl/registration/impl/correspondence_estimation.hpp +++ b/registration/include/pcl/registration/impl/correspondence_estimation.hpp @@ -37,15 +37,22 @@ * $Id$ * */ + #ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_H_ #define PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_H_ #include #include -/////////////////////////////////////////////////////////////////////////////////////////// + +namespace pcl +{ + +namespace registration +{ + template void -pcl::registration::CorrespondenceEstimationBase::setInputTarget ( +CorrespondenceEstimationBase::setInputTarget ( const PointCloudTargetConstPtr &cloud) { if (cloud->points.empty ()) @@ -62,9 +69,9 @@ pcl::registration::CorrespondenceEstimationBase bool -pcl::registration::CorrespondenceEstimationBase::initCompute () +CorrespondenceEstimationBase::initCompute () { if (!target_) { @@ -87,9 +94,9 @@ pcl::registration::CorrespondenceEstimationBase::initCompute ()); } -/////////////////////////////////////////////////////////////////////////////////////////// + template bool -pcl::registration::CorrespondenceEstimationBase::initComputeReciprocal () +CorrespondenceEstimationBase::initComputeReciprocal () { // Only update source kd-tree if a new target cloud was set if (source_cloud_updated_ && !force_no_recompute_reciprocal_) @@ -108,9 +115,9 @@ pcl::registration::CorrespondenceEstimationBase void -pcl::registration::CorrespondenceEstimation::determineCorrespondences ( +CorrespondenceEstimation::determineCorrespondences ( pcl::Correspondences &correspondences, double max_distance) { if (!initCompute ()) @@ -124,7 +131,7 @@ pcl::registration::CorrespondenceEstimation::d std::vector distance (1); pcl::Correspondence corr; unsigned int nr_valid_correspondences = 0; - + // Check if the template types are the same. If true, avoid a copy. // Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT macro! if (isSamePointType ()) @@ -145,7 +152,7 @@ pcl::registration::CorrespondenceEstimation::d else { PointTarget pt; - + // Iterate over the input set of source indices for (std::vector::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx) { @@ -166,9 +173,9 @@ pcl::registration::CorrespondenceEstimation::d deinitCompute (); } -/////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::registration::CorrespondenceEstimation::determineReciprocalCorrespondences ( +CorrespondenceEstimation::determineReciprocalCorrespondences ( pcl::Correspondences &correspondences, double max_distance) { if (!initCompute ()) @@ -216,7 +223,7 @@ pcl::registration::CorrespondenceEstimation::d { PointTarget pt_src; PointSource pt_tgt; - + // Iterate over the input set of source indices for (std::vector::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx) { @@ -246,6 +253,10 @@ pcl::registration::CorrespondenceEstimation::d deinitCompute (); } +} // namespace registration +} // namespace pcl + //#define PCL_INSTANTIATE_CorrespondenceEstimation(T,U) template class PCL_EXPORTS pcl::registration::CorrespondenceEstimation; #endif /* PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_H_ */ + diff --git a/registration/include/pcl/registration/impl/correspondence_estimation_backprojection.hpp b/registration/include/pcl/registration/impl/correspondence_estimation_backprojection.hpp index fa80ca36..1fa9ba1c 100644 --- a/registration/include/pcl/registration/impl/correspondence_estimation_backprojection.hpp +++ b/registration/include/pcl/registration/impl/correspondence_estimation_backprojection.hpp @@ -36,14 +36,21 @@ * $Id$ * */ + #ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_BACK_PROJECTION_HPP_ #define PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_BACK_PROJECTION_HPP_ #include -/////////////////////////////////////////////////////////////////////////////////////////// + +namespace pcl +{ + +namespace registration +{ + template bool -pcl::registration::CorrespondenceEstimationBackProjection::initCompute () +CorrespondenceEstimationBackProjection::initCompute () { if (!source_normals_ || !target_normals_) { @@ -56,7 +63,7 @@ pcl::registration::CorrespondenceEstimationBackProjection void -pcl::registration::CorrespondenceEstimationBackProjection::determineCorrespondences ( +CorrespondenceEstimationBackProjection::determineCorrespondences ( pcl::Correspondences &correspondences, double max_distance) { if (!initCompute ()) @@ -67,9 +74,8 @@ pcl::registration::CorrespondenceEstimationBackProjection nn_indices (k_); std::vector nn_dists (k_); - float min_dist = std::numeric_limits::max (); int min_index = 0; - + pcl::Correspondence corr; unsigned int nr_valid_correspondences = 0; @@ -84,8 +90,8 @@ pcl::registration::CorrespondenceEstimationBackProjectionnearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists); // Among the K nearest neighbours find the one with minimum perpendicular distance to the normal - min_dist = std::numeric_limits::max (); - + float min_dist = std::numeric_limits::max (); + // Find the best correspondence for (std::size_t j = 0; j < nn_indices.size (); j++) { @@ -93,7 +99,7 @@ pcl::registration::CorrespondenceEstimationBackProjectionpoints[*idx_i].normal_y * target_normals_->points[nn_indices[j]].normal_y + source_normals_->points[*idx_i].normal_z * target_normals_->points[nn_indices[j]].normal_z ; float dist = nn_dists[j] * (2.0f - cos_angle * cos_angle); - + if (dist < min_dist) { min_dist = dist; @@ -112,15 +118,15 @@ pcl::registration::CorrespondenceEstimationBackProjection::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i) { tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists); - + // Among the K nearest neighbours find the one with minimum perpendicular distance to the normal - min_dist = std::numeric_limits::max (); - + float min_dist = std::numeric_limits::max (); + // Find the best correspondence for (std::size_t j = 0; j < nn_indices.size (); j++) { @@ -132,7 +138,7 @@ pcl::registration::CorrespondenceEstimationBackProjectionpoints[*idx_i].normal_y * target_normals_->points[nn_indices[j]].normal_y + source_normals_->points[*idx_i].normal_z * target_normals_->points[nn_indices[j]].normal_z ; float dist = nn_dists[j] * (2.0f - cos_angle * cos_angle); - + if (dist < min_dist) { min_dist = dist; @@ -141,7 +147,7 @@ pcl::registration::CorrespondenceEstimationBackProjection max_distance) continue; - + corr.index_query = *idx_i; corr.index_match = nn_indices[min_index]; corr.distance = nn_dists[min_index];//min_dist; @@ -152,9 +158,9 @@ pcl::registration::CorrespondenceEstimationBackProjection void -pcl::registration::CorrespondenceEstimationBackProjection::determineReciprocalCorrespondences ( +CorrespondenceEstimationBackProjection::determineReciprocalCorrespondences ( pcl::Correspondences &correspondences, double max_distance) { if (!initCompute ()) @@ -171,9 +177,8 @@ pcl::registration::CorrespondenceEstimationBackProjection index_reciprocal (1); std::vector distance_reciprocal (1); - float min_dist = std::numeric_limits::max (); int min_index = 0; - + pcl::Correspondence corr; unsigned int nr_valid_correspondences = 0; int target_idx = 0; @@ -189,8 +194,8 @@ pcl::registration::CorrespondenceEstimationBackProjectionnearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists); // Among the K nearest neighbours find the one with minimum perpendicular distance to the normal - min_dist = std::numeric_limits::max (); - + float min_dist = std::numeric_limits::max (); + // Find the best correspondence for (std::size_t j = 0; j < nn_indices.size (); j++) { @@ -198,7 +203,7 @@ pcl::registration::CorrespondenceEstimationBackProjectionpoints[*idx_i].normal_y * target_normals_->points[nn_indices[j]].normal_y + source_normals_->points[*idx_i].normal_z * target_normals_->points[nn_indices[j]].normal_z ; float dist = nn_dists[j] * (2.0f - cos_angle * cos_angle); - + if (dist < min_dist) { min_dist = dist; @@ -224,15 +229,15 @@ pcl::registration::CorrespondenceEstimationBackProjection::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i) { tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists); - + // Among the K nearest neighbours find the one with minimum perpendicular distance to the normal - min_dist = std::numeric_limits::max (); - + float min_dist = std::numeric_limits::max (); + // Find the best correspondence for (std::size_t j = 0; j < nn_indices.size (); j++) { @@ -244,7 +249,7 @@ pcl::registration::CorrespondenceEstimationBackProjectionpoints[*idx_i].normal_y * target_normals_->points[nn_indices[j]].normal_y + source_normals_->points[*idx_i].normal_z * target_normals_->points[nn_indices[j]].normal_z ; float dist = nn_dists[j] * (2.0f - cos_angle * cos_angle); - + if (dist < min_dist) { min_dist = dist; @@ -253,7 +258,7 @@ pcl::registration::CorrespondenceEstimationBackProjection max_distance) continue; - + // Check if the correspondence is reciprocal target_idx = nn_indices[min_index]; tree_reciprocal_->nearestKSearch (target_->points[target_idx], 1, index_reciprocal, distance_reciprocal); @@ -271,4 +276,7 @@ pcl::registration::CorrespondenceEstimationBackProjection -/////////////////////////////////////////////////////////////////////////////////////////// + +namespace pcl +{ + +namespace registration +{ + template bool -pcl::registration::CorrespondenceEstimationNormalShooting::initCompute () +CorrespondenceEstimationNormalShooting::initCompute () { if (!source_normals_) { @@ -55,9 +62,9 @@ pcl::registration::CorrespondenceEstimationNormalShooting::initCompute ()); } -/////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::registration::CorrespondenceEstimationNormalShooting::determineCorrespondences ( +CorrespondenceEstimationNormalShooting::determineCorrespondences ( pcl::Correspondences &correspondences, double max_distance) { if (!initCompute ()) @@ -68,9 +75,8 @@ pcl::registration::CorrespondenceEstimationNormalShooting nn_indices (k_); std::vector nn_dists (k_); - double min_dist = std::numeric_limits::max (); int min_index = 0; - + pcl::Correspondence corr; unsigned int nr_valid_correspondences = 0; @@ -85,12 +91,12 @@ pcl::registration::CorrespondenceEstimationNormalShootingnearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists); // Among the K nearest neighbours find the one with minimum perpendicular distance to the normal - min_dist = std::numeric_limits::max (); - + double min_dist = std::numeric_limits::max (); + // Find the best correspondence for (std::size_t j = 0; j < nn_indices.size (); j++) { - // computing the distance between a point and a line in 3d. + // computing the distance between a point and a line in 3d. // Reference - http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html pt.x = target_->points[nn_indices[j]].x - input_->points[*idx_i].x; pt.y = target_->points[nn_indices[j]].y - input_->points[*idx_i].y; @@ -100,7 +106,7 @@ pcl::registration::CorrespondenceEstimationNormalShooting::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i) { tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists); - + // Among the K nearest neighbours find the one with minimum perpendicular distance to the normal - min_dist = std::numeric_limits::max (); - + double min_dist = std::numeric_limits::max (); + // Find the best correspondence for (std::size_t j = 0; j < nn_indices.size (); j++) { @@ -142,12 +148,12 @@ pcl::registration::CorrespondenceEstimationNormalShootingpoints[nn_indices[j]].x - pt_src.x; pt.y = target_->points[nn_indices[j]].y - pt_src.y; pt.z = target_->points[nn_indices[j]].z - pt_src.z; - + const NormalT &normal = source_normals_->points[*idx_i]; Eigen::Vector3d N (normal.normal_x, normal.normal_y, normal.normal_z); Eigen::Vector3d V (pt.x, pt.y, pt.z); Eigen::Vector3d C = N.cross (V); - + // Check if we have a better correspondence double dist = C.dot (C); if (dist < min_dist) @@ -158,7 +164,7 @@ pcl::registration::CorrespondenceEstimationNormalShooting max_distance) continue; - + corr.index_query = *idx_i; corr.index_match = nn_indices[min_index]; corr.distance = nn_dists[min_index];//min_dist; @@ -169,9 +175,9 @@ pcl::registration::CorrespondenceEstimationNormalShooting void -pcl::registration::CorrespondenceEstimationNormalShooting::determineReciprocalCorrespondences ( +CorrespondenceEstimationNormalShooting::determineReciprocalCorrespondences ( pcl::Correspondences &correspondences, double max_distance) { if (!initCompute ()) @@ -189,9 +195,8 @@ pcl::registration::CorrespondenceEstimationNormalShooting index_reciprocal (1); std::vector distance_reciprocal (1); - double min_dist = std::numeric_limits::max (); int min_index = 0; - + pcl::Correspondence corr; unsigned int nr_valid_correspondences = 0; int target_idx = 0; @@ -207,12 +212,12 @@ pcl::registration::CorrespondenceEstimationNormalShootingnearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists); // Among the K nearest neighbours find the one with minimum perpendicular distance to the normal - min_dist = std::numeric_limits::max (); - + double min_dist = std::numeric_limits::max (); + // Find the best correspondence for (std::size_t j = 0; j < nn_indices.size (); j++) { - // computing the distance between a point and a line in 3d. + // computing the distance between a point and a line in 3d. // Reference - http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html pt.x = target_->points[nn_indices[j]].x - input_->points[*idx_i].x; pt.y = target_->points[nn_indices[j]].y - input_->points[*idx_i].y; @@ -222,7 +227,7 @@ pcl::registration::CorrespondenceEstimationNormalShooting::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i) { tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists); // Among the K nearest neighbours find the one with minimum perpendicular distance to the normal - min_dist = std::numeric_limits::max (); - + double min_dist = std::numeric_limits::max (); + // Find the best correspondence for (std::size_t j = 0; j < nn_indices.size (); j++) { @@ -267,17 +272,17 @@ pcl::registration::CorrespondenceEstimationNormalShootingpoints[*idx_i], pt_src); - // computing the distance between a point and a line in 3d. + // computing the distance between a point and a line in 3d. // Reference - http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html pt.x = target_->points[nn_indices[j]].x - pt_src.x; pt.y = target_->points[nn_indices[j]].y - pt_src.y; pt.z = target_->points[nn_indices[j]].z - pt_src.z; - + const NormalT &normal = source_normals_->points[*idx_i]; Eigen::Vector3d N (normal.normal_x, normal.normal_y, normal.normal_z); Eigen::Vector3d V (pt.x, pt.y, pt.z); Eigen::Vector3d C = N.cross (V); - + // Check if we have a better correspondence double dist = C.dot (C); if (dist < min_dist) @@ -307,4 +312,7 @@ pcl::registration::CorrespondenceEstimationNormalShooting bool -pcl::registration::CorrespondenceEstimationOrganizedProjection::initCompute () +CorrespondenceEstimationOrganizedProjection::initCompute () { // Set the target_cloud_updated_ variable to true, so that the kd-tree is not built - it is not needed for this class target_cloud_updated_ = false; @@ -66,9 +73,9 @@ pcl::registration::CorrespondenceEstimationOrganizedProjection void -pcl::registration::CorrespondenceEstimationOrganizedProjection::determineCorrespondences ( +CorrespondenceEstimationOrganizedProjection::determineCorrespondences ( pcl::Correspondences &correspondences, double max_distance) { @@ -113,9 +120,9 @@ pcl::registration::CorrespondenceEstimationOrganizedProjection void -pcl::registration::CorrespondenceEstimationOrganizedProjection::determineReciprocalCorrespondences ( +CorrespondenceEstimationOrganizedProjection::determineReciprocalCorrespondences ( pcl::Correspondences &correspondences, double max_distance) { @@ -123,5 +130,8 @@ pcl::registration::CorrespondenceEstimationOrganizedProjection inline void -pcl::registration::CorrespondenceRejectorFeatures::setSourceFeature ( + +namespace pcl +{ + +namespace registration +{ + +template inline void +CorrespondenceRejectorFeatures::setSourceFeature ( const typename pcl::PointCloud::ConstPtr &source_feature, const std::string &key) { if (features_map_.count (key) == 0) @@ -50,18 +58,18 @@ pcl::registration::CorrespondenceRejectorFeatures::setSourceFeature ( boost::static_pointer_cast > (features_map_[key])->setSourceFeature (source_feature); } -////////////////////////////////////////////////////////////////////////////////////////////// -template inline typename pcl::PointCloud::ConstPtr -pcl::registration::CorrespondenceRejectorFeatures::getSourceFeature (const std::string &key) + +template inline typename pcl::PointCloud::ConstPtr +CorrespondenceRejectorFeatures::getSourceFeature (const std::string &key) { if (features_map_.count (key) == 0) return (nullptr); return (boost::static_pointer_cast > (features_map_[key])->getSourceFeature ()); } -////////////////////////////////////////////////////////////////////////////////////////////// -template inline void -pcl::registration::CorrespondenceRejectorFeatures::setTargetFeature ( + +template inline void +CorrespondenceRejectorFeatures::setTargetFeature ( const typename pcl::PointCloud::ConstPtr &target_feature, const std::string &key) { if (features_map_.count (key) == 0) @@ -69,18 +77,18 @@ pcl::registration::CorrespondenceRejectorFeatures::setTargetFeature ( boost::static_pointer_cast > (features_map_[key])->setTargetFeature (target_feature); } -////////////////////////////////////////////////////////////////////////////////////////////// -template inline typename pcl::PointCloud::ConstPtr -pcl::registration::CorrespondenceRejectorFeatures::getTargetFeature (const std::string &key) + +template inline typename pcl::PointCloud::ConstPtr +CorrespondenceRejectorFeatures::getTargetFeature (const std::string &key) { if (features_map_.count (key) == 0) return (nullptr); return (boost::static_pointer_cast > (features_map_[key])->getTargetFeature ()); } -////////////////////////////////////////////////////////////////////////////////////////////// -template inline void -pcl::registration::CorrespondenceRejectorFeatures::setDistanceThreshold ( + +template inline void +CorrespondenceRejectorFeatures::setDistanceThreshold ( double thresh, const std::string &key) { if (features_map_.count (key) == 0) @@ -89,10 +97,8 @@ pcl::registration::CorrespondenceRejectorFeatures::setDistanceThreshold ( } - -////////////////////////////////////////////////////////////////////////////////////////////// -template inline void -pcl::registration::CorrespondenceRejectorFeatures::setFeatureRepresentation ( +template inline void +CorrespondenceRejectorFeatures::setFeatureRepresentation ( const typename pcl::PointRepresentation::ConstPtr &fr, const std::string &key) { @@ -101,5 +107,8 @@ pcl::registration::CorrespondenceRejectorFeatures::setFeatureRepresentation ( boost::static_pointer_cast > (features_map_[key])->setFeatureRepresentation (fr); } +} // namespace registration +} // namespace pcl #endif /* PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_FEATURES_HPP_ */ + diff --git a/registration/include/pcl/registration/impl/correspondence_rejection_poly.hpp b/registration/include/pcl/registration/impl/correspondence_rejection_poly.hpp index dac077e0..906d8f05 100644 --- a/registration/include/pcl/registration/impl/correspondence_rejection_poly.hpp +++ b/registration/include/pcl/registration/impl/correspondence_rejection_poly.hpp @@ -35,18 +35,26 @@ * POSSIBILITY OF SUCH DAMAGE. * */ + + #ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_POLY_HPP_ #define PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_POLY_HPP_ -/////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::registration::CorrespondenceRejectorPoly::getRemainingCorrespondences ( - const pcl::Correspondences& original_correspondences, + +namespace pcl +{ + +namespace registration +{ + +template void +CorrespondenceRejectorPoly::getRemainingCorrespondences ( + const pcl::Correspondences& original_correspondences, pcl::Correspondences& remaining_correspondences) { // This is reset after all the checks below remaining_correspondences = original_correspondences; - + // Check source/target if (!input_) { @@ -61,7 +69,7 @@ pcl::registration::CorrespondenceRejectorPoly::getRemainingCor getClassName ().c_str ()); return; } - + // Check cardinality if (cardinality_ < 2) { @@ -69,7 +77,7 @@ pcl::registration::CorrespondenceRejectorPoly::getRemainingCor getClassName ().c_str() ); return; } - + // Number of input correspondences const int nr_correspondences = static_cast (original_correspondences.size ()); @@ -80,7 +88,7 @@ pcl::registration::CorrespondenceRejectorPoly::getRemainingCor getClassName ().c_str() ); return; } - + // Check similarity if (similarity_threshold_ < 0.0f || similarity_threshold_ > 1.0f) { @@ -88,24 +96,24 @@ pcl::registration::CorrespondenceRejectorPoly::getRemainingCor getClassName ().c_str() ); return; } - + // Similarity, squared similarity_threshold_squared_ = similarity_threshold_ * similarity_threshold_; // Initialization of result remaining_correspondences.clear (); remaining_correspondences.reserve (nr_correspondences); - + // Number of times a correspondence is sampled and number of times it was accepted std::vector num_samples (nr_correspondences, 0); std::vector num_accepted (nr_correspondences, 0); - + // Main loop for (int i = 0; i < iterations_; ++i) { // Sample cardinality_ correspondences without replacement const std::vector idx = getUniqueRandomIndices (nr_correspondences, cardinality_); - + // Verify the polygon similarity if (thresholdPolygon (original_correspondences, idx)) { @@ -123,7 +131,7 @@ pcl::registration::CorrespondenceRejectorPoly::getRemainingCor ++num_samples[ idx[j] ]; } } - + // Now calculate the acceptance rate of each correspondence std::vector accept_rate (nr_correspondences, 0.0f); for (int i = 0; i < nr_correspondences; ++i) @@ -134,50 +142,50 @@ pcl::registration::CorrespondenceRejectorPoly::getRemainingCor else accept_rate[i] = static_cast (num_accepted[i]) / static_cast (numsi); } - + // Compute a histogram in range [0,1] for acceptance rates const int hist_size = nr_correspondences / 2; // TODO: Optimize this const std::vector histogram = computeHistogram (accept_rate, 0.0f, 1.0f, hist_size); - + // Find the cut point between outliers and inliers using Otsu's thresholding method const int cut_idx = findThresholdOtsu (histogram); const float cut = static_cast (cut_idx) / static_cast (hist_size); - + // Threshold for (int i = 0; i < nr_correspondences; ++i) if (accept_rate[i] > cut) remaining_correspondences.push_back (original_correspondences[i]); } -////////////////////////////////////////////////////////////////////////////////////////////// -template std::vector -pcl::registration::CorrespondenceRejectorPoly::computeHistogram (const std::vector& data, - float lower, float upper, int bins) + +template std::vector +CorrespondenceRejectorPoly::computeHistogram (const std::vector& data, + float lower, float upper, int bins) { // Result std::vector result (bins, 0); - + // Last index into result and increment factor from data value --> index const int last_idx = bins - 1; const float idx_per_val = static_cast (bins) / (upper - lower); - + // Accumulate for (const float &value : data) ++result[ std::min (last_idx, int (value*idx_per_val)) ]; - + return (result); } -////////////////////////////////////////////////////////////////////////////////////////////// -template int -pcl::registration::CorrespondenceRejectorPoly::findThresholdOtsu (const std::vector& histogram) + +template int +CorrespondenceRejectorPoly::findThresholdOtsu (const std::vector& histogram) { // Precision const double eps = std::numeric_limits::epsilon(); - + // Histogram dimension const int nbins = static_cast (histogram.size ()); - + // Mean and inverse of the number of data points double mean = 0.0; double sum_inv = 0.0; @@ -188,45 +196,45 @@ pcl::registration::CorrespondenceRejectorPoly::findThresholdOt } sum_inv = 1.0/sum_inv; mean *= sum_inv; - + // Probability and mean of class 1 (data to the left of threshold) double class_mean1 = 0.0; double class_prob1 = 0.0; double class_prob2 = 1.0; - + // Maximized between class variance and associated bin value double between_class_variance_max = 0.0; int result = 0; - + // Loop over all bin values for (int i = 0; i < nbins; ++i) { class_mean1 *= class_prob1; - + // Probability of bin i const double prob_i = static_cast (histogram[i]) * sum_inv; - + // Class probability 1: sum of probabilities from 0 to i class_prob1 += prob_i; - + // Class probability 2: sum of probabilities from i+1 to nbins-1 class_prob2 -= prob_i; - + // Avoid division by zero below if (std::min (class_prob1,class_prob2) < eps || std::max (class_prob1,class_prob2) > 1.0-eps) continue; - + // Class mean 1: sum of probabilities from 0 to i, weighted by bin value class_mean1 = (class_mean1 + static_cast (i) * prob_i) / class_prob1; - + // Class mean 2: sum of probabilities from i+1 to nbins-1, weighted by bin value const double class_mean2 = (mean - class_prob1*class_mean1) / class_prob2; - + // Between class variance const double between_class_variance = class_prob1 * class_prob2 * (class_mean1 - class_mean2) * (class_mean1 - class_mean2); - + // If between class variance is maximized, update result if (between_class_variance > between_class_variance_max) { @@ -234,8 +242,12 @@ pcl::registration::CorrespondenceRejectorPoly::findThresholdOt result = i; } } - + return (result); } +} // namespace registration +} // namespace pcl + #endif // PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_POLY_HPP_ + 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 d8a4073b..7aa36b13 100644 --- a/registration/include/pcl/registration/impl/correspondence_rejection_sample_consensus.hpp +++ b/registration/include/pcl/registration/impl/correspondence_rejection_sample_consensus.hpp @@ -37,15 +37,22 @@ * $Id$ * */ + #ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_SAMPLE_CONSENSUS_HPP_ #define PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_SAMPLE_CONSENSUS_HPP_ #include -/////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::registration::CorrespondenceRejectorSampleConsensus::getRemainingCorrespondences ( - const pcl::Correspondences& original_correspondences, + +namespace pcl +{ + +namespace registration +{ + +template void +CorrespondenceRejectorSampleConsensus::getRemainingCorrespondences ( + const pcl::Correspondences& original_correspondences, pcl::Correspondences& remaining_correspondences) { if (!input_) @@ -133,4 +140,8 @@ pcl::registration::CorrespondenceRejectorSampleConsensus::getRemainingCo } } +} // namespace registration +} // namespace pcl + #endif // PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_SAMPLE_CONSENSUS_HPP_ + 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 12357dfe..173c524a 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 @@ -35,6 +35,7 @@ * * */ + #ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_SAMPLE_CONSENSUS_2D_HPP_ #define PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_SAMPLE_CONSENSUS_2D_HPP_ @@ -43,10 +44,16 @@ #include -/////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::registration::CorrespondenceRejectorSampleConsensus2D::getRemainingCorrespondences ( - const pcl::Correspondences& original_correspondences, + +namespace pcl +{ + +namespace registration +{ + +template void +CorrespondenceRejectorSampleConsensus2D::getRemainingCorrespondences ( + const pcl::Correspondences& original_correspondences, pcl::Correspondences& remaining_correspondences) { if (!input_) @@ -127,5 +134,8 @@ pcl::registration::CorrespondenceRejectorSampleConsensus2D::getRemaining best_transformation_.row (3) = model_coefficients.segment<4>(12); } +} // namespace registration +} // namespace pcl + #endif // PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_SAMPLE_CONSENSUS_2D_HPP_ diff --git a/registration/include/pcl/registration/impl/correspondence_types.hpp b/registration/include/pcl/registration/impl/correspondence_types.hpp index bc89e93c..2a2eb88d 100644 --- a/registration/include/pcl/registration/impl/correspondence_types.hpp +++ b/registration/include/pcl/registration/impl/correspondence_types.hpp @@ -45,8 +45,15 @@ #include #include + +namespace pcl +{ + +namespace registration +{ + inline void -pcl::registration::getCorDistMeanStd (const pcl::Correspondences &correspondences, double &mean, double &stddev) +getCorDistMeanStd (const pcl::Correspondences &correspondences, double &mean, double &stddev) { if (correspondences.empty ()) return; @@ -64,7 +71,7 @@ pcl::registration::getCorDistMeanStd (const pcl::Correspondences &correspondence } inline void -pcl::registration::getQueryIndices (const pcl::Correspondences& correspondences, std::vector& indices) +getQueryIndices (const pcl::Correspondences& correspondences, std::vector& indices) { indices.resize (correspondences.size ()); for (std::size_t i = 0; i < correspondences.size (); ++i) @@ -73,9 +80,13 @@ pcl::registration::getQueryIndices (const pcl::Correspondences& correspondences, inline void -pcl::registration::getMatchIndices (const pcl::Correspondences& correspondences, std::vector& indices) +getMatchIndices (const pcl::Correspondences& correspondences, std::vector& indices) { indices.resize (correspondences.size ()); for (std::size_t i = 0; i < correspondences.size (); ++i) indices[i] = correspondences[i].index_match; } + +} // namespace registration +} // namespace pcl + diff --git a/registration/include/pcl/registration/impl/default_convergence_criteria.hpp b/registration/include/pcl/registration/impl/default_convergence_criteria.hpp index 1dac2786..b6743997 100644 --- a/registration/include/pcl/registration/impl/default_convergence_criteria.hpp +++ b/registration/include/pcl/registration/impl/default_convergence_criteria.hpp @@ -42,9 +42,15 @@ #include -////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +namespace pcl +{ + +namespace registration +{ + template bool -pcl::registration::DefaultConvergenceCriteria::hasConverged () +DefaultConvergenceCriteria::hasConverged () { if (convergence_state_ != CONVERGENCE_CRITERIA_NOT_CONVERGED) { @@ -52,7 +58,7 @@ pcl::registration::DefaultConvergenceCriteria::hasConverged () iterations_similar_transforms_ = 0; convergence_state_ = CONVERGENCE_CRITERIA_NOT_CONVERGED; } - + bool is_similar = false; PCL_DEBUG ("[pcl::DefaultConvergenceCriteria::hasConverged] Iteration %d out of %d.\n", iterations_, max_iterations_); @@ -98,7 +104,7 @@ pcl::registration::DefaultConvergenceCriteria::hasConverged () } is_similar = true; } - + // Relative if (std::abs (correspondences_cur_mse_ - correspondences_prev_mse_) / correspondences_prev_mse_ < mse_threshold_relative_) { @@ -126,4 +132,8 @@ pcl::registration::DefaultConvergenceCriteria::hasConverged () return (false); } +} // namespace registration +} // namespace pcl + #endif // PCL_REGISTRATION_DEFAULT_CONVERGENCE_CRITERIA_HPP_ + diff --git a/registration/include/pcl/registration/impl/gicp.hpp b/registration/include/pcl/registration/impl/gicp.hpp index 234b28ef..651ab34f 100644 --- a/registration/include/pcl/registration/impl/gicp.hpp +++ b/registration/include/pcl/registration/impl/gicp.hpp @@ -37,18 +37,22 @@ * $Id$ * */ + #ifndef PCL_REGISTRATION_IMPL_GICP_HPP_ #define PCL_REGISTRATION_IMPL_GICP_HPP_ #include #include -//////////////////////////////////////////////////////////////////////////////////////// + +namespace pcl +{ + template template void -pcl::GeneralizedIterativeClosestPoint::computeCovariances(typename pcl::PointCloud::ConstPtr cloud, - const typename pcl::search::KdTree::Ptr kdtree, - MatricesVector& cloud_covariances) +GeneralizedIterativeClosestPoint::computeCovariances(typename pcl::PointCloud::ConstPtr cloud, + const typename pcl::search::KdTree::Ptr kdtree, + MatricesVector& cloud_covariances) { if (k_correspondences_ > int (cloud->size ())) { @@ -121,9 +125,9 @@ pcl::GeneralizedIterativeClosestPoint::computeCovarian } } -//////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::GeneralizedIterativeClosestPoint::computeRDerivative(const Vector6d &x, const Eigen::Matrix3d &R, Vector6d& g) const +GeneralizedIterativeClosestPoint::computeRDerivative(const Vector6d &x, const Eigen::Matrix3d &R, Vector6d& g) const { Eigen::Matrix3d dR_dPhi; Eigen::Matrix3d dR_dTheta; @@ -176,13 +180,13 @@ pcl::GeneralizedIterativeClosestPoint::computeRDerivat g[5] = matricesInnerProd(dR_dPsi, R); } -//////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::GeneralizedIterativeClosestPoint::estimateRigidTransformationBFGS (const PointCloudSource &cloud_src, - const std::vector &indices_src, - const PointCloudTarget &cloud_tgt, - const std::vector &indices_tgt, - Eigen::Matrix4f &transformation_matrix) +GeneralizedIterativeClosestPoint::estimateRigidTransformationBFGS (const PointCloudSource &cloud_src, + const std::vector &indices_src, + const PointCloudTarget &cloud_tgt, + const std::vector &indices_tgt, + Eigen::Matrix4f &transformation_matrix) { if (indices_src.size () < 4) // need at least 4 samples { @@ -192,9 +196,12 @@ pcl::GeneralizedIterativeClosestPoint::estimateRigidTr } // Set the initial solution Vector6d x = Vector6d::Zero (); + // translation part x[0] = transformation_matrix (0,3); x[1] = transformation_matrix (1,3); x[2] = transformation_matrix (2,3); + // rotation part (Z Y X euler angles convention) + // see: https://en.wikipedia.org/wiki/Rotation_matrix#General_rotations x[3] = std::atan2 (transformation_matrix (2,1), transformation_matrix (2,2)); x[4] = asin (-transformation_matrix (2,0)); x[5] = std::atan2 (transformation_matrix (1,0), transformation_matrix (0,0)); @@ -206,7 +213,6 @@ pcl::GeneralizedIterativeClosestPoint::estimateRigidTr tmp_idx_tgt_ = &indices_tgt; // Optimize using forward-difference approximation LM - const double gradient_tol = 1e-2; OptimizationFunctorWithIndices functor(this); BFGS bfgs (functor); bfgs.parameters.sigma = 0.01; @@ -227,7 +233,7 @@ pcl::GeneralizedIterativeClosestPoint::estimateRigidTr { break; } - result = bfgs.testGradient(gradient_tol); + result = bfgs.testGradient(); } while(result == BFGSSpace::Running && inner_iterations_ < max_inner_iterations_); if(result == BFGSSpace::NoProgress || result == BFGSSpace::Success || inner_iterations_ == max_inner_iterations_) { @@ -241,9 +247,9 @@ pcl::GeneralizedIterativeClosestPoint::estimateRigidTr "[pcl::" << getClassName () << "::TransformationEstimationBFGS::estimateRigidTransformation] BFGS solver didn't converge!"); } -//////////////////////////////////////////////////////////////////////////////////////// + template inline double -pcl::GeneralizedIterativeClosestPoint::OptimizationFunctorWithIndices::operator() (const Vector6d& x) +GeneralizedIterativeClosestPoint::OptimizationFunctorWithIndices::operator() (const Vector6d& x) { Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_; gicp_->applyState(transformation_matrix, x); @@ -266,9 +272,9 @@ pcl::GeneralizedIterativeClosestPoint::OptimizationFun return f/m; } -//////////////////////////////////////////////////////////////////////////////////////// + template inline void -pcl::GeneralizedIterativeClosestPoint::OptimizationFunctorWithIndices::df (const Vector6d& x, Vector6d& g) +GeneralizedIterativeClosestPoint::OptimizationFunctorWithIndices::df (const Vector6d& x, Vector6d& g) { Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_; gicp_->applyState(transformation_matrix, x); @@ -302,9 +308,9 @@ pcl::GeneralizedIterativeClosestPoint::OptimizationFun gicp_->computeRDerivative(x, R, g); } -//////////////////////////////////////////////////////////////////////////////////////// + template inline void -pcl::GeneralizedIterativeClosestPoint::OptimizationFunctorWithIndices::fdf (const Vector6d& x, double& f, Vector6d& g) +GeneralizedIterativeClosestPoint::OptimizationFunctorWithIndices::fdf (const Vector6d& x, double& f, Vector6d& g) { Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_; gicp_->applyState(transformation_matrix, x); @@ -339,9 +345,29 @@ pcl::GeneralizedIterativeClosestPoint::OptimizationFun gicp_->computeRDerivative(x, R, g); } -//////////////////////////////////////////////////////////////////////////////////////// +template inline BFGSSpace::Status +GeneralizedIterativeClosestPoint::OptimizationFunctorWithIndices::checkGradient(const Vector6d& g) +{ + auto translation_epsilon = gicp_->translation_gradient_tolerance_; + auto rotation_epsilon = gicp_->rotation_gradient_tolerance_; + + if ((translation_epsilon < 0.) || (rotation_epsilon < 0.)) + return BFGSSpace::NegativeGradientEpsilon; + + // express translation gradient as norm of translation parameters + auto translation_grad = g.head<3>().norm(); + + // express rotation gradient as a norm of rotation parameters + auto rotation_grad = g.tail<3>().norm(); + + if ((translation_grad < translation_epsilon) && (rotation_grad < rotation_epsilon)) + return BFGSSpace::Success; + + return BFGSSpace::Running; +} + template inline void -pcl::GeneralizedIterativeClosestPoint::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess) +GeneralizedIterativeClosestPoint::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess) { pcl::IterativeClosestPoint::initComputeReciprocal (); using namespace std; @@ -354,7 +380,7 @@ pcl::GeneralizedIterativeClosestPoint::computeTransfor // Compute target cloud covariance matrices if ((!target_covariances_) || (target_covariances_->empty ())) { - target_covariances_.reset (new MatricesVector); + target_covariances_.reset (new MatricesVector); computeCovariances (target_, tree_, *target_covariances_); } // Compute input cloud covariance matrices @@ -463,10 +489,11 @@ pcl::GeneralizedIterativeClosestPoint::computeTransfor pcl::transformPointCloud (*input_, output, final_transformation_); } + template void -pcl::GeneralizedIterativeClosestPoint::applyState(Eigen::Matrix4f &t, const Vector6d& x) const +GeneralizedIterativeClosestPoint::applyState(Eigen::Matrix4f &t, const Vector6d& x) const { - // !!! CAUTION Stanford GICP uses the Z Y X euler angles convention + // Z Y X euler angles convention Eigen::Matrix3f R; R = Eigen::AngleAxisf (static_cast (x[5]), Eigen::Vector3f::UnitZ ()) * Eigen::AngleAxisf (static_cast (x[4]), Eigen::Vector3f::UnitY ()) @@ -476,4 +503,7 @@ pcl::GeneralizedIterativeClosestPoint::applyState(Eige t.col (3) += T; } +} // namespace pcl + #endif //PCL_REGISTRATION_IMPL_GICP_HPP_ + diff --git a/registration/include/pcl/registration/impl/ia_fpcs.hpp b/registration/include/pcl/registration/impl/ia_fpcs.hpp index d758c14d..10101695 100644 --- a/registration/include/pcl/registration/impl/ia_fpcs.hpp +++ b/registration/include/pcl/registration/impl/ia_fpcs.hpp @@ -41,6 +41,7 @@ #include #include #include +#include #include #include @@ -59,13 +60,14 @@ pcl::getMeanPointDensity (const typename pcl::PointCloud::ConstPtr &clou std::vector ids (2); std::vector dists_sqr (2); -#ifdef _OPENMP + pcl::utils::ignore(nr_threads); #pragma omp parallel for \ - reduction (+:mean_dist, num) \ - private (ids, dists_sqr) shared (tree, cloud) firstprivate (s, max_dist_sqr) \ - default (none)num_threads (nr_threads) -#endif - + default(none) \ + shared(tree, cloud) \ + private(ids, dists_sqr) \ + reduction(+:mean_dist, num) \ + firstprivate(s, max_dist_sqr) \ + num_threads(nr_threads) for (int i = 0; i < 1000; i++) { tree.nearestKSearch (cloud->points[rand () % s], 2, ids, dists_sqr); @@ -96,13 +98,14 @@ pcl::getMeanPointDensity (const typename pcl::PointCloud::ConstPtr &clou std::vector ids (2); std::vector dists_sqr (2); -#ifdef _OPENMP + pcl::utils::ignore(nr_threads); #pragma omp parallel for \ - reduction (+:mean_dist, num) \ - private (ids, dists_sqr) shared (tree, cloud, indices) firstprivate (s, max_dist_sqr) \ - default (none)num_threads (nr_threads) -#endif - + default(none) \ + shared(tree, cloud, indices) \ + private(ids, dists_sqr) \ + reduction(+:mean_dist, num) \ + firstprivate(s, max_dist_sqr) \ + num_threads(nr_threads) for (int i = 0; i < 1000; i++) { tree.nearestKSearch (cloud->points[indices[rand () % s]], 2, ids, dists_sqr); @@ -162,20 +165,18 @@ pcl::registration::FPCSInitialAlignment all_candidates (max_iterations_); pcl::StopWatch timer; - #ifdef _OPENMP - #pragma omp parallel num_threads (nr_threads_) - #endif + #pragma omp parallel \ + default(none) \ + shared(abort, all_candidates, timer) \ + num_threads(nr_threads_) { #ifdef _OPENMP std::srand (static_cast (std::time (NULL)) ^ omp_get_thread_num ()); - #pragma omp for schedule (dynamic) + #pragma omp for schedule(dynamic) #endif for (int i = 0; i < max_iterations_; i++) { - - #ifdef _OPENMP #pragma omp flush (abort) - #endif MatchingCandidates candidates (1); std::vector base_indices (4); @@ -209,9 +210,7 @@ pcl::registration::FPCSInitialAlignment max_runtime_); - #ifdef _OPENMP #pragma omp flush (abort) - #endif } } } diff --git a/registration/include/pcl/registration/impl/ia_kfpcs.hpp b/registration/include/pcl/registration/impl/ia_kfpcs.hpp index 7f08fd54..42c86e25 100644 --- a/registration/include/pcl/registration/impl/ia_kfpcs.hpp +++ b/registration/include/pcl/registration/impl/ia_kfpcs.hpp @@ -37,11 +37,17 @@ #ifndef PCL_REGISTRATION_IMPL_IA_KFPCS_H_ #define PCL_REGISTRATION_IMPL_IA_KFPCS_H_ -/////////////////////////////////////////////////////////////////////////////////////////// + +namespace pcl +{ + +namespace registration +{ + template -pcl::registration::KFPCSInitialAlignment ::KFPCSInitialAlignment () : +KFPCSInitialAlignment ::KFPCSInitialAlignment () : lower_trl_boundary_ (-1.f), - upper_trl_boundary_ (-1.f), + upper_trl_boundary_ (-1.f), lambda_ (0.5f), use_trl_score_ (false), indices_validation_ (new std::vector ) @@ -50,9 +56,8 @@ pcl::registration::KFPCSInitialAlignment bool -pcl::registration::KFPCSInitialAlignment ::initCompute () +KFPCSInitialAlignment ::initCompute () { // due to sparse keypoint cloud, do not normalize delta with estimated point density if (normalize_delta_) @@ -92,22 +97,20 @@ pcl::registration::KFPCSInitialAlignment void -pcl::registration::KFPCSInitialAlignment ::handleMatches ( +KFPCSInitialAlignment ::handleMatches ( const std::vector &base_indices, std::vector > &matches, MatchingCandidates &candidates) { candidates.clear (); - float fitness_score = FLT_MAX; // loop over all Candidate matches for (auto &match : matches) { Eigen::Matrix4f transformation_temp; pcl::Correspondences correspondences_temp; - fitness_score = FLT_MAX; // reset to FLT_MAX to accept all candidates and not only best + float fitness_score = FLT_MAX; // reset to FLT_MAX to accept all candidates and not only best // determine corresondences between base and match according to their distance to centroid linkMatchWithBase (base_indices, match, correspondences_temp); @@ -126,9 +129,8 @@ pcl::registration::KFPCSInitialAlignment int -pcl::registration::KFPCSInitialAlignment ::validateTransformation ( +KFPCSInitialAlignment ::validateTransformation ( Eigen::Matrix4f &transformation, float &fitness_score) { @@ -138,7 +140,7 @@ pcl::registration::KFPCSInitialAlignment ids; std::vector dists_sqr; @@ -173,9 +175,8 @@ pcl::registration::KFPCSInitialAlignment void -pcl::registration::KFPCSInitialAlignment ::finalCompute ( +KFPCSInitialAlignment ::finalCompute ( const std::vector &candidates) { // reorganize candidates into single vector @@ -210,9 +211,9 @@ pcl::registration::KFPCSInitialAlignment void -pcl::registration::KFPCSInitialAlignment ::getNBestCandidates ( +KFPCSInitialAlignment ::getNBestCandidates ( int n, float min_angle3d, float min_translation3d, @@ -249,9 +250,9 @@ pcl::registration::KFPCSInitialAlignment void -pcl::registration::KFPCSInitialAlignment ::getTBestCandidates ( +KFPCSInitialAlignment ::getTBestCandidates ( float t, float min_angle3d, float min_translation3d, @@ -284,6 +285,8 @@ pcl::registration::KFPCSInitialAlignment -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::SampleConsensusInitialAlignment::setSourceFeatures (const FeatureCloudConstPtr &features) + +namespace pcl +{ + +template void +SampleConsensusInitialAlignment::setSourceFeatures (const FeatureCloudConstPtr &features) { if (features == nullptr || features->empty ()) { @@ -55,9 +58,9 @@ pcl::SampleConsensusInitialAlignment::setSou input_features_ = features; } -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::SampleConsensusInitialAlignment::setTargetFeatures (const FeatureCloudConstPtr &features) + +template void +SampleConsensusInitialAlignment::setTargetFeatures (const FeatureCloudConstPtr &features) { if (features == nullptr || features->empty ()) { @@ -68,10 +71,10 @@ pcl::SampleConsensusInitialAlignment::setTar feature_tree_->setInputCloud (target_features_); } -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::SampleConsensusInitialAlignment::selectSamples ( - const PointCloudSource &cloud, int nr_samples, float min_sample_distance, + +template void +SampleConsensusInitialAlignment::selectSamples ( + const PointCloudSource &cloud, int nr_samples, float min_sample_distance, std::vector &sample_indices) { if (nr_samples > static_cast (cloud.points.size ())) @@ -127,10 +130,10 @@ pcl::SampleConsensusInitialAlignment::select } } -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::SampleConsensusInitialAlignment::findSimilarFeatures ( - const FeatureCloud &input_features, const std::vector &sample_indices, + +template void +SampleConsensusInitialAlignment::findSimilarFeatures ( + const FeatureCloud &input_features, const std::vector &sample_indices, std::vector &corresponding_indices) { std::vector nn_indices (k_correspondences_); @@ -148,9 +151,9 @@ pcl::SampleConsensusInitialAlignment::findSi } } -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -template float -pcl::SampleConsensusInitialAlignment::computeErrorMetric ( + +template float +SampleConsensusInitialAlignment::computeErrorMetric ( const PointCloudSource &cloud, float) { std::vector nn_index (1); @@ -170,9 +173,9 @@ pcl::SampleConsensusInitialAlignment::comput return (error); } -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::SampleConsensusInitialAlignment::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess) + +template void +SampleConsensusInitialAlignment::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess) { // Some sanity checks first if (!input_features_) @@ -216,7 +219,7 @@ pcl::SampleConsensusInitialAlignment::comput final_transformation_ = guess; int i_iter = 0; converged_ = false; - if (!guess.isApprox (Eigen::Matrix4f::Identity (), 0.01f)) + if (!guess.isApprox (Eigen::Matrix4f::Identity (), 0.01f)) { // If guess is not the Identity matrix we check it. transformPointCloud (*input_, input_transformed, final_transformation_); @@ -252,5 +255,7 @@ pcl::SampleConsensusInitialAlignment::comput transformPointCloud (*input_, output, final_transformation_); } +} // namespace pcl + #endif //#ifndef IA_RANSAC_HPP_ diff --git a/registration/include/pcl/registration/impl/icp.hpp b/registration/include/pcl/registration/impl/icp.hpp index fb7f62f5..f533abbd 100644 --- a/registration/include/pcl/registration/impl/icp.hpp +++ b/registration/include/pcl/registration/impl/icp.hpp @@ -44,11 +44,14 @@ #include #include -/////////////////////////////////////////////////////////////////////////////////////////// + +namespace pcl +{ + template void -pcl::IterativeClosestPoint::transformCloud ( - const PointCloudSource &input, - PointCloudSource &output, +IterativeClosestPoint::transformCloud ( + const PointCloudSource &input, + PointCloudSource &output, const Matrix4 &transform) { Eigen::Vector4f pt (0.0f, 0.0f, 0.0f, 1.0f), pt_t; @@ -111,12 +114,11 @@ pcl::IterativeClosestPoint::transformCloud ( memcpy (data_out + z_idx_offset_, &pt_t[2], sizeof (float)); } } - } -/////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::IterativeClosestPoint::computeTransformation ( +IterativeClosestPoint::computeTransformation ( PointCloudSource &output, const Matrix4 &guess) { // Point cloud containing the correspondences of each point in @@ -137,7 +139,7 @@ pcl::IterativeClosestPoint::computeTransformat } else *input_transformed = *input_; - + transformation_ = Matrix4::Identity (); // Make blobs if necessary @@ -224,7 +226,7 @@ pcl::IterativeClosestPoint::computeTransformat // Transform the data transformCloud (*input_transformed, *input_transformed, transformation_); - // Obtain the final transformation + // Obtain the final transformation final_transformation_ = transformation_ * final_transformation_; ++nr_iterations_; @@ -250,8 +252,9 @@ pcl::IterativeClosestPoint::computeTransformat transformCloud (*input_, output, final_transformation_); } + template void -pcl::IterativeClosestPoint::determineRequiredBlobData () +IterativeClosestPoint::determineRequiredBlobData () { need_source_blob_ = false; need_target_blob_ = false; @@ -286,15 +289,17 @@ pcl::IterativeClosestPoint::determineRequiredB } } -/////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::IterativeClosestPointWithNormals::transformCloud ( - const PointCloudSource &input, - PointCloudSource &output, +IterativeClosestPointWithNormals::transformCloud ( + const PointCloudSource &input, + PointCloudSource &output, const Matrix4 &transform) { pcl::transformPointCloudWithNormals (input, output, transform); } +} // namespace pcl #endif /* PCL_REGISTRATION_IMPL_ICP_HPP_ */ + diff --git a/registration/include/pcl/registration/impl/incremental_registration.hpp b/registration/include/pcl/registration/impl/incremental_registration.hpp index 47b18431..d7bc4116 100644 --- a/registration/include/pcl/registration/impl/incremental_registration.hpp +++ b/registration/include/pcl/registration/impl/incremental_registration.hpp @@ -38,14 +38,21 @@ #ifndef PCL_REGISTRATION_IMPL_INCREMENTAL_REGISTRATION_HPP_ #define PCL_REGISTRATION_IMPL_INCREMENTAL_REGISTRATION_HPP_ + +namespace pcl +{ + +namespace registration +{ + template -pcl::registration::IncrementalRegistration::IncrementalRegistration () : +IncrementalRegistration::IncrementalRegistration () : delta_transform_ (Matrix4::Identity ()), abs_transform_ (Matrix4::Identity ()) {} template bool -pcl::registration::IncrementalRegistration::registerCloud (const PointCloudConstPtr& cloud, const Matrix4& delta_estimate) +IncrementalRegistration::registerCloud (const PointCloudConstPtr& cloud, const Matrix4& delta_estimate) { assert (registration_); @@ -76,28 +83,32 @@ pcl::registration::IncrementalRegistration::registerCloud (const } template inline typename pcl::registration::IncrementalRegistration::Matrix4 -pcl::registration::IncrementalRegistration::getDeltaTransform () const +IncrementalRegistration::getDeltaTransform () const { return (delta_transform_); } template inline typename pcl::registration::IncrementalRegistration::Matrix4 -pcl::registration::IncrementalRegistration::getAbsoluteTransform () const +IncrementalRegistration::getAbsoluteTransform () const { return (abs_transform_); } template inline void -pcl::registration::IncrementalRegistration::reset () +IncrementalRegistration::reset () { last_cloud_.reset (); delta_transform_ = abs_transform_ = Matrix4::Identity (); } template inline void -pcl::registration::IncrementalRegistration::setRegistration (RegistrationPtr registration) +IncrementalRegistration::setRegistration (RegistrationPtr registration) { registration_ = registration; } +} // namespace registration +} // namespace pcl + #endif /*PCL_REGISTRATION_IMPL_INCREMENTAL_REGISTRATION_HPP_*/ + diff --git a/registration/include/pcl/registration/impl/joint_icp.hpp b/registration/include/pcl/registration/impl/joint_icp.hpp index f8b16e73..bde6c317 100644 --- a/registration/include/pcl/registration/impl/joint_icp.hpp +++ b/registration/include/pcl/registration/impl/joint_icp.hpp @@ -1,16 +1,16 @@ /* * Software License Agreement (BSD License) - * + * * Point Cloud Library (PCL) - www.pointclouds.org * Copyright (c) 2009-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 @@ -20,7 +20,7 @@ * * 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 @@ -44,9 +44,11 @@ #include -/////////////////////////////////////////////////////////////////////////////////////////// +namespace pcl +{ + template void -pcl::JointIterativeClosestPoint::computeTransformation ( +JointIterativeClosestPoint::computeTransformation ( PointCloudSource &output, const Matrix4 &guess) { // Point clouds containing the correspondences of each point in @@ -63,7 +65,7 @@ pcl::JointIterativeClosestPoint::computeTransf correspondence_estimations_.resize (sources_.size ()); for (std::size_t i = 0; i < sources_.size (); i++) { - correspondence_estimations_[i] = correspondence_estimation_->clone (); + correspondence_estimations_[i] = correspondence_estimation_->clone (); KdTreeReciprocalPtr src_tree (new KdTreeReciprocal); KdTreePtr tgt_tree (new KdTree); correspondence_estimations_[i]->setSearchMethodTarget (tgt_tree); @@ -117,8 +119,6 @@ pcl::JointIterativeClosestPoint::computeTransf target_offset += targets_[i]->size (); } - - transformation_ = Matrix4::Identity (); // Make blobs if necessary determineRequiredBlobData (); @@ -176,7 +176,7 @@ pcl::JointIterativeClosestPoint::computeTransf toPCLPointCloud2 (*inputs_transformed[i], *input_transformed_blob); correspondence_estimations_[i]->setSourceNormals (input_transformed_blob); } - + // Estimate correspondences on each cloud pair separately if (use_reciprocal_correspondence_) { @@ -186,8 +186,8 @@ pcl::JointIterativeClosestPoint::computeTransf { correspondence_estimations_[i]->determineCorrespondences (*partial_correspondences_[i], corr_dist_threshold_); } - PCL_DEBUG ("[pcl::%s::computeTransformation] Found %d partial correspondences for cloud [%d]\n", - getClassName ().c_str (), + PCL_DEBUG ("[pcl::%s::computeTransformation] Found %d partial correspondences for cloud [%d]\n", + getClassName ().c_str (), partial_correspondences_[i]->size (), i); for (std::size_t j = 0; j < partial_correspondences_[i]->size (); j++) { @@ -199,7 +199,7 @@ pcl::JointIterativeClosestPoint::computeTransf } } PCL_DEBUG ("[pcl::%s::computeTransformation] Total correspondences: %d\n", getClassName ().c_str (), correspondences_->size ()); - + PCLPointCloud2::Ptr inputs_transformed_combined_blob; if (need_source_blob_) { @@ -244,7 +244,7 @@ pcl::JointIterativeClosestPoint::computeTransf this->transformCloud (*inputs_transformed[i], *inputs_transformed[i], transformation_); } - // Obtain the final transformation + // Obtain the final transformation final_transformation_ = transformation_ * final_transformation_; ++nr_iterations_; @@ -257,7 +257,7 @@ pcl::JointIterativeClosestPoint::computeTransf } while (!converged_); - PCL_DEBUG ("Transformation is:\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n", + PCL_DEBUG ("Transformation is:\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n", final_transformation_ (0, 0), final_transformation_ (0, 1), final_transformation_ (0, 2), final_transformation_ (0, 3), final_transformation_ (1, 0), final_transformation_ (1, 1), final_transformation_ (1, 2), final_transformation_ (1, 3), final_transformation_ (2, 0), final_transformation_ (2, 1), final_transformation_ (2, 2), final_transformation_ (2, 3), @@ -274,13 +274,14 @@ pcl::JointIterativeClosestPoint::computeTransf } - // By definition, this method will return an empty cloud (for compliance with the ICP API). + // By definition, this method will return an empty cloud (for compliance with the ICP API). // We can figure out a better solution, if necessary. output = PointCloudSource (); } - template void -pcl::JointIterativeClosestPoint::determineRequiredBlobData () + +template void +JointIterativeClosestPoint::determineRequiredBlobData () { need_source_blob_ = false; need_target_blob_ = false; @@ -320,7 +321,7 @@ pcl::JointIterativeClosestPoint::determineRequ } } +} // namespace pcl #endif /* PCL_REGISTRATION_IMPL_JOINT_ICP_HPP_ */ - diff --git a/registration/include/pcl/registration/impl/lum.hpp b/registration/include/pcl/registration/impl/lum.hpp index 9a54279a..51747480 100644 --- a/registration/include/pcl/registration/impl/lum.hpp +++ b/registration/include/pcl/registration/impl/lum.hpp @@ -43,58 +43,64 @@ #include -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +namespace pcl +{ + +namespace registration +{ + template inline void -pcl::registration::LUM::setLoopGraph (const SLAMGraphPtr &slam_graph) +LUM::setLoopGraph (const SLAMGraphPtr &slam_graph) { slam_graph_ = slam_graph; } -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -template inline typename pcl::registration::LUM::SLAMGraphPtr -pcl::registration::LUM::getLoopGraph () const + +template inline typename LUM::SLAMGraphPtr +LUM::getLoopGraph () const { return (slam_graph_); } -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -template typename pcl::registration::LUM::SLAMGraph::vertices_size_type -pcl::registration::LUM::getNumVertices () const + +template typename LUM::SLAMGraph::vertices_size_type +LUM::getNumVertices () const { return (num_vertices (*slam_graph_)); } -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::registration::LUM::setMaxIterations (int max_iterations) +LUM::setMaxIterations (int max_iterations) { max_iterations_ = max_iterations; } -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template inline int -pcl::registration::LUM::getMaxIterations () const +LUM::getMaxIterations () const { return (max_iterations_); } -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::registration::LUM::setConvergenceThreshold (float convergence_threshold) +LUM::setConvergenceThreshold (float convergence_threshold) { convergence_threshold_ = convergence_threshold; } -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template inline float -pcl::registration::LUM::getConvergenceThreshold () const +LUM::getConvergenceThreshold () const { return (convergence_threshold_); } -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -template typename pcl::registration::LUM::Vertex -pcl::registration::LUM::addPointCloud (const PointCloudPtr &cloud, const Eigen::Vector6f &pose) + +template typename LUM::Vertex +LUM::addPointCloud (const PointCloudPtr &cloud, const Eigen::Vector6f &pose) { Vertex v = add_vertex (*slam_graph_); (*slam_graph_)[v].cloud_ = cloud; @@ -108,9 +114,9 @@ pcl::registration::LUM::addPointCloud (const PointCloudPtr &cloud, const return (v); } -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template inline void -pcl::registration::LUM::setPointCloud (const Vertex &vertex, const PointCloudPtr &cloud) +LUM::setPointCloud (const Vertex &vertex, const PointCloudPtr &cloud) { if (vertex >= getNumVertices ()) { @@ -120,9 +126,9 @@ pcl::registration::LUM::setPointCloud (const Vertex &vertex, const Point (*slam_graph_)[vertex].cloud_ = cloud; } -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -template inline typename pcl::registration::LUM::PointCloudPtr -pcl::registration::LUM::getPointCloud (const Vertex &vertex) const + +template inline typename LUM::PointCloudPtr +LUM::getPointCloud (const Vertex &vertex) const { if (vertex >= getNumVertices ()) { @@ -132,9 +138,9 @@ pcl::registration::LUM::getPointCloud (const Vertex &vertex) const return ((*slam_graph_)[vertex].cloud_); } -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template inline void -pcl::registration::LUM::setPose (const Vertex &vertex, const Eigen::Vector6f &pose) +LUM::setPose (const Vertex &vertex, const Eigen::Vector6f &pose) { if (vertex >= getNumVertices ()) { @@ -149,9 +155,9 @@ pcl::registration::LUM::setPose (const Vertex &vertex, const Eigen::Vect (*slam_graph_)[vertex].pose_ = pose; } -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template inline Eigen::Vector6f -pcl::registration::LUM::getPose (const Vertex &vertex) const +LUM::getPose (const Vertex &vertex) const { if (vertex >= getNumVertices ()) { @@ -161,17 +167,17 @@ pcl::registration::LUM::getPose (const Vertex &vertex) const return ((*slam_graph_)[vertex].pose_); } -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template inline Eigen::Affine3f -pcl::registration::LUM::getTransformation (const Vertex &vertex) const +LUM::getTransformation (const Vertex &vertex) const { Eigen::Vector6f pose = getPose (vertex); return (pcl::getTransformation (pose (0), pose (1), pose (2), pose (3), pose (4), pose (5))); } -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::registration::LUM::setCorrespondences (const Vertex &source_vertex, const Vertex &target_vertex, const pcl::CorrespondencesPtr &corrs) +LUM::setCorrespondences (const Vertex &source_vertex, const Vertex &target_vertex, const pcl::CorrespondencesPtr &corrs) { if (source_vertex >= getNumVertices () || target_vertex >= getNumVertices () || source_vertex == target_vertex) { @@ -186,9 +192,9 @@ pcl::registration::LUM::setCorrespondences (const Vertex &source_vertex, (*slam_graph_)[e].corrs_ = corrs; } -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template inline pcl::CorrespondencesPtr -pcl::registration::LUM::getCorrespondences (const Vertex &source_vertex, const Vertex &target_vertex) const +LUM::getCorrespondences (const Vertex &source_vertex, const Vertex &target_vertex) const { if (source_vertex >= getNumVertices () || target_vertex >= getNumVertices ()) { @@ -206,9 +212,9 @@ pcl::registration::LUM::getCorrespondences (const Vertex &source_vertex, return ((*slam_graph_)[e].corrs_); } -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::registration::LUM::compute () +LUM::compute () { int n = static_cast (getNumVertices ()); if (n < 2) @@ -271,18 +277,18 @@ pcl::registration::LUM::compute () } } -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -template typename pcl::registration::LUM::PointCloudPtr -pcl::registration::LUM::getTransformedCloud (const Vertex &vertex) const + +template typename LUM::PointCloudPtr +LUM::getTransformedCloud (const Vertex &vertex) const { PointCloudPtr out (new PointCloud); pcl::transformPointCloud (*getPointCloud (vertex), *out, getTransformation (vertex)); return (out); } -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -template typename pcl::registration::LUM::PointCloudPtr -pcl::registration::LUM::getConcatenatedCloud () const + +template typename LUM::PointCloudPtr +LUM::getConcatenatedCloud () const { PointCloudPtr out (new PointCloud); typename SLAMGraph::vertex_iterator v, v_end; @@ -295,9 +301,9 @@ pcl::registration::LUM::getConcatenatedCloud () const return (out); } -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::registration::LUM::computeEdge (const Edge &e) +LUM::computeEdge (const Edge &e) { // Get necessary local data from graph PointCloudPtr source_cloud = (*slam_graph_)[source (e, *slam_graph_)].cloud_; @@ -399,9 +405,9 @@ pcl::registration::LUM::computeEdge (const Edge &e) (*slam_graph_)[e].cinvd_ = MZ * (1.0f / ss); } -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template inline Eigen::Matrix6f -pcl::registration::LUM::incidenceCorrection (const Eigen::Vector6f &pose) +LUM::incidenceCorrection (const Eigen::Vector6f &pose) { Eigen::Matrix6f out = Eigen::Matrix6f::Identity (); float cx = std::cos (pose (3)), sx = sinf (pose (3)), cy = std::cos (pose (4)), sy = sinf (pose (4)); @@ -421,6 +427,9 @@ pcl::registration::LUM::incidenceCorrection (const Eigen::Vector6f &pose return (out); } +} // namespace registration +} // namespace pcl + #define PCL_INSTANTIATE_LUM(T) template class PCL_EXPORTS pcl::registration::LUM; #endif // PCL_REGISTRATION_IMPL_LUM_HPP_ diff --git a/registration/include/pcl/registration/impl/meta_registration.hpp b/registration/include/pcl/registration/impl/meta_registration.hpp index b1a9d161..9a3479f7 100644 --- a/registration/include/pcl/registration/impl/meta_registration.hpp +++ b/registration/include/pcl/registration/impl/meta_registration.hpp @@ -38,13 +38,20 @@ #ifndef PCL_REGISTRATION_IMPL_META_REGISTRATION_HPP_ #define PCL_REGISTRATION_IMPL_META_REGISTRATION_HPP_ + +namespace pcl +{ + +namespace registration +{ + template -pcl::registration::MetaRegistration::MetaRegistration () : +MetaRegistration::MetaRegistration () : abs_transform_ (Matrix4::Identity ()) {} template bool -pcl::registration::MetaRegistration::registerCloud (const PointCloudConstPtr& new_cloud, const Matrix4& delta_estimate) +MetaRegistration::registerCloud (const PointCloudConstPtr& new_cloud, const Matrix4& delta_estimate) { assert (registration_); @@ -74,28 +81,33 @@ pcl::registration::MetaRegistration::registerCloud (const PointC return (converged); } -template inline typename pcl::registration::MetaRegistration::Matrix4 -pcl::registration::MetaRegistration::getAbsoluteTransform () const +template inline typename MetaRegistration::Matrix4 +MetaRegistration::getAbsoluteTransform () const { return (abs_transform_); } template inline void -pcl::registration::MetaRegistration::reset () +MetaRegistration::reset () { full_cloud_.reset (); abs_transform_ = Matrix4::Identity (); } template inline void -pcl::registration::MetaRegistration::setRegistration (RegistrationPtr reg) +MetaRegistration::setRegistration (RegistrationPtr reg) { registration_ = reg; } -template inline typename pcl::registration::MetaRegistration::PointCloudConstPtr -pcl::registration::MetaRegistration::getMetaCloud () const +template inline typename MetaRegistration::PointCloudConstPtr +MetaRegistration::getMetaCloud () const { return full_cloud_; } + +} // namespace registration +} // namespace pcl + #endif /*PCL_REGISTRATION_IMPL_META_REGISTRATION_HPP_*/ + diff --git a/registration/include/pcl/registration/impl/ndt.hpp b/registration/include/pcl/registration/impl/ndt.hpp index d5bfd774..da11a35a 100644 --- a/registration/include/pcl/registration/impl/ndt.hpp +++ b/registration/include/pcl/registration/impl/ndt.hpp @@ -41,9 +41,12 @@ #ifndef PCL_REGISTRATION_NDT_IMPL_H_ #define PCL_REGISTRATION_NDT_IMPL_H_ -////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +namespace pcl +{ + template -pcl::NormalDistributionsTransform::NormalDistributionsTransform () +NormalDistributionsTransform::NormalDistributionsTransform () : target_cells_ () , resolution_ (1.0f) , step_size_ (0.1) @@ -67,9 +70,9 @@ pcl::NormalDistributionsTransform::NormalDistributions max_iterations_ = 35; } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::NormalDistributionsTransform::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess) +NormalDistributionsTransform::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess) { nr_iterations_ = 0; converged_ = false; @@ -171,13 +174,13 @@ pcl::NormalDistributionsTransform::computeTransformati trans_probability_ = score / static_cast (input_->points.size ()); } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template double -pcl::NormalDistributionsTransform::computeDerivatives (Eigen::Matrix &score_gradient, - Eigen::Matrix &hessian, - PointCloudSource &trans_cloud, - Eigen::Matrix &p, - bool compute_hessian) +NormalDistributionsTransform::computeDerivatives (Eigen::Matrix &score_gradient, + Eigen::Matrix &hessian, + PointCloudSource &trans_cloud, + Eigen::Matrix &p, + bool compute_hessian) { // Original Point and Transformed Point PointSource x_pt, x_trans_pt; @@ -228,9 +231,9 @@ pcl::NormalDistributionsTransform::computeDerivatives return (score); } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::NormalDistributionsTransform::computeAngleDerivatives (Eigen::Matrix &p, bool compute_hessian) +NormalDistributionsTransform::computeAngleDerivatives (Eigen::Matrix &p, bool compute_hessian) { // Simplified math for near 0 angles double cx, cy, cz, sx, sy, sz; @@ -305,9 +308,9 @@ pcl::NormalDistributionsTransform::computeAngleDerivat } } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::NormalDistributionsTransform::computePointDerivatives (Eigen::Vector3d &x, bool compute_hessian) +NormalDistributionsTransform::computePointDerivatives (Eigen::Vector3d &x, bool compute_hessian) { // Calculate first derivative of Transformation Equation 6.17 w.r.t. transform vector p. // Derivative w.r.t. ith element of transform vector corresponds to column i, Equation 6.18 and 6.19 [Magnusson 2009] @@ -346,12 +349,12 @@ pcl::NormalDistributionsTransform::computePointDerivat } } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template double -pcl::NormalDistributionsTransform::updateDerivatives (Eigen::Matrix &score_gradient, - Eigen::Matrix &hessian, - Eigen::Vector3d &x_trans, Eigen::Matrix3d &c_inv, - bool compute_hessian) +NormalDistributionsTransform::updateDerivatives (Eigen::Matrix &score_gradient, + Eigen::Matrix &hessian, + Eigen::Vector3d &x_trans, Eigen::Matrix3d &c_inv, + bool compute_hessian) { Eigen::Vector3d cov_dxd_pi; // e^(-d_2/2 * (x_k - mu_k)^T Sigma_k^-1 (x_k - mu_k)) Equation 6.9 [Magnusson 2009] @@ -392,10 +395,10 @@ pcl::NormalDistributionsTransform::updateDerivatives ( return (score_inc); } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::NormalDistributionsTransform::computeHessian (Eigen::Matrix &hessian, - PointCloudSource &trans_cloud, Eigen::Matrix &) +NormalDistributionsTransform::computeHessian (Eigen::Matrix &hessian, + PointCloudSource &trans_cloud, Eigen::Matrix &) { // Original Point and Transformed Point PointSource x_pt, x_trans_pt; @@ -444,9 +447,9 @@ pcl::NormalDistributionsTransform::computeHessian (Eig } } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::NormalDistributionsTransform::updateHessian (Eigen::Matrix &hessian, Eigen::Vector3d &x_trans, Eigen::Matrix3d &c_inv) +NormalDistributionsTransform::updateHessian (Eigen::Matrix &hessian, Eigen::Vector3d &x_trans, Eigen::Matrix3d &c_inv) { Eigen::Vector3d cov_dxd_pi; // e^(-d_2/2 * (x_k - mu_k)^T Sigma_k^-1 (x_k - mu_k)) Equation 6.9 [Magnusson 2009] @@ -475,11 +478,11 @@ pcl::NormalDistributionsTransform::updateHessian (Eige } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template bool -pcl::NormalDistributionsTransform::updateIntervalMT (double &a_l, double &f_l, double &g_l, - double &a_u, double &f_u, double &g_u, - double a_t, double f_t, double g_t) +NormalDistributionsTransform::updateIntervalMT (double &a_l, double &f_l, double &g_l, + double &a_u, double &f_u, double &g_u, + double a_t, double f_t, double g_t) { // Case U1 in Update Algorithm and Case a in Modified Update Algorithm [More, Thuente 1994] if (f_t > f_l) @@ -513,11 +516,11 @@ pcl::NormalDistributionsTransform::updateIntervalMT (d return (true); } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template double -pcl::NormalDistributionsTransform::trialValueSelectionMT (double a_l, double f_l, double g_l, - double a_u, double f_u, double g_u, - double a_t, double f_t, double g_t) +NormalDistributionsTransform::trialValueSelectionMT (double a_l, double f_l, double g_l, + double a_u, double f_u, double g_u, + double a_t, double f_t, double g_t) { // Case 1 in Trial Value Selection [More, Thuente 1994] if (f_t > f_l) @@ -588,11 +591,11 @@ pcl::NormalDistributionsTransform::trialValueSelection return (a_u + (a_t - a_u) * (w - g_u - z) / (g_t - g_u + 2 * w)); } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template double -pcl::NormalDistributionsTransform::computeStepLengthMT (const Eigen::Matrix &x, Eigen::Matrix &step_dir, double step_init, double step_max, - double step_min, double &score, Eigen::Matrix &score_gradient, Eigen::Matrix &hessian, - PointCloudSource &trans_cloud) +NormalDistributionsTransform::computeStepLengthMT (const Eigen::Matrix &x, Eigen::Matrix &step_dir, double step_init, double step_max, + double step_min, double &score, Eigen::Matrix &score_gradient, Eigen::Matrix &hessian, + PointCloudSource &trans_cloud) { // Set the value of phi(0), Equation 1.3 [More, Thuente 1994] double phi_0 = -score; @@ -748,4 +751,7 @@ pcl::NormalDistributionsTransform::computeStepLengthMT return (a_t); } +} // namespace pcl + #endif // PCL_REGISTRATION_NDT_IMPL_H_ + diff --git a/registration/include/pcl/registration/impl/ndt_2d.hpp b/registration/include/pcl/registration/impl/ndt_2d.hpp index c34bdf35..d725e1e8 100644 --- a/registration/include/pcl/registration/impl/ndt_2d.hpp +++ b/registration/include/pcl/registration/impl/ndt_2d.hpp @@ -1,42 +1,43 @@ /* - * Software License Agreement (BSD License) - * - * Point Cloud Library (PCL) - www.pointclouds.org - * Copyright (c) 2011-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. - * - * $Id$ - * - */ +* Software License Agreement (BSD License) +* +* Point Cloud Library (PCL) - www.pointclouds.org +* Copyright (c) 2011-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. +* +* $Id$ +* +*/ + #ifndef PCL_NDT_2D_IMPL_H_ #define PCL_NDT_2D_IMPL_H_ @@ -48,332 +49,340 @@ namespace pcl { - namespace ndt2d + +namespace ndt2d +{ +/** \brief Class to store vector value and first and second derivatives + * (grad vector and hessian matrix), so they can be returned easily from + * functions + */ +template +struct ValueAndDerivatives +{ + ValueAndDerivatives () : hessian (), grad (), value () {} + + Eigen::Matrix hessian; + Eigen::Matrix grad; + T value; + + static ValueAndDerivatives + Zero () + { + ValueAndDerivatives r; + r.hessian = Eigen::Matrix::Zero (); + r.grad = Eigen::Matrix::Zero (); + r.value = 0; + return r; + } + + ValueAndDerivatives& + operator+= (ValueAndDerivatives const& r) + { + hessian += r.hessian; + grad += r.grad; + value += r.value; + return *this; + } +}; + +/** \brief A normal distribution estimation class. + * + * First the indices of of the points from a point cloud that should be + * modelled by the distribution are added with addIdx (...). + * + * Then estimateParams (...) uses the stored point indices to estimate the + * parameters of a normal distribution, and discards the stored indices. + * + * Finally the distriubution, and its derivatives, may be evaluated at any + * point using test (...). + */ +template +class NormalDist +{ + using PointCloud = pcl::PointCloud; + +public: + NormalDist () + : min_n_ (3), n_ (0) + { + } + + /** \brief Store a point index to use later for estimating distribution parameters. + * \param[in] i Point index to store + */ + void + addIdx (std::size_t i) { - /** \brief Class to store vector value and first and second derivatives - * (grad vector and hessian matrix), so they can be returned easily from - * functions - */ - template - struct ValueAndDerivatives + pt_indices_.push_back (i); + } + + /** \brief Estimate the normal distribution parameters given the point indices provided. Memory of point indices is cleared. + * \param[in] cloud Point cloud corresponding to indices passed to addIdx. + * \param[in] min_covar_eigvalue_mult Set the smallest eigenvalue to this times the largest. + */ + void + estimateParams (const PointCloud& cloud, double min_covar_eigvalue_mult = 0.001) + { + Eigen::Vector2d sx = Eigen::Vector2d::Zero (); + Eigen::Matrix2d sxx = Eigen::Matrix2d::Zero (); + + for (auto i = pt_indices_.cbegin (); i != pt_indices_.cend (); i++) { - ValueAndDerivatives () : hessian (), grad (), value () {} - - Eigen::Matrix hessian; - Eigen::Matrix grad; - T value; - - static ValueAndDerivatives - Zero () - { - ValueAndDerivatives r; - r.hessian = Eigen::Matrix::Zero (); - r.grad = Eigen::Matrix::Zero (); - r.value = 0; - return r; - } + Eigen::Vector2d p (cloud[*i]. x, cloud[*i]. y); + sx += p; + sxx += p * p.transpose (); + } + + n_ = pt_indices_.size (); + + if (n_ >= min_n_) + { + mean_ = sx / static_cast (n_); + // Using maximum likelihood estimation as in the original paper + Eigen::Matrix2d covar = (sxx - 2 * (sx * mean_.transpose ())) / static_cast (n_) + mean_ * mean_.transpose (); - ValueAndDerivatives& - operator+= (ValueAndDerivatives const& r) + Eigen::SelfAdjointEigenSolver solver (covar); + if (solver.eigenvalues ()[0] < min_covar_eigvalue_mult * solver.eigenvalues ()[1]) { - hessian += r.hessian; - grad += r.grad; - value += r.value; - return *this; + PCL_DEBUG ("[pcl::NormalDist::estimateParams] NDT normal fit: adjusting eigenvalue %f\n", solver.eigenvalues ()[0]); + Eigen::Matrix2d l = solver.eigenvalues ().asDiagonal (); + Eigen::Matrix2d q = solver.eigenvectors (); + // set minimum smallest eigenvalue: + l (0,0) = l (1,1) * min_covar_eigvalue_mult; + covar = q * l * q.transpose (); } - }; - - /** \brief A normal distribution estimation class. - * - * First the indices of of the points from a point cloud that should be - * modelled by the distribution are added with addIdx (...). - * - * Then estimateParams (...) uses the stored point indices to estimate the - * parameters of a normal distribution, and discards the stored indices. - * - * Finally the distriubution, and its derivatives, may be evaluated at any - * point using test (...). - */ - template - class NormalDist - { - using PointCloud = pcl::PointCloud; - - public: - NormalDist () - : min_n_ (3), n_ (0) - { - } - - /** \brief Store a point index to use later for estimating distribution parameters. - * \param[in] i Point index to store - */ - void - addIdx (std::size_t i) - { - pt_indices_.push_back (i); - } - - /** \brief Estimate the normal distribution parameters given the point indices provided. Memory of point indices is cleared. - * \param[in] cloud Point cloud corresponding to indices passed to addIdx. - * \param[in] min_covar_eigvalue_mult Set the smallest eigenvalue to this times the largest. - */ - void - estimateParams (const PointCloud& cloud, double min_covar_eigvalue_mult = 0.001) - { - Eigen::Vector2d sx = Eigen::Vector2d::Zero (); - Eigen::Matrix2d sxx = Eigen::Matrix2d::Zero (); - - for (auto i = pt_indices_.cbegin (); i != pt_indices_.cend (); i++) - { - Eigen::Vector2d p (cloud[*i]. x, cloud[*i]. y); - sx += p; - sxx += p * p.transpose (); - } - - n_ = pt_indices_.size (); - - if (n_ >= min_n_) - { - mean_ = sx / static_cast (n_); - // Using maximum likelihood estimation as in the original paper - Eigen::Matrix2d covar = (sxx - 2 * (sx * mean_.transpose ())) / static_cast (n_) + mean_ * mean_.transpose (); - - Eigen::SelfAdjointEigenSolver solver (covar); - if (solver.eigenvalues ()[0] < min_covar_eigvalue_mult * solver.eigenvalues ()[1]) - { - PCL_DEBUG ("[pcl::NormalDist::estimateParams] NDT normal fit: adjusting eigenvalue %f\n", solver.eigenvalues ()[0]); - Eigen::Matrix2d l = solver.eigenvalues ().asDiagonal (); - Eigen::Matrix2d q = solver.eigenvectors (); - // set minimum smallest eigenvalue: - l (0,0) = l (1,1) * min_covar_eigvalue_mult; - covar = q * l * q.transpose (); - } - covar_inv_ = covar.inverse (); - } - - pt_indices_.clear (); - } - - /** \brief Return the 'score' (denormalised likelihood) and derivatives of score of the point p given this distribution. - * \param[in] transformed_pt Location to evaluate at. - * \param[in] cos_theta sin(theta) of the current rotation angle of rigid transformation: to avoid repeated evaluation - * \param[in] sin_theta cos(theta) of the current rotation angle of rigid transformation: to avoid repeated evaluation - * estimateParams must have been called after at least three points were provided, or this will return zero. - * - */ - ValueAndDerivatives<3,double> - test (const PointT& transformed_pt, const double& cos_theta, const double& sin_theta) const - { - if (n_ < min_n_) - return ValueAndDerivatives<3,double>::Zero (); - - ValueAndDerivatives<3,double> r; - const double x = transformed_pt.x; - const double y = transformed_pt.y; - const Eigen::Vector2d p_xy (transformed_pt.x, transformed_pt.y); - const Eigen::Vector2d q = p_xy - mean_; - const Eigen::RowVector2d qt_cvi (q.transpose () * covar_inv_); - const double exp_qt_cvi_q = std::exp (-0.5 * double (qt_cvi * q)); - r.value = -exp_qt_cvi_q; - - Eigen::Matrix jacobian; - jacobian << - 1, 0, -(x * sin_theta + y*cos_theta), - 0, 1, x * cos_theta - y*sin_theta; - - for (std::size_t i = 0; i < 3; i++) - r.grad[i] = double (qt_cvi * jacobian.col (i)) * exp_qt_cvi_q; - - // second derivative only for i == j == 2: - const Eigen::Vector2d d2q_didj ( - y * sin_theta - x*cos_theta, - -(x * sin_theta + y*cos_theta) - ); - - for (std::size_t i = 0; i < 3; i++) - for (std::size_t j = 0; j < 3; j++) - r.hessian (i,j) = -exp_qt_cvi_q * ( - double (-qt_cvi*jacobian.col (i)) * double (-qt_cvi*jacobian.col (j)) + - (-qt_cvi * ((i==2 && j==2)? d2q_didj : Eigen::Vector2d::Zero ())) + - (-jacobian.col (j).transpose () * covar_inv_ * jacobian.col (i)) - ); - - return r; - } - - protected: - const std::size_t min_n_; - - std::size_t n_; - std::vector pt_indices_; - Eigen::Vector2d mean_; - Eigen::Matrix2d covar_inv_; - }; - - /** \brief Build a set of normal distributions modelling a 2D point cloud, - * and provide the value and derivatives of the model at any point via the - * test (...) function. - */ - template - class NDTSingleGrid: public boost::noncopyable - { - using PointCloud = pcl::PointCloud; - using PointCloudConstPtr = typename PointCloud::ConstPtr; - using NormalDist = pcl::ndt2d::NormalDist; - - public: - NDTSingleGrid (PointCloudConstPtr cloud, - const Eigen::Vector2f& about, - const Eigen::Vector2f& extent, - const Eigen::Vector2f& step) - : min_ (about - extent), max_ (min_ + 2*extent), step_ (step), - cells_ ((max_[0]-min_[0]) / step_[0], - (max_[1]-min_[1]) / step_[1]), - normal_distributions_ (cells_[0], cells_[1]) - { - // sort through all points, assigning them to distributions: - std::size_t used_points = 0; - for (std::size_t i = 0; i < cloud->size (); i++) - if (NormalDist* n = normalDistForPoint (cloud->at (i))) - { - n->addIdx (i); - used_points++; - } - - PCL_DEBUG ("[pcl::NDTSingleGrid] NDT single grid %dx%d using %d/%d points\n", cells_[0], cells_[1], used_points, cloud->size ()); - - // then bake the distributions such that they approximate the - // points (and throw away memory of the points) - for (int x = 0; x < cells_[0]; x++) - for (int y = 0; y < cells_[1]; y++) - normal_distributions_.coeffRef (x,y).estimateParams (*cloud); - } - - /** \brief Return the 'score' (denormalised likelihood) and derivatives of score of the point p given this distribution. - * \param[in] transformed_pt Location to evaluate at. - * \param[in] cos_theta sin(theta) of the current rotation angle of rigid transformation: to avoid repeated evaluation - * \param[in] sin_theta cos(theta) of the current rotation angle of rigid transformation: to avoid repeated evaluation - */ - ValueAndDerivatives<3,double> - test (const PointT& transformed_pt, const double& cos_theta, const double& sin_theta) const - { - const NormalDist* n = normalDistForPoint (transformed_pt); - // index is in grid, return score from the normal distribution from - // the correct part of the grid: - if (n) - return n->test (transformed_pt, cos_theta, sin_theta); - return ValueAndDerivatives<3,double>::Zero (); - } - - protected: - /** \brief Return the normal distribution covering the location of point p - * \param[in] p a point - */ - NormalDist* - normalDistForPoint (PointT const& p) const - { - // this would be neater in 3d... - Eigen::Vector2f idxf; - for (std::size_t i = 0; i < 2; i++) - idxf[i] = (p.getVector3fMap ()[i] - min_[i]) / step_[i]; - Eigen::Vector2i idxi = idxf.cast (); - for (std::size_t i = 0; i < 2; i++) - if (idxi[i] >= cells_[i] || idxi[i] < 0) - return nullptr; - // const cast to avoid duplicating this function in const and - // non-const variants... - return const_cast (&normal_distributions_.coeffRef (idxi[0], idxi[1])); - } - - Eigen::Vector2f min_; - Eigen::Vector2f max_; - Eigen::Vector2f step_; - Eigen::Vector2i cells_; - - Eigen::Matrix normal_distributions_; - }; - - /** \brief Build a Normal Distributions Transform of a 2D point cloud. This - * consists of the sum of four overlapping models of the original points - * with normal distributions. - * The value and derivatives of the model at any point can be evaluated - * with the test (...) function. - */ - template - class NDT2D: public boost::noncopyable + covar_inv_ = covar.inverse (); + } + + pt_indices_.clear (); + } + + /** \brief Return the 'score' (denormalised likelihood) and derivatives of score of the point p given this distribution. + * \param[in] transformed_pt Location to evaluate at. + * \param[in] cos_theta sin(theta) of the current rotation angle of rigid transformation: to avoid repeated evaluation + * \param[in] sin_theta cos(theta) of the current rotation angle of rigid transformation: to avoid repeated evaluation + * estimateParams must have been called after at least three points were provided, or this will return zero. + * + */ + ValueAndDerivatives<3,double> + test (const PointT& transformed_pt, const double& cos_theta, const double& sin_theta) const + { + if (n_ < min_n_) + return ValueAndDerivatives<3,double>::Zero (); + + ValueAndDerivatives<3,double> r; + const double x = transformed_pt.x; + const double y = transformed_pt.y; + const Eigen::Vector2d p_xy (transformed_pt.x, transformed_pt.y); + const Eigen::Vector2d q = p_xy - mean_; + const Eigen::RowVector2d qt_cvi (q.transpose () * covar_inv_); + const double exp_qt_cvi_q = std::exp (-0.5 * double (qt_cvi * q)); + r.value = -exp_qt_cvi_q; + + Eigen::Matrix jacobian; + jacobian << + 1, 0, -(x * sin_theta + y*cos_theta), + 0, 1, x * cos_theta - y*sin_theta; + + for (std::size_t i = 0; i < 3; i++) + r.grad[i] = double (qt_cvi * jacobian.col (i)) * exp_qt_cvi_q; + + // second derivative only for i == j == 2: + const Eigen::Vector2d d2q_didj ( + y * sin_theta - x*cos_theta, + -(x * sin_theta + y*cos_theta) + ); + + for (std::size_t i = 0; i < 3; i++) + for (std::size_t j = 0; j < 3; j++) + r.hessian (i,j) = -exp_qt_cvi_q * ( + double (-qt_cvi*jacobian.col (i)) * double (-qt_cvi*jacobian.col (j)) + + (-qt_cvi * ((i==2 && j==2)? d2q_didj : Eigen::Vector2d::Zero ())) + + (-jacobian.col (j).transpose () * covar_inv_ * jacobian.col (i)) + ); + + return r; + } + +protected: + const std::size_t min_n_; + + std::size_t n_; + std::vector pt_indices_; + Eigen::Vector2d mean_; + Eigen::Matrix2d covar_inv_; +}; + +/** \brief Build a set of normal distributions modelling a 2D point cloud, + * and provide the value and derivatives of the model at any point via the + * test (...) function. + */ +template +class NDTSingleGrid: public boost::noncopyable +{ + using PointCloud = pcl::PointCloud; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + using NormalDist = pcl::ndt2d::NormalDist; + +public: + NDTSingleGrid (PointCloudConstPtr cloud, + const Eigen::Vector2f& about, + const Eigen::Vector2f& extent, + const Eigen::Vector2f& step) + : min_ (about - extent), max_ (min_ + 2*extent), step_ (step), + cells_ ((max_[0]-min_[0]) / step_[0], + (max_[1]-min_[1]) / step_[1]), + normal_distributions_ (cells_[0], cells_[1]) + { + // sort through all points, assigning them to distributions: + std::size_t used_points = 0; + for (std::size_t i = 0; i < cloud->size (); i++) + if (NormalDist* n = normalDistForPoint (cloud->at (i))) { - using PointCloud = pcl::PointCloud; - using PointCloudConstPtr = typename PointCloud::ConstPtr; - using SingleGrid = NDTSingleGrid; - - public: - /** \brief - * \param[in] cloud the input point cloud - * \param[in] about Centre of the grid for normal distributions model - * \param[in] extent Extent of grid for normal distributions model - * \param[in] step Size of region that each normal distribution will model - */ - NDT2D (PointCloudConstPtr cloud, - const Eigen::Vector2f& about, - const Eigen::Vector2f& extent, - const Eigen::Vector2f& step) - { - Eigen::Vector2f dx (step[0]/2, 0); - Eigen::Vector2f dy (0, step[1]/2); - single_grids_[0].reset(new SingleGrid (cloud, about, extent, step)); - single_grids_[1].reset(new SingleGrid (cloud, about +dx, extent, step)); - single_grids_[2].reset(new SingleGrid (cloud, about +dy, extent, step)); - single_grids_[3].reset(new SingleGrid (cloud, about +dx+dy, extent, step)); - } - - /** \brief Return the 'score' (denormalised likelihood) and derivatives of score of the point p given this distribution. - * \param[in] transformed_pt Location to evaluate at. - * \param[in] cos_theta sin(theta) of the current rotation angle of rigid transformation: to avoid repeated evaluation - * \param[in] sin_theta cos(theta) of the current rotation angle of rigid transformation: to avoid repeated evaluation - */ - ValueAndDerivatives<3,double> - test (const PointT& transformed_pt, const double& cos_theta, const double& sin_theta) const - { - ValueAndDerivatives<3,double> r = ValueAndDerivatives<3,double>::Zero (); - for (const auto &single_grid : single_grids_) - r += single_grid->test (transformed_pt, cos_theta, sin_theta); - return r; - } - - protected: - std::shared_ptr single_grids_[4]; - }; - - } // namespace ndt2d + n->addIdx (i); + used_points++; + } + + PCL_DEBUG ("[pcl::NDTSingleGrid] NDT single grid %dx%d using %d/%d points\n", cells_[0], cells_[1], used_points, cloud->size ()); + + // then bake the distributions such that they approximate the + // points (and throw away memory of the points) + for (int x = 0; x < cells_[0]; x++) + for (int y = 0; y < cells_[1]; y++) + normal_distributions_.coeffRef (x,y).estimateParams (*cloud); + } + + /** \brief Return the 'score' (denormalised likelihood) and derivatives of score of the point p given this distribution. + * \param[in] transformed_pt Location to evaluate at. + * \param[in] cos_theta sin(theta) of the current rotation angle of rigid transformation: to avoid repeated evaluation + * \param[in] sin_theta cos(theta) of the current rotation angle of rigid transformation: to avoid repeated evaluation + */ + ValueAndDerivatives<3,double> + test (const PointT& transformed_pt, const double& cos_theta, const double& sin_theta) const + { + const NormalDist* n = normalDistForPoint (transformed_pt); + // index is in grid, return score from the normal distribution from + // the correct part of the grid: + if (n) + return n->test (transformed_pt, cos_theta, sin_theta); + return ValueAndDerivatives<3,double>::Zero (); + } + +protected: + /** \brief Return the normal distribution covering the location of point p + * \param[in] p a point + */ + NormalDist* + normalDistForPoint (PointT const& p) const + { + // this would be neater in 3d... + Eigen::Vector2f idxf; + for (std::size_t i = 0; i < 2; i++) + idxf[i] = (p.getVector3fMap ()[i] - min_[i]) / step_[i]; + Eigen::Vector2i idxi = idxf.cast (); + for (std::size_t i = 0; i < 2; i++) + if (idxi[i] >= cells_[i] || idxi[i] < 0) + return nullptr; + // const cast to avoid duplicating this function in const and + // non-const variants... + return const_cast (&normal_distributions_.coeffRef (idxi[0], idxi[1])); + } + + Eigen::Vector2f min_; + Eigen::Vector2f max_; + Eigen::Vector2f step_; + Eigen::Vector2i cells_; + + Eigen::Matrix normal_distributions_; +}; + +/** \brief Build a Normal Distributions Transform of a 2D point cloud. This + * consists of the sum of four overlapping models of the original points + * with normal distributions. + * The value and derivatives of the model at any point can be evaluated + * with the test (...) function. + */ +template +class NDT2D: public boost::noncopyable +{ + using PointCloud = pcl::PointCloud; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + using SingleGrid = NDTSingleGrid; + +public: + /** \brief + * \param[in] cloud the input point cloud + * \param[in] about Centre of the grid for normal distributions model + * \param[in] extent Extent of grid for normal distributions model + * \param[in] step Size of region that each normal distribution will model + */ + NDT2D (PointCloudConstPtr cloud, + const Eigen::Vector2f& about, + const Eigen::Vector2f& extent, + const Eigen::Vector2f& step) + { + Eigen::Vector2f dx (step[0]/2, 0); + Eigen::Vector2f dy (0, step[1]/2); + single_grids_[0].reset(new SingleGrid (cloud, about, extent, step)); + single_grids_[1].reset(new SingleGrid (cloud, about +dx, extent, step)); + single_grids_[2].reset(new SingleGrid (cloud, about +dy, extent, step)); + single_grids_[3].reset(new SingleGrid (cloud, about +dx+dy, extent, step)); + } + + /** \brief Return the 'score' (denormalised likelihood) and derivatives of score of the point p given this distribution. + * \param[in] transformed_pt Location to evaluate at. + * \param[in] cos_theta sin(theta) of the current rotation angle of rigid transformation: to avoid repeated evaluation + * \param[in] sin_theta cos(theta) of the current rotation angle of rigid transformation: to avoid repeated evaluation + */ + ValueAndDerivatives<3,double> + test (const PointT& transformed_pt, const double& cos_theta, const double& sin_theta) const + { + ValueAndDerivatives<3,double> r = ValueAndDerivatives<3,double>::Zero (); + for (const auto &single_grid : single_grids_) + r += single_grid->test (transformed_pt, cos_theta, sin_theta); + return r; + } + +protected: + std::shared_ptr single_grids_[4]; +}; + +} // namespace ndt2d } // namespace pcl namespace Eigen { - /* This NumTraits specialisation is necessary because NormalDist is used as - * the element type of an Eigen Matrix. - */ - template struct NumTraits > + +/* This NumTraits specialisation is necessary because NormalDist is used as +* the element type of an Eigen Matrix. +*/ +template +struct NumTraits > +{ + using Real = double; + using Literal = double; + static Real dummy_precision () { return 1.0; } + enum { - using Real = double; - using Literal = double; - static Real dummy_precision () { return 1.0; } - enum { - IsComplex = 0, - IsInteger = 0, - IsSigned = 0, - RequireInitialization = 1, - ReadCost = 1, - AddCost = 1, - MulCost = 1 - }; + IsComplex = 0, + IsInteger = 0, + IsSigned = 0, + RequireInitialization = 1, + ReadCost = 1, + AddCost = 1, + MulCost = 1 }; -} +}; + +} // namespace Eigen + + +namespace pcl +{ -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template void -pcl::NormalDistributionsTransform2D::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess) +NormalDistributionsTransform2D::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess) { PointCloudSource intm_cloud = output; @@ -384,13 +393,13 @@ pcl::NormalDistributionsTransform2D::computeTransforma { transformation_ = guess; transformPointCloud (output, intm_cloud, transformation_); - } + } // build Normal Distribution Transform of target cloud: ndt2d::NDT2D target_ndt (target_, grid_centre_, grid_extent_, grid_step_); - + // can't seem to use .block<> () member function on transformation_ - // directly... gcc bug? + // directly... gcc bug? Eigen::Matrix4f& transformation = transformation_; @@ -410,12 +419,12 @@ pcl::NormalDistributionsTransform2D::computeTransforma { const double cos_theta = std::cos (xytheta_transformation[2]); const double sin_theta = std::sin (xytheta_transformation[2]); - previous_transformation_ = transformation; + previous_transformation_ = transformation; ndt2d::ValueAndDerivatives<3, double> score = ndt2d::ValueAndDerivatives<3, double>::Zero (); for (std::size_t i = 0; i < intm_cloud.size (); i++) score += target_ndt.test (intm_cloud[i], cos_theta, sin_theta); - + PCL_DEBUG ("[pcl::NormalDistributionsTransform2D::computeTransformation] NDT score %f (x=%f,y=%f,r=%f)\n", float (score.value), xytheta_transformation[0], xytheta_transformation[1], xytheta_transformation[2] ); @@ -447,12 +456,12 @@ pcl::NormalDistributionsTransform2D::computeTransforma assert (solver.eigenvalues ()[0].real () >= 0 && solver.eigenvalues ()[1].real () >= 0 && solver.eigenvalues ()[2].real () >= 0); - + Eigen::Vector3d delta_transformation (-score.hessian.inverse () * score.grad); Eigen::Vector3d new_transformation = xytheta_transformation + newton_lambda_.cwiseProduct (delta_transformation); xytheta_transformation = new_transformation; - + // update transformation matrix from x, y, theta: transformation.block<3,3> (0,0).matrix () = Eigen::Matrix3f (Eigen::AngleAxisf (static_cast (xytheta_transformation[2]), Eigen::Vector3f::UnitZ ())); transformation.block<3,1> (0,3).matrix () = Eigen::Vector3f (static_cast (xytheta_transformation[0]), static_cast (xytheta_transformation[1]), 0.0f); @@ -464,11 +473,11 @@ pcl::NormalDistributionsTransform2D::computeTransforma PCL_ERROR ("[pcl::NormalDistributionsTransform2D::computeTransformation] no overlap: try increasing the size or reducing the step of the grid\n"); break; } - + transformPointCloud (output, intm_cloud, transformation); nr_iterations_++; - + if (update_visualizer_) update_visualizer_ (output, *indices_, *target_, *indices_); @@ -492,5 +501,7 @@ pcl::NormalDistributionsTransform2D::computeTransforma output = intm_cloud; } +} // namespace pcl + #endif // PCL_NDT_2D_IMPL_H_ - + diff --git a/registration/include/pcl/registration/impl/pairwise_graph_registration.hpp b/registration/include/pcl/registration/impl/pairwise_graph_registration.hpp index 2a577734..1f2de87a 100644 --- a/registration/include/pcl/registration/impl/pairwise_graph_registration.hpp +++ b/registration/include/pcl/registration/impl/pairwise_graph_registration.hpp @@ -37,11 +37,16 @@ * $Id$ * */ + #ifndef PCL_REGISTRATION_IMPL_PAIRWISE_GRAPH_REGISTRATION_HPP_ #define PCL_REGISTRATION_IMPL_PAIRWISE_GRAPH_REGISTRATION_HPP_ -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + +namespace pcl +{ + template void -pcl::PairwiseGraphRegistration::computeRegistration () +PairwiseGraphRegistration::computeRegistration () { if (!registration_method_) { @@ -76,4 +81,8 @@ pcl::PairwiseGraphRegistration::computeRegistration () registration_method_->setInputTarget (boost::get_cloud (last_aligned_vertex_, *(graph_handler_->getGraph ()))); } } + +} // namespace pcl + #endif //PCL_REGISTRATION_IMPL_PAIRWISE_GRAPH_REGISTRATION_HPP_ + diff --git a/registration/include/pcl/registration/impl/pyramid_feature_matching.hpp b/registration/include/pcl/registration/impl/pyramid_feature_matching.hpp index 5dfd6c49..f986e8b8 100644 --- a/registration/include/pcl/registration/impl/pyramid_feature_matching.hpp +++ b/registration/include/pcl/registration/impl/pyramid_feature_matching.hpp @@ -45,10 +45,13 @@ #include #include -////////////////////////////////////////////////////////////////////////////////////////////// + +namespace pcl +{ + template float -pcl::PyramidFeatureHistogram::comparePyramidFeatureHistograms (const PyramidFeatureHistogramPtr &pyramid_a, - const PyramidFeatureHistogramPtr &pyramid_b) +PyramidFeatureHistogram::comparePyramidFeatureHistograms (const PyramidFeatureHistogramPtr &pyramid_a, + const PyramidFeatureHistogramPtr &pyramid_b) { // do a few consistency checks before and during the computation if (pyramid_a->nr_dimensions != pyramid_b->nr_dimensions) @@ -113,9 +116,8 @@ pcl::PyramidFeatureHistogram::comparePyramidFeatureHistograms (con } -////////////////////////////////////////////////////////////////////////////////////////////// template -pcl::PyramidFeatureHistogram::PyramidFeatureHistogram () : +PyramidFeatureHistogram::PyramidFeatureHistogram () : nr_dimensions (0), nr_levels (0), nr_features (0), feature_representation_ (new DefaultPointRepresentation), is_computed_ (false), @@ -123,9 +125,9 @@ pcl::PyramidFeatureHistogram::PyramidFeatureHistogram () : { } -////////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::PyramidFeatureHistogram::PyramidFeatureHistogramLevel::initializeHistogramLevel () +PyramidFeatureHistogram::PyramidFeatureHistogramLevel::initializeHistogramLevel () { std::size_t total_vector_size = 1; for (std::vector::iterator dim_it = bins_per_dimension.begin (); dim_it != bins_per_dimension.end (); ++dim_it) @@ -135,9 +137,8 @@ pcl::PyramidFeatureHistogram::PyramidFeatureHistogramLevel::initia } -////////////////////////////////////////////////////////////////////////////////////////////// template bool -pcl::PyramidFeatureHistogram::initializeHistogram () +PyramidFeatureHistogram::initializeHistogram () { // a few consistency checks before starting the computations if (!PCLBase::initCompute ()) @@ -202,10 +203,9 @@ pcl::PyramidFeatureHistogram::initializeHistogram () } -////////////////////////////////////////////////////////////////////////////////////////////// template unsigned int& -pcl::PyramidFeatureHistogram::at (std::vector &access, - std::size_t &level) +PyramidFeatureHistogram::at (std::vector &access, + std::size_t &level) { if (access.size () != nr_dimensions) { @@ -231,10 +231,9 @@ pcl::PyramidFeatureHistogram::at (std::vector &access } -////////////////////////////////////////////////////////////////////////////////////////////// template unsigned int& -pcl::PyramidFeatureHistogram::at (std::vector &feature, - std::size_t &level) +PyramidFeatureHistogram::at (std::vector &feature, + std::size_t &level) { if (feature.size () != nr_dimensions) { @@ -255,10 +254,9 @@ pcl::PyramidFeatureHistogram::at (std::vector &feature, } -////////////////////////////////////////////////////////////////////////////////////////////// template void -pcl::PyramidFeatureHistogram::convertFeatureToVector (const PointFeature &feature, - std::vector &feature_vector) +PyramidFeatureHistogram::convertFeatureToVector (const PointFeature &feature, + std::vector &feature_vector) { // convert feature to vector representation feature_vector.resize (feature_representation_->getNumberOfDimensions ()); @@ -271,9 +269,8 @@ pcl::PyramidFeatureHistogram::convertFeatureToVector (const PointF } -////////////////////////////////////////////////////////////////////////////////////////////// template void -pcl::PyramidFeatureHistogram::compute () +PyramidFeatureHistogram::compute () { if (!initializeHistogram ()) return; @@ -289,14 +286,16 @@ pcl::PyramidFeatureHistogram::compute () } -////////////////////////////////////////////////////////////////////////////////////////////// template void -pcl::PyramidFeatureHistogram::addFeature (std::vector &feature) +PyramidFeatureHistogram::addFeature (std::vector &feature) { for (std::size_t level_i = 0; level_i < nr_levels; ++level_i) at (feature, level_i) ++; } +} // namespace pcl + #define PCL_INSTANTIATE_PyramidFeatureHistogram(PointFeature) template class PCL_EXPORTS pcl::PyramidFeatureHistogram; #endif /* PCL_REGISTRATION_IMPL_PYRAMID_FEATURE_MATCHING_H_ */ + diff --git a/registration/include/pcl/registration/impl/registration.hpp b/registration/include/pcl/registration/impl/registration.hpp index 88dfab75..bd40767a 100644 --- a/registration/include/pcl/registration/impl/registration.hpp +++ b/registration/include/pcl/registration/impl/registration.hpp @@ -38,9 +38,13 @@ * */ -/////////////////////////////////////////////////////////////////////////////////////////// +#pragma once + +namespace pcl +{ + template inline void -pcl::Registration::setInputTarget (const PointCloudTargetConstPtr &cloud) +Registration::setInputTarget (const PointCloudTargetConstPtr &cloud) { if (cloud->points.empty ()) { @@ -51,9 +55,9 @@ pcl::Registration::setInputTarget (const Point target_cloud_updated_ = true; } -/////////////////////////////////////////////////////////////////////////////////////////// + template bool -pcl::Registration::initCompute () +Registration::initCompute () { if (!target_) { @@ -68,23 +72,22 @@ pcl::Registration::initCompute () target_cloud_updated_ = false; } - // Update the correspondence estimation if (correspondence_estimation_) { correspondence_estimation_->setSearchMethodTarget (tree_, force_no_recompute_); correspondence_estimation_->setSearchMethodSource (tree_reciprocal_, force_no_recompute_reciprocal_); } - + // Note: we /cannot/ update the search method on all correspondence rejectors, because we know // nothing about them. If they should be cached, they must be cached individually. return (PCLBase::initCompute ()); } -/////////////////////////////////////////////////////////////////////////////////////////// + template bool -pcl::Registration::initComputeReciprocal () +Registration::initComputeReciprocal () { if (!input_) { @@ -100,9 +103,9 @@ pcl::Registration::initComputeReciprocal () return (true); } -////////////////////////////////////////////////////////////////////////////////////////////// + template inline double -pcl::Registration::getFitnessScore ( +Registration::getFitnessScore ( const std::vector &distances_a, const std::vector &distances_b) { @@ -112,11 +115,10 @@ pcl::Registration::getFitnessScore ( return (static_cast ((map_a - map_b).sum ()) / static_cast (nr_elem)); } -////////////////////////////////////////////////////////////////////////////////////////////// + template inline double -pcl::Registration::getFitnessScore (double max_range) +Registration::getFitnessScore (double max_range) { - double fitness_score = 0.0; // Transform the input dataset using the final transformation @@ -132,7 +134,7 @@ pcl::Registration::getFitnessScore (double max { // Find its nearest neighbor in the target tree_->nearestKSearch (input_transformed.points[i], 1, nn_indices, nn_dists); - + // Deal with occlusions (incomplete targets) if (nn_dists[0] <= max_range) { @@ -148,18 +150,18 @@ pcl::Registration::getFitnessScore (double max } -////////////////////////////////////////////////////////////////////////////////////////////// + template inline void -pcl::Registration::align (PointCloudSource &output) +Registration::align (PointCloudSource &output) { align (output, Matrix4::Identity ()); } -////////////////////////////////////////////////////////////////////////////////////////////// + template inline void -pcl::Registration::align (PointCloudSource &output, const Matrix4& guess) +Registration::align (PointCloudSource &output, const Matrix4& guess) { - if (!initCompute ()) + if (!initCompute ()) return; // Resize the output dataset @@ -202,3 +204,5 @@ pcl::Registration::align (PointCloudSource &ou deinitCompute (); } +} // namespace pcl + diff --git a/registration/include/pcl/registration/impl/sample_consensus_prerejective.hpp b/registration/include/pcl/registration/impl/sample_consensus_prerejective.hpp index 2fa4dd60..aec01f8b 100644 --- a/registration/include/pcl/registration/impl/sample_consensus_prerejective.hpp +++ b/registration/include/pcl/registration/impl/sample_consensus_prerejective.hpp @@ -41,9 +41,12 @@ #ifndef PCL_REGISTRATION_SAMPLE_CONSENSUS_PREREJECTIVE_HPP_ #define PCL_REGISTRATION_SAMPLE_CONSENSUS_PREREJECTIVE_HPP_ -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::SampleConsensusPrerejective::setSourceFeatures (const FeatureCloudConstPtr &features) + +namespace pcl +{ + +template void +SampleConsensusPrerejective::setSourceFeatures (const FeatureCloudConstPtr &features) { if (features == nullptr || features->empty ()) { @@ -53,9 +56,9 @@ pcl::SampleConsensusPrerejective::setSourceF input_features_ = features; } -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::SampleConsensusPrerejective::setTargetFeatures (const FeatureCloudConstPtr &features) + +template void +SampleConsensusPrerejective::setTargetFeatures (const FeatureCloudConstPtr &features) { if (features == nullptr || features->empty ()) { @@ -66,9 +69,9 @@ pcl::SampleConsensusPrerejective::setTargetF feature_tree_->setInputCloud (target_features_); } -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::SampleConsensusPrerejective::selectSamples ( + +template void +SampleConsensusPrerejective::selectSamples ( const PointCloudSource &cloud, int nr_samples, std::vector &sample_indices) { if (nr_samples > static_cast (cloud.points.size ())) @@ -78,7 +81,7 @@ pcl::SampleConsensusPrerejective::selectSamp nr_samples, cloud.points.size ()); return; } - + sample_indices.resize (nr_samples); int temp_sample; @@ -87,7 +90,7 @@ pcl::SampleConsensusPrerejective::selectSamp { // Select a random number sample_indices[i] = getRandomIndex (static_cast (cloud.points.size ()) - i); - + // Run trough list of numbers, starting at the lowest, to avoid duplicates for (int j = 0; j < i; j++) { @@ -102,7 +105,7 @@ pcl::SampleConsensusPrerejective::selectSamp temp_sample = sample_indices[i]; for (int k = i; k > j; k--) sample_indices[k] = sample_indices[k - 1]; - + sample_indices[j] = temp_sample; break; } @@ -110,9 +113,9 @@ pcl::SampleConsensusPrerejective::selectSamp } } -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::SampleConsensusPrerejective::findSimilarFeatures ( +SampleConsensusPrerejective::findSimilarFeatures ( const std::vector &sample_indices, std::vector >& similar_features, std::vector &corresponding_indices) @@ -120,13 +123,13 @@ pcl::SampleConsensusPrerejective::findSimila // Allocate results corresponding_indices.resize (sample_indices.size ()); std::vector nn_distances (k_correspondences_); - + // Loop over the sampled features for (std::size_t i = 0; i < sample_indices.size (); ++i) { // Current feature index const int idx = sample_indices[i]; - + // Find the k nearest feature neighbors to the sampled input feature if they are not in the cache already if (similar_features[idx].empty ()) feature_tree_->nearestKSearch (*input_features_, idx, k_correspondences_, similar_features[idx], nn_distances); @@ -139,9 +142,9 @@ pcl::SampleConsensusPrerejective::findSimila } } -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::SampleConsensusPrerejective::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess) + +template void +SampleConsensusPrerejective::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess) { // Some sanity checks first if (!input_features_) @@ -180,7 +183,7 @@ pcl::SampleConsensusPrerejective::computeTra inlier_fraction_); return; } - + const float similarity_threshold = correspondence_rejector_poly_->getSimilarityThreshold (); if (similarity_threshold < 0.0f || similarity_threshold >= 1.0f) { @@ -189,7 +192,7 @@ pcl::SampleConsensusPrerejective::computeTra similarity_threshold); return; } - + if (k_correspondences_ <= 0) { PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ()); @@ -197,30 +200,30 @@ pcl::SampleConsensusPrerejective::computeTra k_correspondences_); return; } - + // Initialize prerejector (similarity threshold already set to default value in constructor) correspondence_rejector_poly_->setInputSource (input_); correspondence_rejector_poly_->setInputTarget (target_); correspondence_rejector_poly_->setCardinality (nr_samples_); int num_rejections = 0; // For debugging - + // Initialize results final_transformation_ = guess; inliers_.clear (); float lowest_error = std::numeric_limits::max (); converged_ = false; - + // Temporaries std::vector inliers; float inlier_fraction; float error; - + // If guess is not the Identity matrix we check it if (!guess.isApprox (Eigen::Matrix4f::Identity (), 0.01f)) { getFitness (inliers, error); inlier_fraction = static_cast (inliers.size ()) / static_cast (input_->size ()); - + if (inlier_fraction >= inlier_fraction_ && error < lowest_error) { inliers_ = inliers; @@ -228,23 +231,23 @@ pcl::SampleConsensusPrerejective::computeTra converged_ = true; } } - + // Feature correspondence cache std::vector > similar_features (input_->size ()); - + // Start for (int i = 0; i < max_iterations_; ++i) { // Temporary containers std::vector sample_indices; std::vector corresponding_indices; - + // Draw nr_samples_ random samples selectSamples (*input_, nr_samples_, sample_indices); - + // Find corresponding features in the target cloud findSimilarFeatures (sample_indices, similar_features, corresponding_indices); - + // Apply prerejection if (!correspondence_rejector_poly_->thresholdPolygon (sample_indices, corresponding_indices)) { @@ -254,16 +257,16 @@ pcl::SampleConsensusPrerejective::computeTra // Estimate the transform from the correspondences, write to transformation_ transformation_estimation_->estimateRigidTransformation (*input_, sample_indices, *target_, corresponding_indices, transformation_); - + // Take a backup of previous result const Matrix4 final_transformation_prev = final_transformation_; - + // Set final result to current transformation final_transformation_ = transformation_; - + // Transform the input and compute the error (uses input_ and final_transformation_) getFitness (inliers, error); - + // Restore previous result final_transformation_ = final_transformation_prev; @@ -283,21 +286,21 @@ pcl::SampleConsensusPrerejective::computeTra // Apply the final transformation if (converged_) transformPointCloud (*input_, output, final_transformation_); - + // Debug output PCL_DEBUG("[pcl::%s::computeTransformation] Rejected %i out of %i generated pose hypotheses.\n", getClassName ().c_str (), num_rejections, max_iterations_); } -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::SampleConsensusPrerejective::getFitness (std::vector& inliers, float& fitness_score) + +template void +SampleConsensusPrerejective::getFitness (std::vector& inliers, float& fitness_score) { // Initialize variables inliers.clear (); inliers.reserve (input_->size ()); fitness_score = 0.0f; - + // Use squared distance for comparison with NN search results const float max_range = corr_dist_threshold_ * corr_dist_threshold_; @@ -305,7 +308,7 @@ pcl::SampleConsensusPrerejective::getFitness PointCloudSource input_transformed; input_transformed.resize (input_->size ()); transformPointCloud (*input_, input_transformed, final_transformation_); - + // For each point in the source dataset for (std::size_t i = 0; i < input_transformed.points.size (); ++i) { @@ -313,13 +316,13 @@ pcl::SampleConsensusPrerejective::getFitness std::vector nn_indices (1); std::vector nn_dists (1); tree_->nearestKSearch (input_transformed.points[i], 1, nn_indices, nn_dists); - + // Check if point is an inlier if (nn_dists[0] < max_range) { // Update inliers inliers.push_back (static_cast (i)); - + // Update fitness score fitness_score += nn_dists[0]; } @@ -332,5 +335,7 @@ pcl::SampleConsensusPrerejective::getFitness fitness_score = std::numeric_limits::max (); } +} // namespace pcl + #endif diff --git a/registration/include/pcl/registration/impl/transformation_estimation_2D.hpp b/registration/include/pcl/registration/impl/transformation_estimation_2D.hpp index 36b9d678..162aad63 100644 --- a/registration/include/pcl/registration/impl/transformation_estimation_2D.hpp +++ b/registration/include/pcl/registration/impl/transformation_estimation_2D.hpp @@ -34,12 +34,19 @@ * POSSIBILITY OF SUCH DAMAGE. * */ + #ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_2D_HPP_ #define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_2D_HPP_ -/////////////////////////////////////////////////////////////////////////////////////////// + +namespace pcl +{ + +namespace registration +{ + template inline void -pcl::registration::TransformationEstimation2D::estimateRigidTransformation ( +TransformationEstimation2D::estimateRigidTransformation ( const pcl::PointCloud &cloud_src, const pcl::PointCloud &cloud_tgt, Matrix4 &transformation_matrix) const @@ -56,9 +63,9 @@ pcl::registration::TransformationEstimation2D: estimateRigidTransformation (source_it, target_it, transformation_matrix); } -/////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::registration::TransformationEstimation2D::estimateRigidTransformation ( +TransformationEstimation2D::estimateRigidTransformation ( const pcl::PointCloud &cloud_src, const std::vector &indices_src, const pcl::PointCloud &cloud_tgt, @@ -76,9 +83,8 @@ pcl::registration::TransformationEstimation2D: } -/////////////////////////////////////////////////////////////////////////////////////////// template inline void -pcl::registration::TransformationEstimation2D::estimateRigidTransformation ( +TransformationEstimation2D::estimateRigidTransformation ( const pcl::PointCloud &cloud_src, const std::vector &indices_src, const pcl::PointCloud &cloud_tgt, @@ -96,9 +102,9 @@ pcl::registration::TransformationEstimation2D: estimateRigidTransformation (source_it, target_it, transformation_matrix); } -/////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::registration::TransformationEstimation2D::estimateRigidTransformation ( +TransformationEstimation2D::estimateRigidTransformation ( const pcl::PointCloud &cloud_src, const pcl::PointCloud &cloud_tgt, const pcl::Correspondences &correspondences, @@ -109,9 +115,9 @@ pcl::registration::TransformationEstimation2D: estimateRigidTransformation (source_it, target_it, transformation_matrix); } -/////////////////////////////////////////////////////////////////////////////////////////// + template inline void -pcl::registration::TransformationEstimation2D::estimateRigidTransformation ( +TransformationEstimation2D::estimateRigidTransformation ( ConstCloudIterator& source_it, ConstCloudIterator& target_it, Matrix4 &transformation_matrix) const @@ -135,9 +141,9 @@ pcl::registration::TransformationEstimation2D: getTransformationFromCorrelation (cloud_src_demean, centroid_src, cloud_tgt_demean, centroid_tgt, transformation_matrix); } -/////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::registration::TransformationEstimation2D::getTransformationFromCorrelation ( +TransformationEstimation2D::getTransformationFromCorrelation ( const Eigen::Matrix &cloud_src_demean, const Eigen::Matrix ¢roid_src, const Eigen::Matrix &cloud_tgt_demean, @@ -148,9 +154,9 @@ pcl::registration::TransformationEstimation2D: // Assemble the correlation matrix H = source * target' Eigen::Matrix H = (cloud_src_demean * cloud_tgt_demean.transpose ()).topLeftCorner (3, 3); - + float angle = std::atan2 ((H (0, 1) - H (1, 0)), (H(0, 0) + H (1, 1))); - + Eigen::Matrix R (Eigen::Matrix::Identity ()); R (0, 0) = R (1, 1) = std::cos (angle); R (0, 1) = -std::sin (angle); @@ -162,4 +168,8 @@ pcl::registration::TransformationEstimation2D: transformation_matrix.block (0, 3, 3, 1).matrix () = centroid_tgt.head (3) - Rc; } +} // namespace registration +} // namespace pcl + #endif // PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_2D_HPP_ + diff --git a/registration/include/pcl/registration/impl/transformation_estimation_dq.hpp b/registration/include/pcl/registration/impl/transformation_estimation_dq.hpp index 102ca7e5..b5a48611 100644 --- a/registration/include/pcl/registration/impl/transformation_estimation_dq.hpp +++ b/registration/include/pcl/registration/impl/transformation_estimation_dq.hpp @@ -36,14 +36,21 @@ * * */ + #ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_DQ_HPP_ #define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_DQ_HPP_ #include -/////////////////////////////////////////////////////////////////////////////////////////// + +namespace pcl +{ + +namespace registration +{ + template inline void -pcl::registration::TransformationEstimationDQ::estimateRigidTransformation ( +TransformationEstimationDQ::estimateRigidTransformation ( const pcl::PointCloud &cloud_src, const pcl::PointCloud &cloud_tgt, Matrix4 &transformation_matrix) const @@ -60,9 +67,9 @@ pcl::registration::TransformationEstimationDQ: estimateRigidTransformation (source_it, target_it, transformation_matrix); } -/////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::registration::TransformationEstimationDQ::estimateRigidTransformation ( +TransformationEstimationDQ::estimateRigidTransformation ( const pcl::PointCloud &cloud_src, const std::vector &indices_src, const pcl::PointCloud &cloud_tgt, @@ -79,9 +86,9 @@ pcl::registration::TransformationEstimationDQ: estimateRigidTransformation (source_it, target_it, transformation_matrix); } -/////////////////////////////////////////////////////////////////////////////////////////// + template inline void -pcl::registration::TransformationEstimationDQ::estimateRigidTransformation ( +TransformationEstimationDQ::estimateRigidTransformation ( const pcl::PointCloud &cloud_src, const std::vector &indices_src, const pcl::PointCloud &cloud_tgt, @@ -99,9 +106,9 @@ pcl::registration::TransformationEstimationDQ: estimateRigidTransformation (source_it, target_it, transformation_matrix); } -/////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::registration::TransformationEstimationDQ::estimateRigidTransformation ( +TransformationEstimationDQ::estimateRigidTransformation ( const pcl::PointCloud &cloud_src, const pcl::PointCloud &cloud_tgt, const pcl::Correspondences &correspondences, @@ -112,9 +119,9 @@ pcl::registration::TransformationEstimationDQ: estimateRigidTransformation (source_it, target_it, transformation_matrix); } -/////////////////////////////////////////////////////////////////////////////////////////// + template inline void -pcl::registration::TransformationEstimationDQ::estimateRigidTransformation ( +TransformationEstimationDQ::estimateRigidTransformation ( ConstCloudIterator& source_it, ConstCloudIterator& target_it, Matrix4 &transformation_matrix) const @@ -203,4 +210,8 @@ pcl::registration::TransformationEstimationDQ: transformation_matrix(2,3) = -t.z(); } +} // namespace registration +} // namespace pcl + #endif /* PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_DQ_HPP_ */ + diff --git a/registration/include/pcl/registration/impl/transformation_estimation_dual_quaternion.hpp b/registration/include/pcl/registration/impl/transformation_estimation_dual_quaternion.hpp index 27f1347d..9bee6a25 100644 --- a/registration/include/pcl/registration/impl/transformation_estimation_dual_quaternion.hpp +++ b/registration/include/pcl/registration/impl/transformation_estimation_dual_quaternion.hpp @@ -36,14 +36,21 @@ * * */ + #ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_DQ_HPP_ #define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_DQ_HPP_ #include -/////////////////////////////////////////////////////////////////////////////////////////// + +namespace pcl +{ + +namespace registration +{ + template inline void -pcl::registration::TransformationEstimationDualQuaternion::estimateRigidTransformation ( +TransformationEstimationDualQuaternion::estimateRigidTransformation ( const pcl::PointCloud &cloud_src, const pcl::PointCloud &cloud_tgt, Matrix4 &transformation_matrix) const @@ -60,9 +67,9 @@ pcl::registration::TransformationEstimationDualQuaternion void -pcl::registration::TransformationEstimationDualQuaternion::estimateRigidTransformation ( +TransformationEstimationDualQuaternion::estimateRigidTransformation ( const pcl::PointCloud &cloud_src, const std::vector &indices_src, const pcl::PointCloud &cloud_tgt, @@ -79,9 +86,9 @@ pcl::registration::TransformationEstimationDualQuaternion inline void -pcl::registration::TransformationEstimationDualQuaternion::estimateRigidTransformation ( +TransformationEstimationDualQuaternion::estimateRigidTransformation ( const pcl::PointCloud &cloud_src, const std::vector &indices_src, const pcl::PointCloud &cloud_tgt, @@ -99,9 +106,9 @@ pcl::registration::TransformationEstimationDualQuaternion void -pcl::registration::TransformationEstimationDualQuaternion::estimateRigidTransformation ( +TransformationEstimationDualQuaternion::estimateRigidTransformation ( const pcl::PointCloud &cloud_src, const pcl::PointCloud &cloud_tgt, const pcl::Correspondences &correspondences, @@ -112,9 +119,9 @@ pcl::registration::TransformationEstimationDualQuaternion inline void -pcl::registration::TransformationEstimationDualQuaternion::estimateRigidTransformation ( +TransformationEstimationDualQuaternion::estimateRigidTransformation ( ConstCloudIterator& source_it, ConstCloudIterator& target_it, Matrix4 &transformation_matrix) const @@ -204,4 +211,8 @@ pcl::registration::TransformationEstimationDualQuaternion -////////////////////////////////////////////////////////////////////////////////////////////// + +namespace pcl +{ + +namespace registration +{ + template inline void -pcl::registration::TransformationEstimationPointToPlaneLLS:: +TransformationEstimationPointToPlaneLLS:: estimateRigidTransformation (const pcl::PointCloud &cloud_src, const pcl::PointCloud &cloud_tgt, Matrix4 &transformation_matrix) const @@ -57,12 +65,12 @@ estimateRigidTransformation (const pcl::PointCloud &cloud_src, ConstCloudIterator source_it (cloud_src); ConstCloudIterator target_it (cloud_tgt); - estimateRigidTransformation (source_it, target_it, transformation_matrix); + estimateRigidTransformation (source_it, target_it, transformation_matrix); } -////////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::registration::TransformationEstimationPointToPlaneLLS:: +TransformationEstimationPointToPlaneLLS:: estimateRigidTransformation (const pcl::PointCloud &cloud_src, const std::vector &indices_src, const pcl::PointCloud &cloud_tgt, @@ -77,13 +85,12 @@ estimateRigidTransformation (const pcl::PointCloud &cloud_src, ConstCloudIterator source_it (cloud_src, indices_src); ConstCloudIterator target_it (cloud_tgt); - estimateRigidTransformation (source_it, target_it, transformation_matrix); + estimateRigidTransformation (source_it, target_it, transformation_matrix); } -////////////////////////////////////////////////////////////////////////////////////////////// template inline void -pcl::registration::TransformationEstimationPointToPlaneLLS:: +TransformationEstimationPointToPlaneLLS:: estimateRigidTransformation (const pcl::PointCloud &cloud_src, const std::vector &indices_src, const pcl::PointCloud &cloud_tgt, @@ -99,12 +106,12 @@ estimateRigidTransformation (const pcl::PointCloud &cloud_src, ConstCloudIterator source_it (cloud_src, indices_src); ConstCloudIterator target_it (cloud_tgt, indices_tgt); - estimateRigidTransformation (source_it, target_it, transformation_matrix); + estimateRigidTransformation (source_it, target_it, transformation_matrix); } -////////////////////////////////////////////////////////////////////////////////////////////// + template inline void -pcl::registration::TransformationEstimationPointToPlaneLLS:: +TransformationEstimationPointToPlaneLLS:: estimateRigidTransformation (const pcl::PointCloud &cloud_src, const pcl::PointCloud &cloud_tgt, const pcl::Correspondences &correspondences, @@ -115,14 +122,14 @@ estimateRigidTransformation (const pcl::PointCloud &cloud_src, estimateRigidTransformation (source_it, target_it, transformation_matrix); } -////////////////////////////////////////////////////////////////////////////////////////////// + template inline void -pcl::registration::TransformationEstimationPointToPlaneLLS:: +TransformationEstimationPointToPlaneLLS:: constructTransformationMatrix (const double & alpha, const double & beta, const double & gamma, const double & tx, const double & ty, const double & tz, Matrix4 &transformation_matrix) const { - // Construct the transformation matrix from rotation and translation + // Construct the transformation matrix from rotation and translation transformation_matrix = Eigen::Matrix::Zero (); transformation_matrix (0, 0) = static_cast ( std::cos (gamma) * std::cos (beta)); transformation_matrix (0, 1) = static_cast (-sin (gamma) * std::cos (alpha) + std::cos (gamma) * sin (beta) * sin (alpha)); @@ -140,9 +147,9 @@ constructTransformationMatrix (const double & alpha, const double & beta, const transformation_matrix (3, 3) = static_cast (1); } -////////////////////////////////////////////////////////////////////////////////////////////// + template inline void -pcl::registration::TransformationEstimationPointToPlaneLLS:: +TransformationEstimationPointToPlaneLLS:: estimateRigidTransformation (ConstCloudIterator& source_it, ConstCloudIterator& target_it, Matrix4 &transformation_matrix) const { using Vector6d = Eigen::Matrix; @@ -167,7 +174,7 @@ estimateRigidTransformation (ConstCloudIterator& source_it, ConstCl !std::isfinite (target_it->normal_z)) { ++target_it; - ++source_it; + ++source_it; continue; } @@ -182,16 +189,16 @@ estimateRigidTransformation (ConstCloudIterator& source_it, ConstCl const float & nz = target_it->normal[2]; double a = nz*sy - ny*sz; - double b = nx*sz - nz*sx; + double b = nx*sz - nz*sx; double c = ny*sx - nx*sy; - + // 0 1 2 3 4 5 // 6 7 8 9 10 11 // 12 13 14 15 16 17 // 18 19 20 21 22 23 // 24 25 26 27 28 29 // 30 31 32 33 34 35 - + ATA.coeffRef (0) += a * a; ATA.coeffRef (1) += a * b; ATA.coeffRef (2) += a * c; @@ -223,8 +230,9 @@ estimateRigidTransformation (ConstCloudIterator& source_it, ConstCl ATb.coeffRef (5) += nz * d; ++target_it; - ++source_it; + ++source_it; } + ATA.coeffRef (6) = ATA.coeff (1); ATA.coeffRef (12) = ATA.coeff (2); ATA.coeffRef (13) = ATA.coeff (8); @@ -243,8 +251,13 @@ estimateRigidTransformation (ConstCloudIterator& source_it, ConstCl // Solve A*x = b Vector6d x = static_cast (ATA.inverse () * ATb); - + // Construct the transformation matrix from x constructTransformationMatrix (x (0), x (1), x (2), x (3), x (4), x (5), transformation_matrix); } + +} // namespace registration +} // namespace pcl + #endif /* PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_HPP_ */ + diff --git a/registration/include/pcl/registration/impl/transformation_estimation_point_to_plane_lls_weighted.hpp b/registration/include/pcl/registration/impl/transformation_estimation_point_to_plane_lls_weighted.hpp index dfbd1610..c377fad4 100644 --- a/registration/include/pcl/registration/impl/transformation_estimation_point_to_plane_lls_weighted.hpp +++ b/registration/include/pcl/registration/impl/transformation_estimation_point_to_plane_lls_weighted.hpp @@ -36,13 +36,21 @@ * $Id$ * */ + #ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_WEIGHTED_HPP_ #define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_WEIGHTED_HPP_ + #include -////////////////////////////////////////////////////////////////////////////////////////////// + +namespace pcl +{ + +namespace registration +{ + template inline void -pcl::registration::TransformationEstimationPointToPlaneLLSWeighted:: +TransformationEstimationPointToPlaneLLSWeighted:: estimateRigidTransformation (const pcl::PointCloud &cloud_src, const pcl::PointCloud &cloud_tgt, Matrix4 &transformation_matrix) const @@ -66,9 +74,9 @@ estimateRigidTransformation (const pcl::PointCloud &cloud_src, estimateRigidTransformation (source_it, target_it, weights_it, transformation_matrix); } -////////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::registration::TransformationEstimationPointToPlaneLLSWeighted:: +TransformationEstimationPointToPlaneLLSWeighted:: estimateRigidTransformation (const pcl::PointCloud &cloud_src, const std::vector &indices_src, const pcl::PointCloud &cloud_tgt, @@ -95,9 +103,8 @@ estimateRigidTransformation (const pcl::PointCloud &cloud_src, } -////////////////////////////////////////////////////////////////////////////////////////////// template inline void -pcl::registration::TransformationEstimationPointToPlaneLLSWeighted:: +TransformationEstimationPointToPlaneLLSWeighted:: estimateRigidTransformation (const pcl::PointCloud &cloud_src, const std::vector &indices_src, const pcl::PointCloud &cloud_tgt, @@ -123,9 +130,9 @@ estimateRigidTransformation (const pcl::PointCloud &cloud_src, estimateRigidTransformation (source_it, target_it, weights_it, transformation_matrix); } -////////////////////////////////////////////////////////////////////////////////////////////// + template inline void -pcl::registration::TransformationEstimationPointToPlaneLLSWeighted:: +TransformationEstimationPointToPlaneLLSWeighted:: estimateRigidTransformation (const pcl::PointCloud &cloud_src, const pcl::PointCloud &cloud_tgt, const pcl::Correspondences &correspondences, @@ -140,14 +147,14 @@ estimateRigidTransformation (const pcl::PointCloud &cloud_src, estimateRigidTransformation (source_it, target_it, weights_it, transformation_matrix); } -////////////////////////////////////////////////////////////////////////////////////////////// + template inline void -pcl::registration::TransformationEstimationPointToPlaneLLSWeighted:: +TransformationEstimationPointToPlaneLLSWeighted:: constructTransformationMatrix (const double & alpha, const double & beta, const double & gamma, const double & tx, const double & ty, const double & tz, Matrix4 &transformation_matrix) const { - // Construct the transformation matrix from rotation and translation + // Construct the transformation matrix from rotation and translation transformation_matrix = Eigen::Matrix::Zero (); transformation_matrix (0, 0) = static_cast ( std::cos (gamma) * std::cos (beta)); transformation_matrix (0, 1) = static_cast (-sin (gamma) * std::cos (alpha) + std::cos (gamma) * sin (beta) * sin (alpha)); @@ -165,9 +172,9 @@ constructTransformationMatrix (const double & alpha, const double & beta, const transformation_matrix (3, 3) = static_cast (1); } -////////////////////////////////////////////////////////////////////////////////////////////// + template inline void -pcl::registration::TransformationEstimationPointToPlaneLLSWeighted:: +TransformationEstimationPointToPlaneLLSWeighted:: estimateRigidTransformation (ConstCloudIterator& source_it, ConstCloudIterator& target_it, typename std::vector::const_iterator& weights_it, @@ -277,4 +284,9 @@ estimateRigidTransformation (ConstCloudIterator& source_it, // Construct the transformation matrix from x constructTransformationMatrix (x (0), x (1), x (2), x (3), x (4), x (5), transformation_matrix); } + +} // namespace registration +} // namespace pcl + #endif /* PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_WEIGHTED_HPP_ */ + diff --git a/registration/include/pcl/registration/impl/transformation_estimation_svd.hpp b/registration/include/pcl/registration/impl/transformation_estimation_svd.hpp index 0717db44..a465aae8 100644 --- a/registration/include/pcl/registration/impl/transformation_estimation_svd.hpp +++ b/registration/include/pcl/registration/impl/transformation_estimation_svd.hpp @@ -37,14 +37,21 @@ * $Id$ * */ + #ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_SVD_HPP_ #define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_SVD_HPP_ #include -/////////////////////////////////////////////////////////////////////////////////////////// + +namespace pcl +{ + +namespace registration +{ + template inline void -pcl::registration::TransformationEstimationSVD::estimateRigidTransformation ( +TransformationEstimationSVD::estimateRigidTransformation ( const pcl::PointCloud &cloud_src, const pcl::PointCloud &cloud_tgt, Matrix4 &transformation_matrix) const @@ -61,9 +68,9 @@ pcl::registration::TransformationEstimationSVD estimateRigidTransformation (source_it, target_it, transformation_matrix); } -/////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::registration::TransformationEstimationSVD::estimateRigidTransformation ( +TransformationEstimationSVD::estimateRigidTransformation ( const pcl::PointCloud &cloud_src, const std::vector &indices_src, const pcl::PointCloud &cloud_tgt, @@ -80,9 +87,9 @@ pcl::registration::TransformationEstimationSVD estimateRigidTransformation (source_it, target_it, transformation_matrix); } -/////////////////////////////////////////////////////////////////////////////////////////// + template inline void -pcl::registration::TransformationEstimationSVD::estimateRigidTransformation ( +TransformationEstimationSVD::estimateRigidTransformation ( const pcl::PointCloud &cloud_src, const std::vector &indices_src, const pcl::PointCloud &cloud_tgt, @@ -100,9 +107,9 @@ pcl::registration::TransformationEstimationSVD estimateRigidTransformation (source_it, target_it, transformation_matrix); } -/////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::registration::TransformationEstimationSVD::estimateRigidTransformation ( +TransformationEstimationSVD::estimateRigidTransformation ( const pcl::PointCloud &cloud_src, const pcl::PointCloud &cloud_tgt, const pcl::Correspondences &correspondences, @@ -113,9 +120,9 @@ pcl::registration::TransformationEstimationSVD estimateRigidTransformation (source_it, target_it, transformation_matrix); } -/////////////////////////////////////////////////////////////////////////////////////////// + template inline void -pcl::registration::TransformationEstimationSVD::estimateRigidTransformation ( +TransformationEstimationSVD::estimateRigidTransformation ( ConstCloudIterator& source_it, ConstCloudIterator& target_it, Matrix4 &transformation_matrix) const @@ -142,7 +149,7 @@ pcl::registration::TransformationEstimationSVD cloud_tgt (2, i) = target_it->z; ++target_it; } - + // Call Umeyama directly from Eigen (PCL patched version until Eigen is released) transformation_matrix = pcl::umeyama (cloud_src, cloud_tgt, false); } @@ -167,9 +174,9 @@ pcl::registration::TransformationEstimationSVD } } -/////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::registration::TransformationEstimationSVD::getTransformationFromCorrelation ( +TransformationEstimationSVD::getTransformationFromCorrelation ( const Eigen::Matrix &cloud_src_demean, const Eigen::Matrix ¢roid_src, const Eigen::Matrix &cloud_tgt_demean, @@ -201,6 +208,10 @@ pcl::registration::TransformationEstimationSVD transformation_matrix.block (0, 3, 3, 1) = centroid_tgt.head (3) - Rc; } +} // namespace registration +} // namespace pcl + //#define PCL_INSTANTIATE_TransformationEstimationSVD(T,U) template class PCL_EXPORTS pcl::registration::TransformationEstimationSVD; #endif /* PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_SVD_HPP_ */ + diff --git a/registration/include/pcl/registration/impl/transformation_estimation_svd_scale.hpp b/registration/include/pcl/registration/impl/transformation_estimation_svd_scale.hpp index 553342ef..e6b9327d 100644 --- a/registration/include/pcl/registration/impl/transformation_estimation_svd_scale.hpp +++ b/registration/include/pcl/registration/impl/transformation_estimation_svd_scale.hpp @@ -36,12 +36,19 @@ * $Id$ * */ + #ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_SVD_SCALE_HPP_ #define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_SVD_SCALE_HPP_ -////////////////////////////////////////////////////////////////////////////////////////////// + +namespace pcl +{ + +namespace registration +{ + template void -pcl::registration::TransformationEstimationSVDScale::getTransformationFromCorrelation ( +TransformationEstimationSVDScale::getTransformationFromCorrelation ( const Eigen::Matrix &cloud_src_demean, const Eigen::Matrix ¢roid_src, const Eigen::Matrix &cloud_tgt_demean, @@ -68,7 +75,7 @@ pcl::registration::TransformationEstimationSVDScale R = v * u.transpose (); // rotated cloud - Eigen::Matrix R4; + Eigen::Matrix R4; R4.block (0, 0, 3, 3) = R; R4 (0, 3) = 0; R4 (1, 3) = 0; @@ -76,32 +83,28 @@ pcl::registration::TransformationEstimationSVDScale src_ = R4 * cloud_src_demean; - - float scale1, scale2; - double sum_ss = 0.0f, sum_tt = 0.0f, sum_tt_ = 0.0f; + + double sum_ss = 0.0f, sum_tt = 0.0f; for (unsigned corrIdx = 0; corrIdx < cloud_src_demean.cols (); ++corrIdx) { sum_ss += cloud_src_demean (0, corrIdx) * cloud_src_demean (0, corrIdx); sum_ss += cloud_src_demean (1, corrIdx) * cloud_src_demean (1, corrIdx); sum_ss += cloud_src_demean (2, corrIdx) * cloud_src_demean (2, corrIdx); - - sum_tt += cloud_tgt_demean (0, corrIdx) * cloud_tgt_demean (0, corrIdx); - sum_tt += cloud_tgt_demean (1, corrIdx) * cloud_tgt_demean (1, corrIdx); - sum_tt += cloud_tgt_demean (2, corrIdx) * cloud_tgt_demean (2, corrIdx); - - sum_tt_ += cloud_tgt_demean (0, corrIdx) * src_ (0, corrIdx); - sum_tt_ += cloud_tgt_demean (1, corrIdx) * src_ (1, corrIdx); - sum_tt_ += cloud_tgt_demean (2, corrIdx) * src_ (2, corrIdx); + + sum_tt += cloud_tgt_demean (0, corrIdx) * src_ (0, corrIdx); + sum_tt += cloud_tgt_demean (1, corrIdx) * src_ (1, corrIdx); + sum_tt += cloud_tgt_demean (2, corrIdx) * src_ (2, corrIdx); } - - scale1 = sqrt (sum_tt / sum_ss); - scale2 = sum_tt_ / sum_ss; - float scale = scale2; + + float scale = sum_tt / sum_ss; transformation_matrix.topLeftCorner (3, 3) = scale * R; const Eigen::Matrix Rc (scale * R * centroid_src.head (3)); transformation_matrix.block (0, 3, 3, 1) = centroid_tgt. head (3) - Rc; } +} // namespace registration +} // namespace pcl + //#define PCL_INSTANTIATE_TransformationEstimationSVD(T,U) template class PCL_EXPORTS pcl::registration::TransformationEstimationSVD; #endif /* PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_SVD_SCALE_HPP_ */ diff --git a/registration/include/pcl/registration/impl/transformation_estimation_symmetric_point_to_plane_lls.hpp b/registration/include/pcl/registration/impl/transformation_estimation_symmetric_point_to_plane_lls.hpp index 1cf74db0..a2f3e997 100644 --- a/registration/include/pcl/registration/impl/transformation_estimation_symmetric_point_to_plane_lls.hpp +++ b/registration/include/pcl/registration/impl/transformation_estimation_symmetric_point_to_plane_lls.hpp @@ -39,9 +39,15 @@ #include -////////////////////////////////////////////////////////////////////////////////////////////// + +namespace pcl +{ + +namespace registration +{ + template inline void -pcl::registration::TransformationEstimationSymmetricPointToPlaneLLS:: +TransformationEstimationSymmetricPointToPlaneLLS:: estimateRigidTransformation (const pcl::PointCloud &cloud_src, const pcl::PointCloud &cloud_tgt, Matrix4 &transformation_matrix) const @@ -55,12 +61,12 @@ estimateRigidTransformation (const pcl::PointCloud &cloud_src, ConstCloudIterator source_it (cloud_src); ConstCloudIterator target_it (cloud_tgt); - estimateRigidTransformation (source_it, target_it, transformation_matrix); + estimateRigidTransformation (source_it, target_it, transformation_matrix); } -////////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::registration::TransformationEstimationSymmetricPointToPlaneLLS:: +TransformationEstimationSymmetricPointToPlaneLLS:: estimateRigidTransformation (const pcl::PointCloud &cloud_src, const std::vector &indices_src, const pcl::PointCloud &cloud_tgt, @@ -75,13 +81,12 @@ estimateRigidTransformation (const pcl::PointCloud &cloud_src, ConstCloudIterator source_it (cloud_src, indices_src); ConstCloudIterator target_it (cloud_tgt); - estimateRigidTransformation (source_it, target_it, transformation_matrix); + estimateRigidTransformation (source_it, target_it, transformation_matrix); } -////////////////////////////////////////////////////////////////////////////////////////////// template inline void -pcl::registration::TransformationEstimationSymmetricPointToPlaneLLS:: +TransformationEstimationSymmetricPointToPlaneLLS:: estimateRigidTransformation (const pcl::PointCloud &cloud_src, const std::vector &indices_src, const pcl::PointCloud &cloud_tgt, @@ -97,12 +102,12 @@ estimateRigidTransformation (const pcl::PointCloud &cloud_src, ConstCloudIterator source_it (cloud_src, indices_src); ConstCloudIterator target_it (cloud_tgt, indices_tgt); - estimateRigidTransformation (source_it, target_it, transformation_matrix); + estimateRigidTransformation (source_it, target_it, transformation_matrix); } -////////////////////////////////////////////////////////////////////////////////////////////// + template inline void -pcl::registration::TransformationEstimationSymmetricPointToPlaneLLS:: +TransformationEstimationSymmetricPointToPlaneLLS:: estimateRigidTransformation (const pcl::PointCloud &cloud_src, const pcl::PointCloud &cloud_tgt, const pcl::Correspondences &correspondences, @@ -113,13 +118,13 @@ estimateRigidTransformation (const pcl::PointCloud &cloud_src, estimateRigidTransformation (source_it, target_it, transformation_matrix); } -////////////////////////////////////////////////////////////////////////////////////////////// + template inline void -pcl::registration::TransformationEstimationSymmetricPointToPlaneLLS:: +TransformationEstimationSymmetricPointToPlaneLLS:: constructTransformationMatrix (const Vector6 ¶meters, Matrix4 &transformation_matrix) const { - // Construct the transformation matrix from rotation and translation + // Construct the transformation matrix from rotation and translation const Eigen::AngleAxis rotation_z (parameters (2), Eigen::Matrix::UnitZ ()); const Eigen::AngleAxis rotation_y (parameters (1), Eigen::Matrix::UnitY ()); const Eigen::AngleAxis rotation_x (parameters (0), Eigen::Matrix::UnitX ()); @@ -130,9 +135,9 @@ constructTransformationMatrix (const Vector6 ¶meters, transformation_matrix = transform.matrix (); } -////////////////////////////////////////////////////////////////////////////////////////////// + template inline void -pcl::registration::TransformationEstimationSymmetricPointToPlaneLLS:: +TransformationEstimationSymmetricPointToPlaneLLS:: estimateRigidTransformation (ConstCloudIterator& source_it, ConstCloudIterator& target_it, Matrix4 &transformation_matrix) const { using Matrix6 = Eigen::Matrix; @@ -176,29 +181,33 @@ estimateRigidTransformation (ConstCloudIterator& source_it, ConstCl Vector6 v; v << (p + q).cross (n), n; M.rankUpdate (v); - + ATb += v * (q - p).dot (n); } // Solve A*x = b const Vector6 x = M.ldlt ().solve (ATb); - + // Construct the transformation matrix from x constructTransformationMatrix (x, transformation_matrix); } -////////////////////////////////////////////////////////////////////////////////////////////// + template inline void -pcl::registration::TransformationEstimationSymmetricPointToPlaneLLS:: +TransformationEstimationSymmetricPointToPlaneLLS:: setEnforceSameDirectionNormals (bool enforce_same_direction_normals) { enforce_same_direction_normals_ = enforce_same_direction_normals; } -////////////////////////////////////////////////////////////////////////////////////////////// -template inline bool -pcl::registration::TransformationEstimationSymmetricPointToPlaneLLS:: + +template inline bool +TransformationEstimationSymmetricPointToPlaneLLS:: getEnforceSameDirectionNormals () { return enforce_same_direction_normals_; } + +} // namespace registration +} // namespace pcl + diff --git a/registration/include/pcl/registration/impl/transformation_validation_euclidean.hpp b/registration/include/pcl/registration/impl/transformation_validation_euclidean.hpp index f5ec7fda..40fbb621 100644 --- a/registration/include/pcl/registration/impl/transformation_validation_euclidean.hpp +++ b/registration/include/pcl/registration/impl/transformation_validation_euclidean.hpp @@ -37,12 +37,19 @@ * $Id$ * */ + #ifndef PCL_REGISTRATION_TRANSFORMATION_VALIDATION_EUCLIDEAN_IMPL_H_ #define PCL_REGISTRATION_TRANSFORMATION_VALIDATION_EUCLIDEAN_IMPL_H_ -///////////////////////////////////////////////////////////////////////////////////////////////////////// + +namespace pcl +{ + +namespace registration +{ + template double -pcl::registration::TransformationValidationEuclidean::validateTransformation ( +TransformationValidationEuclidean::validateTransformation ( const PointCloudSourceConstPtr &cloud_src, const PointCloudTargetConstPtr &cloud_tgt, const Matrix4 &transformation_matrix) const @@ -78,7 +85,7 @@ pcl::registration::TransformationValidationEuclideannearestKSearch (input_transformed.points[i], 1, nn_indices, nn_dists); - + // Deal with occlusions (incomplete targets) if (nn_dists[0] > max_range_) continue; @@ -93,5 +100,8 @@ pcl::registration::TransformationValidationEuclidean::max ()); } +} // namespace registration +} // namespace pcl + #endif // PCL_REGISTRATION_TRANSFORMATION_VALIDATION_EUCLIDEAN_IMPL_H_ diff --git a/registration/include/pcl/registration/lum.h b/registration/include/pcl/registration/lum.h index 85a49c07..f5ed9726 100644 --- a/registration/include/pcl/registration/lum.h +++ b/registration/include/pcl/registration/lum.h @@ -40,6 +40,7 @@ #pragma once +#include #include #include #include diff --git a/registration/include/pcl/registration/matching_candidate.h b/registration/include/pcl/registration/matching_candidate.h index 87679575..78c3223f 100644 --- a/registration/include/pcl/registration/matching_candidate.h +++ b/registration/include/pcl/registration/matching_candidate.h @@ -37,6 +37,7 @@ #pragma once +#include #include #include #include diff --git a/registration/include/pcl/registration/meta_registration.h b/registration/include/pcl/registration/meta_registration.h index 9cca7f37..3ddb5d72 100644 --- a/registration/include/pcl/registration/meta_registration.h +++ b/registration/include/pcl/registration/meta_registration.h @@ -87,10 +87,10 @@ namespace pcl { virtual ~MetaRegistration () {}; /** \brief Register new point cloud - * \note You have to set a valid registration object with @ref setICP before using this + * \note You have to set a valid registration object with \ref setRegistration before using this * \param[in] cloud point cloud to register * \param[in] delta_estimate estimated transform between last registered cloud and this one - * \return true if ICP converged + * \return true if registration converged */ bool registerCloud (const PointCloudConstPtr& cloud, const Matrix4& delta_estimate = Matrix4::Identity ()); @@ -99,7 +99,7 @@ namespace pcl { inline Matrix4 getAbsoluteTransform () const; - /** \brief Reset MetaICP without resetting registration_ */ + /** \brief Reset MetaRegistration without resetting registration_ */ inline void reset (); diff --git a/registration/include/pcl/registration/ndt.h b/registration/include/pcl/registration/ndt.h index b78aa2c7..36f10f7e 100644 --- a/registration/include/pcl/registration/ndt.h +++ b/registration/include/pcl/registration/ndt.h @@ -40,6 +40,7 @@ #pragma once +#include #include #include #include diff --git a/registration/include/pcl/registration/ndt_2d.h b/registration/include/pcl/registration/ndt_2d.h index e514fe59..8551db4f 100644 --- a/registration/include/pcl/registration/ndt_2d.h +++ b/registration/include/pcl/registration/ndt_2d.h @@ -40,6 +40,7 @@ #pragma once +#include #include #include diff --git a/registration/include/pcl/registration/ppf_registration.h b/registration/include/pcl/registration/ppf_registration.h index fd0aa7d7..0275020c 100644 --- a/registration/include/pcl/registration/ppf_registration.h +++ b/registration/include/pcl/registration/ppf_registration.h @@ -121,15 +121,15 @@ namespace pcl /** \brief Returns the angle discretization step parameter (the step value between each bin of the hash map for the angular values) */ inline float - getAngleDiscretizationStep () { return angle_discretization_step_; } + getAngleDiscretizationStep () const { return angle_discretization_step_; } /** \brief Returns the distance discretization step parameter (the step value between each bin of the hash map for the distance values) */ inline float - getDistanceDiscretizationStep () { return distance_discretization_step_; } + getDistanceDiscretizationStep () const { return distance_discretization_step_; } /** \brief Returns the maximum distance found between any feature pair in the given input feature cloud */ inline float - getModelDiameter () { return max_dist_; } + getModelDiameter () const { return max_dist_; } std::vector > alpha_m_; private: diff --git a/registration/include/pcl/registration/registration.h b/registration/include/pcl/registration/registration.h index cf8ede47..ab77ded7 100644 --- a/registration/include/pcl/registration/registration.h +++ b/registration/include/pcl/registration/registration.h @@ -43,6 +43,7 @@ // PCL includes #include #include +#include #include #include #include @@ -608,6 +609,19 @@ namespace pcl private: /** \brief The point representation used (internal). */ PointRepresentationConstPtr point_representation_; + + /** + * \brief Remove from public API in favor of \ref setInputSource + * + * Still gives the correct result (with a warning) + */ + void setInputCloud (const PointCloudSourceConstPtr &cloud) override + { + PCL_WARN ("[pcl::registration::Registration] setInputCloud is deprecated." + "Please use setInputSource instead.\n"); + setInputSource (cloud); + } + public: PCL_MAKE_ALIGNED_OPERATOR_NEW }; diff --git a/registration/include/pcl/registration/transformation_estimation_lm.h b/registration/include/pcl/registration/transformation_estimation_lm.h index 21c29a85..06778486 100644 --- a/registration/include/pcl/registration/transformation_estimation_lm.h +++ b/registration/include/pcl/registration/transformation_estimation_lm.h @@ -40,6 +40,7 @@ #pragma once +#include #include #include #include diff --git a/registration/include/pcl/registration/transformation_estimation_point_to_plane_weighted.h b/registration/include/pcl/registration/transformation_estimation_point_to_plane_weighted.h index 165812b8..4430702b 100644 --- a/registration/include/pcl/registration/transformation_estimation_point_to_plane_weighted.h +++ b/registration/include/pcl/registration/transformation_estimation_point_to_plane_weighted.h @@ -38,6 +38,7 @@ #pragma once +#include #include #include #include diff --git a/registration/include/pcl/registration/transformation_estimation_symmetric_point_to_plane_lls.h b/registration/include/pcl/registration/transformation_estimation_symmetric_point_to_plane_lls.h index 8db6f187..f70bdc78 100644 --- a/registration/include/pcl/registration/transformation_estimation_symmetric_point_to_plane_lls.h +++ b/registration/include/pcl/registration/transformation_estimation_symmetric_point_to_plane_lls.h @@ -48,7 +48,7 @@ namespace pcl /** \brief @b TransformationEstimationSymmetricPointToPlaneLLS implements a Linear Least Squares (LLS) approximation * for minimizing the symmetric point-to-plane distance between two clouds of corresponding points with normals. * - * For additional details, see + * For additional details, see * "Linear Least-Squares Optimization for Point-to-Plane ICP Surface Registration", Kok-Lim Low, 2004 * "A Symmetric Objective Function for ICP", Szymon Rusinkiewicz, 2019 * @@ -66,7 +66,7 @@ namespace pcl using Matrix4 = typename TransformationEstimation::Matrix4; using Vector6 = Eigen::Matrix; - + TransformationEstimationSymmetricPointToPlaneLLS () : enforce_same_direction_normals_ (true) {}; ~TransformationEstimationSymmetricPointToPlaneLLS () {}; @@ -127,25 +127,25 @@ namespace pcl */ inline void setEnforceSameDirectionNormals (bool enforce_same_direction_normals); - + /** \brief Obtain whether source or target normals are negated on a per-point basis such that they point in the same direction or not */ inline bool getEnforceSameDirectionNormals (); protected: - + /** \brief Estimate a rigid rotation transformation between a source and a target * \param[in] source_it an iterator over the source point cloud dataset * \param[in] target_it an iterator over the target point cloud dataset * \param[out] transformation_matrix the resultant transformation matrix */ - void - estimateRigidTransformation (ConstCloudIterator& source_it, - ConstCloudIterator& target_it, + void + estimateRigidTransformation (ConstCloudIterator& source_it, + ConstCloudIterator& target_it, Matrix4 &transformation_matrix) const; /** \brief Construct a 4 by 4 transformation matrix from the provided rotation and translation. - * \param[in] (alpha, beta, gamma, tx, ty, tz) specifying rotation about the x, y, and z-axis and translation along the the x, y, and z-axis respectively + * \param[in] parameters (alpha, beta, gamma, tx, ty, tz) specifying rotation about the x, y, and z-axis and translation along the the x, y, and z-axis respectively * \param[out] transformation_matrix the resultant transformation matrix */ inline void diff --git a/registration/include/pcl/registration/transformation_validation_euclidean.h b/registration/include/pcl/registration/transformation_validation_euclidean.h index 838f6044..5d2cb4ad 100644 --- a/registration/include/pcl/registration/transformation_validation_euclidean.h +++ b/registration/include/pcl/registration/transformation_validation_euclidean.h @@ -40,6 +40,7 @@ #pragma once +#include #include #include #include diff --git a/registration/include/pcl/registration/warp_point_rigid.h b/registration/include/pcl/registration/warp_point_rigid.h index c96626c4..6c1a76d4 100644 --- a/registration/include/pcl/registration/warp_point_rigid.h +++ b/registration/include/pcl/registration/warp_point_rigid.h @@ -40,6 +40,7 @@ #pragma once +#include #include #include diff --git a/registration/src/correspondence_rejection_organized_boundary.cpp b/registration/src/correspondence_rejection_organized_boundary.cpp index bdf4400b..7b23e855 100644 --- a/registration/src/correspondence_rejection_organized_boundary.cpp +++ b/registration/src/correspondence_rejection_organized_boundary.cpp @@ -37,13 +37,15 @@ */ #include +#include // for static_pointer_cast + ////////////////////////////////////////////////////////////////////////////////////////////// void pcl::registration::CorrespondenceRejectionOrganizedBoundary::getRemainingCorrespondences (const pcl::Correspondences& original_correspondences, pcl::Correspondences& remaining_correspondences) { - pcl::PointCloud::ConstPtr cloud = boost::static_pointer_cast >(data_container_)->getInputTarget (); + pcl::PointCloud::ConstPtr cloud = static_pointer_cast >(data_container_)->getInputTarget (); if (!cloud->isOrganized ()) { diff --git a/registration/src/correspondence_rejection_var_trimmed.cpp b/registration/src/correspondence_rejection_var_trimmed.cpp index cc8e972c..50e6e3a3 100644 --- a/registration/src/correspondence_rejection_var_trimmed.cpp +++ b/registration/src/correspondence_rejection_var_trimmed.cpp @@ -79,7 +79,7 @@ pcl::registration::CorrespondenceRejectorVarTrimmed::getRemainingCorrespondences ////////////////////////////////////////////////////////////////////////////////////////////// float -pcl::registration::CorrespondenceRejectorVarTrimmed::optimizeInlierRatio (std::vector & dists) +pcl::registration::CorrespondenceRejectorVarTrimmed::optimizeInlierRatio (std::vector & dists) const { unsigned int points_nbr = static_cast (dists.size ()); std::sort (dists.begin (), dists.end ()); diff --git a/registration/src/gicp6d.cpp b/registration/src/gicp6d.cpp index 91a13895..aaee37f2 100644 --- a/registration/src/gicp6d.cpp +++ b/registration/src/gicp6d.cpp @@ -38,6 +38,8 @@ #include +#include // for pcl::make_shared + namespace pcl { // convert sRGB to CIELAB @@ -160,7 +162,7 @@ namespace pcl // ...and build 6d-tree target_tree_lab_.setInputCloud (target_lab_); target_tree_lab_.setPointRepresentation ( - boost::make_shared (point_rep_)); + pcl::make_shared (point_rep_)); } bool diff --git a/sample_consensus/include/pcl/sample_consensus/impl/lmeds.hpp b/sample_consensus/include/pcl/sample_consensus/impl/lmeds.hpp index dd4809ee..3816cdeb 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/lmeds.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/lmeds.hpp @@ -57,23 +57,24 @@ pcl::LeastMedianSquares::computeModel (int debug_verbosity_level) iterations_ = 0; double d_best_penalty = std::numeric_limits::max(); - std::vector selection; + Indices selection; Eigen::VectorXf model_coefficients; std::vector distances; - int n_inliers_count = 0; - 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; // Iterate - while (iterations_ < max_iterations_ && skipped_count < max_skip) + while ((iterations_ < max_iterations_) && (skipped_count < max_skip)) { // Get X samples which satisfy the model criteria sac_model_->getSamples (iterations_, selection); - if (selection.empty ()) break; + if (selection.empty ()) + { + break; + } // Search for inliers in the point cloud for the current plane model M if (!sac_model_->computeModelCoefficients (selection, model_coefficients)) @@ -83,7 +84,7 @@ pcl::LeastMedianSquares::computeModel (int debug_verbosity_level) continue; } - double d_cur_penalty = 0; + double d_cur_penalty; // d_cur_penalty = sum (min (dist, threshold)) // Iterate through the 3d points and calculate the distances from them to the model @@ -96,11 +97,14 @@ pcl::LeastMedianSquares::computeModel (int debug_verbosity_level) ++skipped_count; continue; } + // Move all NaNs in distances to the end + const auto new_end = (sac_model_->getInputCloud()->is_dense ? distances.end() : std::partition (distances.begin(), distances.end(), [](double d){return !std::isnan (d);})); + const auto nr_valid_dists = std::distance (distances.begin (), new_end); - std::sort (distances.begin (), distances.end ()); // d_cur_penalty = median (distances) - std::size_t mid = sac_model_->getIndices ()->size () / 2; - if (mid >= distances.size ()) + const std::size_t mid = nr_valid_dists / 2; + PCL_DEBUG ("[pcl::LeastMedianSquares::computeModel] There are %lu valid distances remaining after removing NaN values.\n", nr_valid_dists); + if (nr_valid_dists == 0) { //iterations_++; ++skipped_count; @@ -108,10 +112,21 @@ pcl::LeastMedianSquares::computeModel (int debug_verbosity_level) } // Do we have a "middle" point or should we "estimate" one ? - if (sac_model_->getIndices ()->size () % 2 == 0) - d_cur_penalty = (sqrt (distances[mid-1]) + sqrt (distances[mid])) / 2; + if ((nr_valid_dists % 2) == 0) + { + // Looking at two values instead of one probably doesn't matter because they are mostly barely different, but let's do it for accuracy's sake + std::nth_element (distances.begin (), distances.begin () + (mid - 1), new_end); + const double tmp = distances[mid-1]; + const double tmp2 = *(std::min_element (distances.begin () + mid, new_end)); + d_cur_penalty = (sqrt (tmp) + sqrt (tmp2)) / 2.0; + PCL_DEBUG ("[pcl::LeastMedianSquares::computeModel] Computing median with two values (%g and %g) because number of distances is even.\n", tmp, distances[mid]); + } else + { + std::nth_element (distances.begin (), distances.begin () + mid, new_end); d_cur_penalty = sqrt (distances[mid]); + PCL_DEBUG ("[pcl::LeastMedianSquares::computeModel] Computing median with one value (%g) because number of distances is odd.\n", distances[mid]); + } // Better match ? if (d_cur_penalty < d_best_penalty) @@ -125,13 +140,17 @@ pcl::LeastMedianSquares::computeModel (int debug_verbosity_level) ++iterations_; if (debug_verbosity_level > 1) + { PCL_DEBUG ("[pcl::LeastMedianSquares::computeModel] Trial %d out of %d. Best penalty is %f.\n", iterations_, max_iterations_, d_best_penalty); + } } if (model_.empty ()) { if (debug_verbosity_level > 0) + { PCL_DEBUG ("[pcl::LeastMedianSquares::computeModel] Unable to find a solution!\n"); + } return (false); } @@ -150,7 +169,7 @@ pcl::LeastMedianSquares::computeModel (int debug_verbosity_level) return (false); } - std::vector &indices = *sac_model_->getIndices (); + Indices &indices = *sac_model_->getIndices (); if (distances.size () != indices.size ()) { @@ -160,16 +179,22 @@ pcl::LeastMedianSquares::computeModel (int debug_verbosity_level) inliers_.resize (distances.size ()); // Get the inliers for the best model found - n_inliers_count = 0; + std::size_t n_inliers_count = 0; for (std::size_t i = 0; i < distances.size (); ++i) + { if (distances[i] <= threshold_) + { inliers_[n_inliers_count++] = indices[i]; + } + } // Resize the inliers vector inliers_.resize (n_inliers_count); if (debug_verbosity_level > 0) - PCL_DEBUG ("[pcl::LeastMedianSquares::computeModel] Model: %lu size, %d inliers.\n", model_.size (), n_inliers_count); + { + PCL_DEBUG ("[pcl::LeastMedianSquares::computeModel] Model: %lu size, %lu inliers.\n", model_.size (), n_inliers_count); + } return (true); } @@ -177,4 +202,3 @@ pcl::LeastMedianSquares::computeModel (int debug_verbosity_level) #define PCL_INSTANTIATE_LeastMedianSquares(T) template class PCL_EXPORTS pcl::LeastMedianSquares; #endif // PCL_SAMPLE_CONSENSUS_IMPL_LMEDS_H_ - diff --git a/sample_consensus/include/pcl/sample_consensus/impl/mlesac.hpp b/sample_consensus/include/pcl/sample_consensus/impl/mlesac.hpp index 0a2ba293..15a4137a 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/mlesac.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/mlesac.hpp @@ -59,7 +59,7 @@ pcl::MaximumLikelihoodSampleConsensus::computeModel (int debug_verbosity double d_best_penalty = std::numeric_limits::max(); double k = 1.0; - std::vector selection; + Indices selection; Eigen::VectorXf model_coefficients; std::vector distances; @@ -178,7 +178,7 @@ pcl::MaximumLikelihoodSampleConsensus::computeModel (int debug_verbosity // Iterate through the 3d points and calculate the distances from them to the model again sac_model_->getDistancesToModel (model_coefficients_, distances); - std::vector &indices = *sac_model_->getIndices (); + Indices &indices = *sac_model_->getIndices (); if (distances.size () != indices.size ()) { PCL_ERROR ("[pcl::MaximumLikelihoodSampleConsensus::computeModel] Estimated distances (%lu) differs than the normal of indices (%lu).\n", distances.size (), indices.size ()); diff --git a/sample_consensus/include/pcl/sample_consensus/impl/msac.hpp b/sample_consensus/include/pcl/sample_consensus/impl/msac.hpp index dd2ed9f2..4dcac5b8 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/msac.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/msac.hpp @@ -58,7 +58,7 @@ pcl::MEstimatorSampleConsensus::computeModel (int debug_verbosity_level) double d_best_penalty = std::numeric_limits::max(); double k = 1.0; - std::vector selection; + Indices selection; Eigen::VectorXf model_coefficients; std::vector distances; @@ -136,7 +136,7 @@ pcl::MEstimatorSampleConsensus::computeModel (int debug_verbosity_level) // Iterate through the 3d points and calculate the distances from them to the model again sac_model_->getDistancesToModel (model_coefficients_, distances); - std::vector &indices = *sac_model_->getIndices (); + Indices &indices = *sac_model_->getIndices (); if (distances.size () != indices.size ()) { diff --git a/sample_consensus/include/pcl/sample_consensus/impl/prosac.hpp b/sample_consensus/include/pcl/sample_consensus/impl/prosac.hpp index 41c08ce5..9ab7540b 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/prosac.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/prosac.hpp @@ -82,12 +82,12 @@ pcl::ProgressiveSampleConsensus::computeModel (int debug_verbosity_level // Initialize the usual RANSAC parameters iterations_ = 0; - std::vector inliers; - std::vector selection; + Indices inliers; + Indices selection; Eigen::VectorXf model_coefficients; // We will increase the pool so the indices_ vector can only contain m elements at first - std::vector index_pool; + Indices index_pool; index_pool.reserve (N); for (unsigned int i = 0; i < n; ++i) index_pool.push_back (sac_model_->indices_->operator[](i)); @@ -164,8 +164,7 @@ pcl::ProgressiveSampleConsensus::computeModel (int debug_verbosity_level // We only need to compute possible better epsilon_n_star for when _n is just about to be removed an inlier std::size_t I_possible_n_star = I_N; - for (std::vector::const_reverse_iterator last_inlier = inliers.rbegin (), - inliers_end = inliers.rend (); + for (auto last_inlier = inliers.crbegin (), inliers_end = inliers.crend (); last_inlier != inliers_end; ++last_inlier, --I_possible_n_star) { diff --git a/sample_consensus/include/pcl/sample_consensus/impl/ransac.hpp b/sample_consensus/include/pcl/sample_consensus/impl/ransac.hpp index 6cf00feb..ae33dbcd 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/ransac.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/ransac.hpp @@ -67,7 +67,7 @@ pcl::RandomSampleConsensus::computeModel (int) std::size_t n_best_inliers_count = 0; double k = std::numeric_limits::max(); - std::vector selection; + Indices selection; Eigen::VectorXf model_coefficients; const double log_probability = std::log (1.0 - probability_); diff --git a/sample_consensus/include/pcl/sample_consensus/impl/rmsac.hpp b/sample_consensus/include/pcl/sample_consensus/impl/rmsac.hpp index 50c94025..ff25956c 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/rmsac.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/rmsac.hpp @@ -58,10 +58,10 @@ pcl::RandomizedMEstimatorSampleConsensus::computeModel (int debug_verbos double d_best_penalty = std::numeric_limits::max(); double k = 1.0; - std::vector selection; + Indices selection; Eigen::VectorXf model_coefficients; std::vector distances; - std::set indices_subset; + std::set indices_subset; int n_inliers_count = 0; unsigned skipped_count = 0; @@ -154,7 +154,7 @@ pcl::RandomizedMEstimatorSampleConsensus::computeModel (int debug_verbos // Iterate through the 3d points and calculate the distances from them to the model again sac_model_->getDistancesToModel (model_coefficients_, distances); - std::vector &indices = *sac_model_->getIndices (); + Indices &indices = *sac_model_->getIndices (); if (distances.size () != indices.size ()) { PCL_ERROR ("[pcl::RandomizedMEstimatorSampleConsensus::computeModel] Estimated distances (%lu) differs than the normal of indices (%lu).\n", distances.size (), indices.size ()); diff --git a/sample_consensus/include/pcl/sample_consensus/impl/rransac.hpp b/sample_consensus/include/pcl/sample_consensus/impl/rransac.hpp index 7a839ff8..0e952f08 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/rransac.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/rransac.hpp @@ -58,9 +58,9 @@ pcl::RandomizedRandomSampleConsensus::computeModel (int debug_verbosity_ std::size_t n_best_inliers_count = 0; double k = std::numeric_limits::max(); - std::vector selection; + Indices selection; Eigen::VectorXf model_coefficients; - std::set indices_subset; + std::set indices_subset; const double log_probability = std::log (1.0 - probability_); const double one_over_indices = 1.0 / static_cast (sac_model_->getIndices ()->size ()); diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_circle.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_circle.hpp index 9ba524a8..1900088f 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_circle.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_circle.hpp @@ -47,8 +47,13 @@ ////////////////////////////////////////////////////////////////////////// template bool -pcl::SampleConsensusModelCircle2D::isSampleGood(const std::vector &samples) const +pcl::SampleConsensusModelCircle2D::isSampleGood(const Indices &samples) const { + if (samples.size () != sample_size_) + { + PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n", samples.size (), sample_size_); + return (false); + } // Get the values at the two points Eigen::Array2d p0 (input_->points[samples[0]].x, input_->points[samples[0]].y); Eigen::Array2d p1 (input_->points[samples[1]].x, input_->points[samples[1]].y); @@ -66,16 +71,16 @@ pcl::SampleConsensusModelCircle2D::isSampleGood(const std::vector & ////////////////////////////////////////////////////////////////////////// template bool -pcl::SampleConsensusModelCircle2D::computeModelCoefficients (const std::vector &samples, Eigen::VectorXf &model_coefficients) const +pcl::SampleConsensusModelCircle2D::computeModelCoefficients (const Indices &samples, Eigen::VectorXf &model_coefficients) const { // Need 3 samples - if (samples.size () != 3) + if (samples.size () != sample_size_) { PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ()); return (false); } - model_coefficients.resize (3); + model_coefficients.resize (model_size_); Eigen::Vector2d p0 (input_->points[samples[0]].x, input_->points[samples[0]].y); Eigen::Vector2d p1 (input_->points[samples[1]].x, input_->points[samples[1]].y); @@ -129,7 +134,7 @@ pcl::SampleConsensusModelCircle2D::getDistancesToModel (const Eigen::Vec template void pcl::SampleConsensusModelCircle2D::selectWithinDistance ( const Eigen::VectorXf &model_coefficients, const double threshold, - std::vector &inliers) + Indices &inliers) { // Check if the model is valid given the user constraints if (!isModelValid (model_coefficients)) @@ -137,9 +142,10 @@ pcl::SampleConsensusModelCircle2D::selectWithinDistance ( inliers.clear (); return; } - int nr_p = 0; - inliers.resize (indices_->size ()); - error_sqr_dists_.resize (indices_->size ()); + inliers.clear (); + error_sqr_dists_.clear (); + inliers.reserve (indices_->size ()); + error_sqr_dists_.reserve (indices_->size ()); // Iterate through the 3d points and calculate the distances from them to the circle for (std::size_t i = 0; i < indices_->size (); ++i) @@ -156,13 +162,10 @@ pcl::SampleConsensusModelCircle2D::selectWithinDistance ( if (distance < threshold) { // Returns the indices of the points whose distances are smaller than the threshold - inliers[nr_p] = (*indices_)[i]; - error_sqr_dists_[nr_p] = static_cast (distance); - ++nr_p; + inliers.push_back ((*indices_)[i]); + error_sqr_dists_.push_back (static_cast (distance)); } } - inliers.resize (nr_p); - error_sqr_dists_.resize (nr_p); } ////////////////////////////////////////////////////////////////////////// @@ -196,21 +199,21 @@ pcl::SampleConsensusModelCircle2D::countWithinDistance ( ////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusModelCircle2D::optimizeModelCoefficients ( - const std::vector &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const + const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const { optimized_coefficients = model_coefficients; // Needs a set of valid model coefficients - if (model_coefficients.size () != 3) + if (!isModelValid (model_coefficients)) { - PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::optimizeModelCoefficients] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::optimizeModelCoefficients] Given model is invalid!\n"); return; } - // Need at least 3 samples - if (inliers.size () <= 3) + // Need more than the minimum sample size to make a difference + if (inliers.size () <= sample_size_) { - PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::optimizeModelCoefficients] Not enough inliers found to support a model (%lu)! Returning the same coefficients.\n", inliers.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::optimizeModelCoefficients] Not enough inliers to refine/optimize the model's coefficients (%lu)! Returning the same coefficients.\n", inliers.size ()); return; } @@ -227,13 +230,13 @@ pcl::SampleConsensusModelCircle2D::optimizeModelCoefficients ( ////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusModelCircle2D::projectPoints ( - const std::vector &inliers, const Eigen::VectorXf &model_coefficients, + const Indices &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields) const { // Needs a valid set of model coefficients - if (model_coefficients.size () != 3) + if (!isModelValid (model_coefficients)) { - PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::projectPoints] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::projectPoints] Given model is invalid!\n"); return; } @@ -255,7 +258,7 @@ pcl::SampleConsensusModelCircle2D::projectPoints ( pcl::for_each_type (NdConcatenateFunctor (input_->points[i], projected_points.points[i])); // Iterate through the points and project them to the circle - for (const int &inlier : inliers) + for (const auto &inlier : inliers) { float dx = input_->points[inlier].x - model_coefficients[0]; float dy = input_->points[inlier].y - model_coefficients[1]; @@ -294,16 +297,16 @@ pcl::SampleConsensusModelCircle2D::projectPoints ( ////////////////////////////////////////////////////////////////////////// template bool pcl::SampleConsensusModelCircle2D::doSamplesVerifyModel ( - const std::set &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const + const std::set &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const { // Needs a valid model coefficients - if (model_coefficients.size () != 3) + if (!isModelValid (model_coefficients)) { - PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::doSamplesVerifyModel] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::doSamplesVerifyModel] Given model is invalid!\n"); return (false); } - for (const int &index : indices) + for (const auto &index : indices) // Calculate the distance from the point to the circle as the difference between //dist(point,circle_origin) and circle_radius if (std::abs (std::sqrt ( diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_circle3d.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_circle3d.hpp index 4273a890..781758c6 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_circle3d.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_circle3d.hpp @@ -46,8 +46,13 @@ ////////////////////////////////////////////////////////////////////////// template bool pcl::SampleConsensusModelCircle3D::isSampleGood ( - const std::vector &samples) const + const Indices &samples) const { + if (samples.size () != sample_size_) + { + PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n", samples.size (), sample_size_); + return (false); + } // Get the values at the three points Eigen::Vector3d p0 (input_->points[samples[0]].x, input_->points[samples[0]].y, input_->points[samples[0]].z); Eigen::Vector3d p1 (input_->points[samples[1]].x, input_->points[samples[1]].y, input_->points[samples[1]].z); @@ -62,16 +67,16 @@ pcl::SampleConsensusModelCircle3D::isSampleGood ( ////////////////////////////////////////////////////////////////////////// template bool -pcl::SampleConsensusModelCircle3D::computeModelCoefficients (const std::vector &samples, Eigen::VectorXf &model_coefficients) const +pcl::SampleConsensusModelCircle3D::computeModelCoefficients (const Indices &samples, Eigen::VectorXf &model_coefficients) const { // Need 3 samples - if (samples.size () != 3) + if (samples.size () != sample_size_) { PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ()); return (false); } - model_coefficients.resize (7); //needing 7 coefficients: centerX, centerY, centerZ, radius, normalX, normalY, normalZ + model_coefficients.resize (model_size_); //needing 7 coefficients: centerX, centerY, centerZ, radius, normalX, normalY, normalZ Eigen::Vector3d p0 (input_->points[samples[0]].x, input_->points[samples[0]].y, input_->points[samples[0]].z); Eigen::Vector3d p1 (input_->points[samples[1]].x, input_->points[samples[1]].y, input_->points[samples[1]].z); @@ -161,7 +166,7 @@ pcl::SampleConsensusModelCircle3D::getDistancesToModel (const Eigen::Vec template void pcl::SampleConsensusModelCircle3D::selectWithinDistance ( const Eigen::VectorXf &model_coefficients, const double threshold, - std::vector &inliers) + Indices &inliers) { // Check if the model is valid given the user constraints if (!isModelValid (model_coefficients)) @@ -169,8 +174,8 @@ pcl::SampleConsensusModelCircle3D::selectWithinDistance ( inliers.clear (); return; } - int nr_p = 0; - inliers.resize (indices_->size ()); + inliers.clear (); + inliers.reserve (indices_->size ()); // Iterate through the 3d points and calculate the distances from them to the sphere for (std::size_t i = 0; i < indices_->size (); ++i) @@ -199,11 +204,9 @@ pcl::SampleConsensusModelCircle3D::selectWithinDistance ( if (distanceVector.norm () < threshold) { // Returns the indices of the points whose distances are smaller than the threshold - inliers[nr_p] = (*indices_)[i]; - nr_p++; + inliers.push_back ((*indices_)[i]); } } - inliers.resize (nr_p); } ////////////////////////////////////////////////////////////////////////// @@ -250,23 +253,23 @@ pcl::SampleConsensusModelCircle3D::countWithinDistance ( ////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusModelCircle3D::optimizeModelCoefficients ( - const std::vector &inliers, + const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const { optimized_coefficients = model_coefficients; // Needs a set of valid model coefficients - if (model_coefficients.size () != 7) + if (!isModelValid (model_coefficients)) { - PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::optimizeModelCoefficients] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::optimizeModelCoefficients] Given model is invalid!\n"); return; } - // Need at least 3 samples - if (inliers.size () <= 3) + // Need more than the minimum sample size to make a difference + if (inliers.size () <= sample_size_) { - PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::optimizeModelCoefficients] Not enough inliers found to support a model (%lu)! Returning the same coefficients.\n", inliers.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::optimizeModelCoefficients] Not enough inliers to refine/optimize the model's coefficients (%lu)! Returning the same coefficients.\n", inliers.size ()); return; } @@ -286,13 +289,13 @@ pcl::SampleConsensusModelCircle3D::optimizeModelCoefficients ( ////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusModelCircle3D::projectPoints ( - const std::vector &inliers, const Eigen::VectorXf &model_coefficients, + const Indices &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields) const { // Needs a valid set of model coefficients - if (model_coefficients.size () != 7) + if (!isModelValid (model_coefficients)) { - PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::projectPoints] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::projectPoints] Given model is invalid!\n"); return; } @@ -388,18 +391,18 @@ pcl::SampleConsensusModelCircle3D::projectPoints ( ////////////////////////////////////////////////////////////////////////// template bool pcl::SampleConsensusModelCircle3D::doSamplesVerifyModel ( - const std::set &indices, + const std::set &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const { // Needs a valid model coefficients - if (model_coefficients.size () != 7) + if (!isModelValid (model_coefficients)) { - PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::doSamplesVerifyModel] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::doSamplesVerifyModel] Given model is invalid!\n"); return (false); } - for (const int &index : indices) + for (const auto &index : indices) { // Calculate the distance from the point to the sphere as the difference between //dist(point,sphere_origin) and sphere_radius diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_cone.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_cone.hpp index dabe69c0..6d52c254 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_cone.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_cone.hpp @@ -45,18 +45,23 @@ ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template bool -pcl::SampleConsensusModelCone::isSampleGood(const std::vector &) const +pcl::SampleConsensusModelCone::isSampleGood(const Indices &samples) const { + if (samples.size () != sample_size_) + { + PCL_ERROR ("[pcl::SampleConsensusModelCone::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n", samples.size (), sample_size_); + return (false); + } return (true); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template bool pcl::SampleConsensusModelCone::computeModelCoefficients ( - const std::vector &samples, Eigen::VectorXf &model_coefficients) const + const Indices &samples, Eigen::VectorXf &model_coefficients) const { // Need 3 samples - if (samples.size () != 3) + if (samples.size () != sample_size_) { PCL_ERROR ("[pcl::SampleConsensusModelCone::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ()); return (false); @@ -68,13 +73,13 @@ pcl::SampleConsensusModelCone::computeModelCoefficients ( return (false); } - Eigen::Vector4f p1 (input_->points[samples[0]].x, input_->points[samples[0]].y, input_->points[samples[0]].z, 0); - Eigen::Vector4f p2 (input_->points[samples[1]].x, input_->points[samples[1]].y, input_->points[samples[1]].z, 0); - Eigen::Vector4f p3 (input_->points[samples[2]].x, input_->points[samples[2]].y, input_->points[samples[2]].z, 0); + Eigen::Vector4f p1 (input_->points[samples[0]].x, input_->points[samples[0]].y, input_->points[samples[0]].z, 0.0f); + Eigen::Vector4f p2 (input_->points[samples[1]].x, input_->points[samples[1]].y, input_->points[samples[1]].z, 0.0f); + Eigen::Vector4f p3 (input_->points[samples[2]].x, input_->points[samples[2]].y, input_->points[samples[2]].z, 0.0f); - Eigen::Vector4f n1 (normals_->points[samples[0]].normal[0], normals_->points[samples[0]].normal[1], normals_->points[samples[0]].normal[2], 0); - Eigen::Vector4f n2 (normals_->points[samples[1]].normal[0], normals_->points[samples[1]].normal[1], normals_->points[samples[1]].normal[2], 0); - Eigen::Vector4f n3 (normals_->points[samples[2]].normal[0], normals_->points[samples[2]].normal[1], normals_->points[samples[2]].normal[2], 0); + Eigen::Vector4f n1 (normals_->points[samples[0]].normal[0], normals_->points[samples[0]].normal[1], normals_->points[samples[0]].normal[2], 0.0f); + Eigen::Vector4f n2 (normals_->points[samples[1]].normal[0], normals_->points[samples[1]].normal[1], normals_->points[samples[1]].normal[2], 0.0f); + Eigen::Vector4f n3 (normals_->points[samples[2]].normal[0], normals_->points[samples[2]].normal[1], normals_->points[samples[2]].normal[2], 0.0f); //calculate apex (intersection of the three planes defined by points and belonging normals Eigen::Vector4f ortho12 = n1.cross3(n2); @@ -112,7 +117,7 @@ pcl::SampleConsensusModelCone::computeModelCoefficients ( //compute opening angle float opening_angle = ( std::acos (ap1.dot (axis_dir)) + std::acos (ap2.dot (axis_dir)) + std::acos (ap3.dot (axis_dir)) ) / 3.0f; - model_coefficients.resize (7); + model_coefficients.resize (model_size_); // model_coefficients.template head<3> () = line_pt.template head<3> (); model_coefficients[0] = apex[0]; model_coefficients[1] = apex[1]; @@ -146,8 +151,8 @@ pcl::SampleConsensusModelCone::getDistancesToModel ( distances.resize (indices_->size ()); - Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); - Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); + Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f); + Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f); float opening_angle = model_coefficients[6]; float apexdotdir = apex.dot (axis_dir); @@ -155,8 +160,8 @@ pcl::SampleConsensusModelCone::getDistancesToModel ( // Iterate through the 3d points and calculate the distances from them to the cone for (std::size_t i = 0; i < indices_->size (); ++i) { - Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0); - Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0); + Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0.0f); + Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0.0f); // Calculate the point's projection on the cone axis float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir; @@ -180,14 +185,14 @@ pcl::SampleConsensusModelCone::getDistancesToModel ( double d_normal = std::abs (getAngle3D (n, cone_normal)); d_normal = (std::min) (d_normal, M_PI - d_normal); - distances[i] = std::abs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid); + distances[i] = std::abs (normal_distance_weight_ * d_normal + (1.0 - normal_distance_weight_) * d_euclid); } } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusModelCone::selectWithinDistance ( - const Eigen::VectorXf &model_coefficients, const double threshold, std::vector &inliers) + const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers) { // Check if the model is valid given the user constraints if (!isModelValid (model_coefficients)) @@ -196,12 +201,13 @@ pcl::SampleConsensusModelCone::selectWithinDistance ( return; } - int nr_p = 0; - inliers.resize (indices_->size ()); - error_sqr_dists_.resize (indices_->size ()); + inliers.clear (); + error_sqr_dists_.clear (); + inliers.reserve (indices_->size ()); + error_sqr_dists_.reserve (indices_->size ()); - Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); - Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); + Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f); + Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f); float opening_angle = model_coefficients[6]; float apexdotdir = apex.dot (axis_dir); @@ -209,8 +215,8 @@ pcl::SampleConsensusModelCone::selectWithinDistance ( // Iterate through the 3d points and calculate the distances from them to the cone for (std::size_t i = 0; i < indices_->size (); ++i) { - Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0); - Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0); + Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0.0f); + Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0.0f); // Calculate the point's projection on the cone axis float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir; @@ -236,18 +242,15 @@ pcl::SampleConsensusModelCone::selectWithinDistance ( double d_normal = std::abs (getAngle3D (n, cone_normal)); d_normal = (std::min) (d_normal, M_PI - d_normal); - double distance = std::abs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid); + double distance = std::abs (normal_distance_weight_ * d_normal + (1.0 - normal_distance_weight_) * d_euclid); if (distance < threshold) { // Returns the indices of the points whose distances are smaller than the threshold - inliers[nr_p] = (*indices_)[i]; - error_sqr_dists_[nr_p] = distance; - ++nr_p; + inliers.push_back ((*indices_)[i]); + error_sqr_dists_.push_back (distance); } } - inliers.resize (nr_p); - error_sqr_dists_.resize (nr_p); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -262,8 +265,8 @@ pcl::SampleConsensusModelCone::countWithinDistance ( std::size_t nr_p = 0; - Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); - Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); + Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f); + Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f); float opening_angle = model_coefficients[6]; float apexdotdir = apex.dot (axis_dir); @@ -271,8 +274,8 @@ pcl::SampleConsensusModelCone::countWithinDistance ( // Iterate through the 3d points and calculate the distances from them to the cone for (std::size_t i = 0; i < indices_->size (); ++i) { - Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0); - Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0); + Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0.0f); + Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0.0f); // Calculate the point's projection on the cone axis float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir; @@ -298,7 +301,7 @@ pcl::SampleConsensusModelCone::countWithinDistance ( double d_normal = std::abs (getAngle3D (n, cone_normal)); d_normal = (std::min) (d_normal, M_PI - d_normal); - if (std::abs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid) < threshold) + if (std::abs (normal_distance_weight_ * d_normal + (1.0 - normal_distance_weight_) * d_euclid) < threshold) nr_p++; } return (nr_p); @@ -307,20 +310,21 @@ pcl::SampleConsensusModelCone::countWithinDistance ( ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusModelCone::optimizeModelCoefficients ( - const std::vector &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const + const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const { optimized_coefficients = model_coefficients; // Needs a set of valid model coefficients - if (model_coefficients.size () != 7) + if (!isModelValid (model_coefficients)) { - PCL_ERROR ("[pcl::SampleConsensusModelCone::optimizeModelCoefficients] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelCone::optimizeModelCoefficients] Given model is invalid!\n"); return; } - if (inliers.empty ()) + // Need more than the minimum sample size to make a difference + if (inliers.size () <= sample_size_) { - PCL_DEBUG ("[pcl::SampleConsensusModelCone:optimizeModelCoefficients] Inliers vector empty! Returning the same coefficients.\n"); + PCL_ERROR ("[pcl::SampleConsensusModelCone:optimizeModelCoefficients] Not enough inliers found to optimize model coefficients (%lu)! Returning the same coefficients.\n", inliers.size ()); return; } @@ -344,20 +348,20 @@ pcl::SampleConsensusModelCone::optimizeModelCoefficients ( ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusModelCone::projectPoints ( - const std::vector &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields) const + const Indices &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields) const { // Needs a valid set of model coefficients - if (model_coefficients.size () != 7) + if (!isModelValid (model_coefficients)) { - PCL_ERROR ("[pcl::SampleConsensusModelCone::projectPoints] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelCone::projectPoints] Given model is invalid!\n"); return; } projected_points.header = input_->header; projected_points.is_dense = input_->is_dense; - Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); - Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); + Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f); + Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f); float opening_angle = model_coefficients[6]; float apexdotdir = apex.dot (axis_dir); @@ -378,7 +382,7 @@ pcl::SampleConsensusModelCone::projectPoints ( pcl::for_each_type (NdConcatenateFunctor (input_->points[i], projected_points.points[i])); // Iterate through the 3d points and calculate the distances from them to the cone - for (const int &inlier : inliers) + for (const auto &inlier : inliers) { Eigen::Vector4f pt (input_->points[inlier].x, input_->points[inlier].y, @@ -440,26 +444,26 @@ pcl::SampleConsensusModelCone::projectPoints ( ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template bool pcl::SampleConsensusModelCone::doSamplesVerifyModel ( - const std::set &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const + const std::set &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const { // Needs a valid model coefficients - if (model_coefficients.size () != 7) + if (!isModelValid (model_coefficients)) { - PCL_ERROR ("[pcl::SampleConsensusModelCone::doSamplesVerifyModel] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelCone::doSamplesVerifyModel] Given model is invalid!\n"); return (false); } - Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); - Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); + Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f); + Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f); float openning_angle = model_coefficients[6]; float apexdotdir = apex.dot (axis_dir); float dirdotdir = 1.0f / axis_dir.dot (axis_dir); // Iterate through the 3d points and calculate the distances from them to the cone - for (const int &index : indices) + for (const auto &index : indices) { - Eigen::Vector4f pt (input_->points[index].x, input_->points[index].y, input_->points[index].z, 0); + Eigen::Vector4f pt (input_->points[index].x, input_->points[index].y, input_->points[index].z, 0.0f); // Calculate the point's projection on the cone axis float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir; @@ -485,8 +489,8 @@ template double pcl::SampleConsensusModelCone::pointToAxisDistance ( const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients) const { - Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); - Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); + Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f); + Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f); return sqrt(pcl::sqrPointToLineDistance (pt, apex, axis_dir)); } @@ -501,14 +505,9 @@ pcl::SampleConsensusModelCone::isModelValid (const Eigen::Vecto if (eps_angle_ > 0.0) { // Obtain the cone direction - Eigen::Vector4f coeff; - coeff[0] = model_coefficients[3]; - coeff[1] = model_coefficients[4]; - coeff[2] = model_coefficients[5]; - coeff[3] = 0; - - Eigen::Vector4f axis (axis_[0], axis_[1], axis_[2], 0); - double angle_diff = std::abs (getAngle3D (axis, coeff)); + const Eigen::Vector3f coeff(model_coefficients[3], model_coefficients[4], model_coefficients[5]); + + double angle_diff = std::abs (getAngle3D (axis_, coeff)); angle_diff = (std::min) (angle_diff, M_PI - angle_diff); // Check whether the current cone model satisfies our angle threshold criterion with respect to the given axis if (angle_diff > eps_angle_) diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_cylinder.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_cylinder.hpp index 733f8d7f..10e32744 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_cylinder.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_cylinder.hpp @@ -47,18 +47,23 @@ ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template bool -pcl::SampleConsensusModelCylinder::isSampleGood (const std::vector &) const +pcl::SampleConsensusModelCylinder::isSampleGood (const Indices &samples) const { + if (samples.size () != sample_size_) + { + PCL_ERROR ("[pcl::SampleConsensusModelCylinder::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n", samples.size (), sample_size_); + return (false); + } return (true); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template bool pcl::SampleConsensusModelCylinder::computeModelCoefficients ( - const std::vector &samples, Eigen::VectorXf &model_coefficients) const + const Indices &samples, Eigen::VectorXf &model_coefficients) const { // Need 2 samples - if (samples.size () != 2) + if (samples.size () != sample_size_) { PCL_ERROR ("[pcl::SampleConsensusModelCylinder::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ()); return (false); @@ -77,11 +82,11 @@ pcl::SampleConsensusModelCylinder::computeModelCoefficients ( return (false); } - Eigen::Vector4f p1 (input_->points[samples[0]].x, input_->points[samples[0]].y, input_->points[samples[0]].z, 0); - Eigen::Vector4f p2 (input_->points[samples[1]].x, input_->points[samples[1]].y, input_->points[samples[1]].z, 0); + Eigen::Vector4f p1 (input_->points[samples[0]].x, input_->points[samples[0]].y, input_->points[samples[0]].z, 0.0f); + Eigen::Vector4f p2 (input_->points[samples[1]].x, input_->points[samples[1]].y, input_->points[samples[1]].z, 0.0f); - Eigen::Vector4f n1 (normals_->points[samples[0]].normal[0], normals_->points[samples[0]].normal[1], normals_->points[samples[0]].normal[2], 0); - Eigen::Vector4f n2 (normals_->points[samples[1]].normal[0], normals_->points[samples[1]].normal[1], normals_->points[samples[1]].normal[2], 0); + Eigen::Vector4f n1 (normals_->points[samples[0]].normal[0], normals_->points[samples[0]].normal[1], normals_->points[samples[0]].normal[2], 0.0f); + Eigen::Vector4f n2 (normals_->points[samples[1]].normal[0], normals_->points[samples[1]].normal[1], normals_->points[samples[1]].normal[2], 0.0f); Eigen::Vector4f w = n1 + p1 - p2; float a = n1.dot (n1); @@ -108,7 +113,7 @@ pcl::SampleConsensusModelCylinder::computeModelCoefficients ( Eigen::Vector4f line_dir = p2 + tc * n2 - line_pt; line_dir.normalize (); - model_coefficients.resize (7); + model_coefficients.resize (model_size_); // model_coefficients.template head<3> () = line_pt.template head<3> (); model_coefficients[0] = line_pt[0]; model_coefficients[1] = line_pt[1]; @@ -140,8 +145,8 @@ pcl::SampleConsensusModelCylinder::getDistancesToModel ( distances.resize (indices_->size ()); - Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); - Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); + Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f); + Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f); float ptdotdir = line_pt.dot (line_dir); float dirdotdir = 1.0f / line_dir.dot (line_dir); // Iterate through the 3d points and calculate the distances from them to the sphere @@ -150,8 +155,8 @@ pcl::SampleConsensusModelCylinder::getDistancesToModel ( // Approximate the distance from the point to the cylinder as the difference between // dist(point,cylinder_axis) and cylinder radius // @note need to revise this. - Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0); - Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0); + Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0.0f); + Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0.0f); double d_euclid = std::abs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]); @@ -165,14 +170,14 @@ pcl::SampleConsensusModelCylinder::getDistancesToModel ( double d_normal = std::abs (getAngle3D (n, dir)); d_normal = (std::min) (d_normal, M_PI - d_normal); - distances[i] = std::abs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid); + distances[i] = std::abs (normal_distance_weight_ * d_normal + (1.0 - normal_distance_weight_) * d_euclid); } } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusModelCylinder::selectWithinDistance ( - const Eigen::VectorXf &model_coefficients, const double threshold, std::vector &inliers) + const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers) { // Check if the model is valid given the user constraints if (!isModelValid (model_coefficients)) @@ -181,12 +186,13 @@ pcl::SampleConsensusModelCylinder::selectWithinDistance ( return; } - int nr_p = 0; - inliers.resize (indices_->size ()); - error_sqr_dists_.resize (indices_->size ()); + inliers.clear (); + error_sqr_dists_.clear (); + inliers.reserve (indices_->size ()); + error_sqr_dists_.reserve (indices_->size ()); - Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); - Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); + Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f); + Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f); float ptdotdir = line_pt.dot (line_dir); float dirdotdir = 1.0f / line_dir.dot (line_dir); // Iterate through the 3d points and calculate the distances from them to the sphere @@ -194,8 +200,8 @@ pcl::SampleConsensusModelCylinder::selectWithinDistance ( { // Approximate the distance from the point to the cylinder as the difference between // dist(point,cylinder_axis) and cylinder radius - Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0); - Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0); + Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0.0f); + Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0.0f); double d_euclid = std::abs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]); // Calculate the point's projection on the cylinder axis @@ -208,17 +214,14 @@ pcl::SampleConsensusModelCylinder::selectWithinDistance ( double d_normal = std::abs (getAngle3D (n, dir)); d_normal = (std::min) (d_normal, M_PI - d_normal); - double distance = std::abs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid); + double distance = std::abs (normal_distance_weight_ * d_normal + (1.0 - normal_distance_weight_) * d_euclid); if (distance < threshold) { // Returns the indices of the points whose distances are smaller than the threshold - inliers[nr_p] = (*indices_)[i]; - error_sqr_dists_[nr_p] = distance; - ++nr_p; + inliers.push_back ((*indices_)[i]); + error_sqr_dists_.push_back (distance); } } - inliers.resize (nr_p); - error_sqr_dists_.resize (nr_p); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -241,8 +244,8 @@ pcl::SampleConsensusModelCylinder::countWithinDistance ( { // Approximate the distance from the point to the cylinder as the difference between // dist(point,cylinder_axis) and cylinder radius - Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0); - Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0); + Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0.0f); + Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0.0f); double d_euclid = std::abs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]); // Calculate the point's projection on the cylinder axis @@ -255,7 +258,7 @@ pcl::SampleConsensusModelCylinder::countWithinDistance ( double d_normal = std::abs (getAngle3D (n, dir)); d_normal = (std::min) (d_normal, M_PI - d_normal); - if (std::abs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid) < threshold) + if (std::abs (normal_distance_weight_ * d_normal + (1.0 - normal_distance_weight_) * d_euclid) < threshold) nr_p++; } return (nr_p); @@ -264,20 +267,21 @@ pcl::SampleConsensusModelCylinder::countWithinDistance ( ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusModelCylinder::optimizeModelCoefficients ( - const std::vector &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const + const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const { optimized_coefficients = model_coefficients; // Needs a set of valid model coefficients - if (model_coefficients.size () != 7) + if (!isModelValid (model_coefficients)) { - PCL_ERROR ("[pcl::SampleConsensusModelCylinder::optimizeModelCoefficients] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelCylinder::optimizeModelCoefficients] Given model is invalid!\n"); return; } - if (inliers.empty ()) + // Need more than the minimum sample size to make a difference + if (inliers.size () <= sample_size_) { - PCL_DEBUG ("[pcl::SampleConsensusModelCylinder:optimizeModelCoefficients] Inliers vector empty! Returning the same coefficients.\n"); + PCL_ERROR ("[pcl::SampleConsensusModelCylinder:optimizeModelCoefficients] Not enough inliers found to optimize model coefficients (%lu)! Returning the same coefficients.\n", inliers.size ()); return; } @@ -301,20 +305,20 @@ pcl::SampleConsensusModelCylinder::optimizeModelCoefficients ( ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusModelCylinder::projectPoints ( - const std::vector &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields) const + const Indices &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields) const { // Needs a valid set of model coefficients - if (model_coefficients.size () != 7) + if (!isModelValid (model_coefficients)) { - PCL_ERROR ("[pcl::SampleConsensusModelCylinder::projectPoints] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelCylinder::projectPoints] Given model is invalid!\n"); return; } projected_points.header = input_->header; projected_points.is_dense = input_->is_dense; - Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); - Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); + Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f); + Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f); float ptdotdir = line_pt.dot (line_dir); float dirdotdir = 1.0f / line_dir.dot (line_dir); @@ -333,7 +337,7 @@ pcl::SampleConsensusModelCylinder::projectPoints ( pcl::for_each_type (NdConcatenateFunctor (input_->points[i], projected_points.points[i])); // Iterate through the 3d points and calculate the distances from them to the cylinder - for (const int &inlier : inliers) + for (const auto &inlier : inliers) { Eigen::Vector4f p (input_->points[inlier].x, input_->points[inlier].y, @@ -387,21 +391,21 @@ pcl::SampleConsensusModelCylinder::projectPoints ( ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template bool pcl::SampleConsensusModelCylinder::doSamplesVerifyModel ( - const std::set &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const + const std::set &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const { // Needs a valid model coefficients - if (model_coefficients.size () != 7) + if (!isModelValid (model_coefficients)) { - PCL_ERROR ("[pcl::SampleConsensusModelCylinder::doSamplesVerifyModel] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelCylinder::doSamplesVerifyModel] Given model is invalid!\n"); return (false); } - for (const int &index : indices) + for (const auto &index : indices) { // Approximate the distance from the point to the cylinder as the difference between // dist(point,cylinder_axis) and cylinder radius // @note need to revise this. - Eigen::Vector4f pt (input_->points[index].x, input_->points[index].y, input_->points[index].z, 0); + Eigen::Vector4f pt (input_->points[index].x, input_->points[index].y, input_->points[index].z, 0.0f); if (std::abs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]) > threshold) return (false); } @@ -414,8 +418,8 @@ template double pcl::SampleConsensusModelCylinder::pointToLineDistance ( const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients) const { - Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); - Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); + Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f); + Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f); return sqrt(pcl::sqrPointToLineDistance (pt, line_pt, line_dir)); } @@ -424,8 +428,8 @@ template void pcl::SampleConsensusModelCylinder::projectPointToCylinder ( const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients, Eigen::Vector4f &pt_proj) const { - Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); - Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); + Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f); + Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f); float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) * line_dir.dot (line_dir); pt_proj = line_pt + k * line_dir; @@ -448,14 +452,9 @@ pcl::SampleConsensusModelCylinder::isModelValid (const Eigen::V if (eps_angle_ > 0.0) { // Obtain the cylinder direction - Eigen::Vector4f coeff; - coeff[0] = model_coefficients[3]; - coeff[1] = model_coefficients[4]; - coeff[2] = model_coefficients[5]; - coeff[3] = 0; - - Eigen::Vector4f axis (axis_[0], axis_[1], axis_[2], 0); - double angle_diff = std::abs (getAngle3D (axis, coeff)); + const Eigen::Vector3f coeff(model_coefficients[3], model_coefficients[4], model_coefficients[5]); + + double angle_diff = std::abs (getAngle3D (axis_, coeff)); angle_diff = (std::min) (angle_diff, M_PI - angle_diff); // Check whether the current cylinder model satisfies our angle threshold criterion with respect to the given axis if (angle_diff > eps_angle_) diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_line.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_line.hpp index 30a15884..bd06fcef 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_line.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_line.hpp @@ -47,8 +47,13 @@ ////////////////////////////////////////////////////////////////////////// template bool -pcl::SampleConsensusModelLine::isSampleGood (const std::vector &samples) const +pcl::SampleConsensusModelLine::isSampleGood (const Indices &samples) const { + if (samples.size () != sample_size_) + { + PCL_ERROR ("[pcl::SampleConsensusModelLine::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n", samples.size (), sample_size_); + return (false); + } // Make sure that the two sample points are not identical if ( (input_->points[samples[0]].x != input_->points[samples[1]].x) @@ -66,10 +71,10 @@ pcl::SampleConsensusModelLine::isSampleGood (const std::vector &sam ////////////////////////////////////////////////////////////////////////// template bool pcl::SampleConsensusModelLine::computeModelCoefficients ( - const std::vector &samples, Eigen::VectorXf &model_coefficients) const + const Indices &samples, Eigen::VectorXf &model_coefficients) const { // Need 2 samples - if (samples.size () != 2) + if (samples.size () != sample_size_) { PCL_ERROR ("[pcl::SampleConsensusModelLine::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ()); return (false); @@ -82,7 +87,7 @@ pcl::SampleConsensusModelLine::computeModelCoefficients ( return (false); } - model_coefficients.resize (6); + model_coefficients.resize (model_size_); model_coefficients[0] = input_->points[samples[0]].x; model_coefficients[1] = input_->points[samples[0]].y; model_coefficients[2] = input_->points[samples[0]].z; @@ -102,7 +107,9 @@ pcl::SampleConsensusModelLine::getDistancesToModel ( { // Needs a valid set of model coefficients if (!isModelValid (model_coefficients)) + { return; + } distances.resize (indices_->size ()); @@ -124,7 +131,7 @@ pcl::SampleConsensusModelLine::getDistancesToModel ( ////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusModelLine::selectWithinDistance ( - const Eigen::VectorXf &model_coefficients, const double threshold, std::vector &inliers) + const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers) { // Needs a valid set of model coefficients if (!isModelValid (model_coefficients)) @@ -132,9 +139,10 @@ pcl::SampleConsensusModelLine::selectWithinDistance ( double sqr_threshold = threshold * threshold; - int nr_p = 0; - inliers.resize (indices_->size ()); - error_sqr_dists_.resize (indices_->size ()); + inliers.clear (); + error_sqr_dists_.clear (); + inliers.reserve (indices_->size ()); + error_sqr_dists_.reserve (indices_->size ()); // Obtain the line point and direction Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); @@ -151,13 +159,10 @@ pcl::SampleConsensusModelLine::selectWithinDistance ( if (sqr_distance < sqr_threshold) { // Returns the indices of the points whose squared distances are smaller than the threshold - inliers[nr_p] = (*indices_)[i]; - error_sqr_dists_[nr_p] = sqr_distance; - ++nr_p; + inliers.push_back ((*indices_)[i]); + error_sqr_dists_.push_back (sqr_distance); } } - inliers.resize (nr_p); - error_sqr_dists_.resize (nr_p); } ////////////////////////////////////////////////////////////////////////// @@ -174,8 +179,8 @@ pcl::SampleConsensusModelLine::countWithinDistance ( std::size_t nr_p = 0; // Obtain the line point and direction - Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); - Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); + Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f); + Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f); line_dir.normalize (); // Iterate through the 3d points and calculate the distances from them to the line @@ -194,7 +199,7 @@ pcl::SampleConsensusModelLine::countWithinDistance ( ////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusModelLine::optimizeModelCoefficients ( - const std::vector &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const + const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const { // Needs a valid set of model coefficients if (!isModelValid (model_coefficients)) @@ -203,15 +208,15 @@ pcl::SampleConsensusModelLine::optimizeModelCoefficients ( return; } - // Need at least 2 points to estimate a line - if (inliers.size () <= 2) + // Need more than the minimum sample size to make a difference + if (inliers.size () <= sample_size_) { - PCL_ERROR ("[pcl::SampleConsensusModelLine::optimizeModelCoefficients] Not enough inliers found to support a model (%lu)! Returning the same coefficients.\n", inliers.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelLine::optimizeModelCoefficients] Not enough inliers to refine/optimize the model's coefficients (%lu)! Returning the same coefficients.\n", inliers.size ()); optimized_coefficients = model_coefficients; return; } - optimized_coefficients.resize (6); + optimized_coefficients.resize (model_size_); // Compute the 3x3 covariance matrix Eigen::Vector4f centroid; @@ -235,15 +240,15 @@ pcl::SampleConsensusModelLine::optimizeModelCoefficients ( ////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusModelLine::projectPoints ( - const std::vector &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields) const + const Indices &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields) const { // Needs a valid model coefficients if (!isModelValid (model_coefficients)) return; // Obtain the line point and direction - Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); - Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); + Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f); + Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f); projected_points.header = input_->header; projected_points.is_dense = input_->is_dense; @@ -263,9 +268,9 @@ pcl::SampleConsensusModelLine::projectPoints ( pcl::for_each_type (NdConcatenateFunctor (input_->points[i], projected_points.points[i])); // Iterate through the 3d points and calculate the distances from them to the line - for (const int &inlier : inliers) + for (const auto &inlier : inliers) { - Eigen::Vector4f pt (input_->points[inlier].x, input_->points[inlier].y, input_->points[inlier].z, 0); + Eigen::Vector4f pt (input_->points[inlier].x, input_->points[inlier].y, input_->points[inlier].z, 0.0f); // double k = (DOT_PROD_3D (points[i], p21) - dotA_B) / dotB_B; float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir); @@ -292,7 +297,7 @@ pcl::SampleConsensusModelLine::projectPoints ( // Iterate through the 3d points and calculate the distances from them to the line for (std::size_t i = 0; i < inliers.size (); ++i) { - Eigen::Vector4f pt (input_->points[inliers[i]].x, input_->points[inliers[i]].y, input_->points[inliers[i]].z, 0); + Eigen::Vector4f pt (input_->points[inliers[i]].x, input_->points[inliers[i]].y, input_->points[inliers[i]].z, 0.0f); // double k = (DOT_PROD_3D (points[i], p21) - dotA_B) / dotB_B; float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir); @@ -308,20 +313,20 @@ pcl::SampleConsensusModelLine::projectPoints ( ////////////////////////////////////////////////////////////////////////// template bool pcl::SampleConsensusModelLine::doSamplesVerifyModel ( - const std::set &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const + const std::set &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const { // Needs a valid set of model coefficients if (!isModelValid (model_coefficients)) return (false); // Obtain the line point and direction - Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); - Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); + Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f); + Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f); line_dir.normalize (); double sqr_threshold = threshold * threshold; // Iterate through the 3d points and calculate the distances from them to the line - for (const int &index : indices) + for (const auto &index : indices) { // Calculate the distance from the point to the line // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p2-p0)) / norm(p2-p1) diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_normal_parallel_plane.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_normal_parallel_plane.hpp index bee55e1c..f9066229 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_normal_parallel_plane.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_normal_parallel_plane.hpp @@ -55,7 +55,7 @@ pcl::SampleConsensusModelNormalParallelPlane::isModelValid (con { // Obtain the plane normal Eigen::Vector4f coeff = model_coefficients; - coeff[3] = 0; + coeff[3] = 0.0f; coeff.normalize (); if (std::abs (axis_.dot (coeff)) < cos_angle_) diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_normal_plane.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_normal_plane.hpp index e8ed669a..62fc5c41 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_normal_plane.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_normal_plane.hpp @@ -46,7 +46,7 @@ ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusModelNormalPlane::selectWithinDistance ( - const Eigen::VectorXf &model_coefficients, const double threshold, std::vector &inliers) + const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers) { if (!normals_) { @@ -64,11 +64,12 @@ pcl::SampleConsensusModelNormalPlane::selectWithinDistance ( // Obtain the plane normal Eigen::Vector4f coeff = model_coefficients; - coeff[3] = 0; + coeff[3] = 0.0f; - int nr_p = 0; - inliers.resize (indices_->size ()); - error_sqr_dists_.resize (indices_->size ()); + inliers.clear (); + error_sqr_dists_.clear (); + inliers.reserve (indices_->size ()); + error_sqr_dists_.reserve (indices_->size ()); // Iterate through the 3d points and calculate the distances from them to the plane for (std::size_t i = 0; i < indices_->size (); ++i) @@ -77,8 +78,8 @@ pcl::SampleConsensusModelNormalPlane::selectWithinDistance ( const PointNT &nt = normals_->points[(*indices_)[i]]; // Calculate the distance from the point to the plane normal as the dot product // D = (P-A).N/|N| - Eigen::Vector4f p (pt.x, pt.y, pt.z, 0); - Eigen::Vector4f n (nt.normal_x, nt.normal_y, nt.normal_z, 0); + Eigen::Vector4f p (pt.x, pt.y, pt.z, 0.0f); + Eigen::Vector4f n (nt.normal_x, nt.normal_y, nt.normal_z, 0.0f); double d_euclid = std::abs (coeff.dot (p) + model_coefficients[3]); // Calculate the angular distance between the point normal and the plane normal @@ -92,13 +93,10 @@ pcl::SampleConsensusModelNormalPlane::selectWithinDistance ( if (distance < threshold) { // Returns the indices of the points whose distances are smaller than the threshold - inliers[nr_p] = (*indices_)[i]; - error_sqr_dists_[nr_p] = distance; - ++nr_p; + inliers.push_back ((*indices_)[i]); + error_sqr_dists_.push_back (distance); } } - inliers.resize (nr_p); - error_sqr_dists_.resize (nr_p); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -118,7 +116,7 @@ pcl::SampleConsensusModelNormalPlane::countWithinDistance ( // Obtain the plane normal Eigen::Vector4f coeff = model_coefficients; - coeff[3] = 0; + coeff[3] = 0.0f; std::size_t nr_p = 0; @@ -129,8 +127,8 @@ pcl::SampleConsensusModelNormalPlane::countWithinDistance ( const PointNT &nt = normals_->points[(*indices_)[i]]; // Calculate the distance from the point to the plane normal as the dot product // D = (P-A).N/|N| - Eigen::Vector4f p (pt.x, pt.y, pt.z, 0); - Eigen::Vector4f n (nt.normal_x, nt.normal_y, nt.normal_z, 0); + Eigen::Vector4f p (pt.x, pt.y, pt.z, 0.0f); + Eigen::Vector4f n (nt.normal_x, nt.normal_y, nt.normal_z, 0.0f); double d_euclid = std::abs (coeff.dot (p) + model_coefficients[3]); // Calculate the angular distance between the point normal and the plane normal @@ -166,7 +164,7 @@ pcl::SampleConsensusModelNormalPlane::getDistancesToModel ( // Obtain the plane normal Eigen::Vector4f coeff = model_coefficients; - coeff[3] = 0; + coeff[3] = 0.0f; distances.resize (indices_->size ()); @@ -177,8 +175,8 @@ pcl::SampleConsensusModelNormalPlane::getDistancesToModel ( const PointNT &nt = normals_->points[(*indices_)[i]]; // Calculate the distance from the point to the plane normal as the dot product // D = (P-A).N/|N| - Eigen::Vector4f p (pt.x, pt.y, pt.z, 0); - Eigen::Vector4f n (nt.normal_x, nt.normal_y, nt.normal_z, 0); + Eigen::Vector4f p (pt.x, pt.y, pt.z, 0.0f); + Eigen::Vector4f n (nt.normal_x, nt.normal_y, nt.normal_z, 0.0f); double d_euclid = std::abs (coeff.dot (p) + model_coefficients[3]); // Calculate the angular distance between the point normal and the plane normal diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_normal_sphere.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_normal_sphere.hpp index c33f0649..8b82c62f 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_normal_sphere.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_normal_sphere.hpp @@ -46,7 +46,7 @@ ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusModelNormalSphere::selectWithinDistance ( - const Eigen::VectorXf &model_coefficients, const double threshold, std::vector &inliers) + const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers) { if (!normals_) { @@ -64,11 +64,12 @@ pcl::SampleConsensusModelNormalSphere::selectWithinDistance ( // Obtain the sphere center Eigen::Vector4f center = model_coefficients; - center[3] = 0; + center[3] = 0.0f; - int nr_p = 0; - inliers.resize (indices_->size ()); - error_sqr_dists_.resize (indices_->size ()); + inliers.clear (); + error_sqr_dists_.clear (); + inliers.reserve (indices_->size ()); + error_sqr_dists_.reserve (indices_->size ()); // Iterate through the 3d points and calculate the distances from them to the sphere for (std::size_t i = 0; i < indices_->size (); ++i) @@ -78,12 +79,12 @@ pcl::SampleConsensusModelNormalSphere::selectWithinDistance ( Eigen::Vector4f p (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, - 0); + 0.0f); Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], - 0); + 0.0f); Eigen::Vector4f n_dir = p - center; double d_euclid = std::abs (n_dir.norm () - model_coefficients[3]); @@ -92,17 +93,14 @@ pcl::SampleConsensusModelNormalSphere::selectWithinDistance ( double d_normal = std::abs (getAngle3D (n, n_dir)); d_normal = (std::min) (d_normal, M_PI - d_normal); - double distance = std::abs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid); + double distance = std::abs (normal_distance_weight_ * d_normal + (1.0 - normal_distance_weight_) * d_euclid); if (distance < threshold) { // Returns the indices of the points whose distances are smaller than the threshold - inliers[nr_p] = (*indices_)[i]; - error_sqr_dists_[nr_p] = static_cast (distance); - ++nr_p; + inliers.push_back ((*indices_)[i]); + error_sqr_dists_.push_back (static_cast (distance)); } } - inliers.resize (nr_p); - error_sqr_dists_.resize (nr_p); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -123,7 +121,7 @@ pcl::SampleConsensusModelNormalSphere::countWithinDistance ( // Obtain the sphere centroid Eigen::Vector4f center = model_coefficients; - center[3] = 0; + center[3] = 0.0f; std::size_t nr_p = 0; @@ -135,12 +133,12 @@ pcl::SampleConsensusModelNormalSphere::countWithinDistance ( Eigen::Vector4f p (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, - 0); + 0.0f); Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], - 0); + 0.0f); Eigen::Vector4f n_dir = (p-center); double d_euclid = std::abs (n_dir.norm () - model_coefficients[3]); @@ -149,7 +147,7 @@ pcl::SampleConsensusModelNormalSphere::countWithinDistance ( double d_normal = std::abs (getAngle3D (n, n_dir)); d_normal = (std::min) (d_normal, M_PI - d_normal); - if (std::abs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid) < threshold) + if (std::abs (normal_distance_weight_ * d_normal + (1.0 - normal_distance_weight_) * d_euclid) < threshold) nr_p++; } return (nr_p); @@ -175,7 +173,7 @@ pcl::SampleConsensusModelNormalSphere::getDistancesToModel ( // Obtain the sphere centroid Eigen::Vector4f center = model_coefficients; - center[3] = 0; + center[3] = 0.0f; distances.resize (indices_->size ()); @@ -187,12 +185,12 @@ pcl::SampleConsensusModelNormalSphere::getDistancesToModel ( Eigen::Vector4f p (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, - 0); + 0.0f); Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], - 0); + 0.0f); Eigen::Vector4f n_dir = (p-center); double d_euclid = std::abs (n_dir.norm () - model_coefficients[3]); @@ -205,21 +203,6 @@ pcl::SampleConsensusModelNormalSphere::getDistancesToModel ( } } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -template bool -pcl::SampleConsensusModelNormalSphere::isModelValid (const Eigen::VectorXf &model_coefficients) const -{ - if (!SampleConsensusModel::isModelValid (model_coefficients)) - return (false); - - if (radius_min_ != -std::numeric_limits::max() && model_coefficients[3] < radius_min_) - return (false); - if (radius_max_ != std::numeric_limits::max() && model_coefficients[3] > radius_max_) - return (false); - - return (true); -} - #define PCL_INSTANTIATE_SampleConsensusModelNormalSphere(PointT, PointNT) template class PCL_EXPORTS pcl::SampleConsensusModelNormalSphere; #endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_NORMAL_SPHERE_H_ diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_parallel_line.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_parallel_line.hpp index 1ab1a430..b6d74a46 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_parallel_line.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_parallel_line.hpp @@ -47,7 +47,7 @@ ////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusModelParallelLine::selectWithinDistance ( - const Eigen::VectorXf &model_coefficients, const double threshold, std::vector &inliers) + const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers) { // Check if the model is valid given the user constraints if (!isModelValid (model_coefficients)) @@ -66,7 +66,9 @@ pcl::SampleConsensusModelParallelLine::countWithinDistance ( { // Check if the model is valid given the user constraints if (!isModelValid (model_coefficients)) + { return (0); + } return (SampleConsensusModelLine::countWithinDistance (model_coefficients, threshold)); } @@ -97,14 +99,16 @@ pcl::SampleConsensusModelParallelLine::isModelValid (const Eigen::Vector if (eps_angle_ > 0.0) { // Obtain the line direction - Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); + Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f); - Eigen::Vector4f axis (axis_[0], axis_[1], axis_[2], 0); + Eigen::Vector4f axis (axis_[0], axis_[1], axis_[2], 0.0f); double angle_diff = std::abs (getAngle3D (axis, line_dir)); angle_diff = (std::min) (angle_diff, M_PI - angle_diff); // Check whether the current line model satisfies our angle threshold criterion with respect to the given axis if (angle_diff > eps_angle_) + { return (false); + } } return (true); diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_parallel_plane.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_parallel_plane.hpp index 3de6be6d..e2db1286 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_parallel_plane.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_parallel_plane.hpp @@ -46,7 +46,7 @@ ////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusModelParallelPlane::selectWithinDistance ( - const Eigen::VectorXf &model_coefficients, const double threshold, std::vector &inliers) + const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers) { // Check if the model is valid given the user constraints if (!isModelValid (model_coefficients)) @@ -65,7 +65,9 @@ pcl::SampleConsensusModelParallelPlane::countWithinDistance ( { // Check if the model is valid given the user constraints if (!isModelValid (model_coefficients)) + { return (0); + } return (SampleConsensusModelPlane::countWithinDistance (model_coefficients, threshold)); } @@ -90,19 +92,23 @@ template bool pcl::SampleConsensusModelParallelPlane::isModelValid (const Eigen::VectorXf &model_coefficients) const { if (!SampleConsensusModel::isModelValid (model_coefficients)) + { return (false); + } // Check against template, if given if (eps_angle_ > 0.0) { // Obtain the plane normal Eigen::Vector4f coeff = model_coefficients; - coeff[3] = 0; + coeff[3] = 0.0f; coeff.normalize (); - Eigen::Vector4f axis (axis_[0], axis_[1], axis_[2], 0); + Eigen::Vector4f axis (axis_[0], axis_[1], axis_[2], 0.0f); if (std::abs (axis.dot (coeff)) > sin_angle_) + { return (false); + } } return (true); diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_perpendicular_plane.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_perpendicular_plane.hpp index e7bc7f0e..9f04f431 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_perpendicular_plane.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_perpendicular_plane.hpp @@ -46,7 +46,7 @@ ////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusModelPerpendicularPlane::selectWithinDistance ( - const Eigen::VectorXf &model_coefficients, const double threshold, std::vector &inliers) + const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers) { // Check if the model is valid given the user constraints if (!isModelValid (model_coefficients)) @@ -65,7 +65,9 @@ pcl::SampleConsensusModelPerpendicularPlane::countWithinDistance ( { // Check if the model is valid given the user constraints if (!isModelValid (model_coefficients)) + { return (0); + } return (SampleConsensusModelPlane::countWithinDistance (model_coefficients, threshold)); } @@ -90,21 +92,25 @@ template bool pcl::SampleConsensusModelPerpendicularPlane::isModelValid (const Eigen::VectorXf &model_coefficients) const { if (!SampleConsensusModel::isModelValid (model_coefficients)) + { return (false); + } // Check against template, if given if (eps_angle_ > 0.0) { // Obtain the plane normal Eigen::Vector4f coeff = model_coefficients; - coeff[3] = 0; + coeff[3] = 0.0f; - Eigen::Vector4f axis (axis_[0], axis_[1], axis_[2], 0); + Eigen::Vector4f axis (axis_[0], axis_[1], axis_[2], 0.0f); double angle_diff = std::abs (getAngle3D (axis, coeff)); angle_diff = (std::min) (angle_diff, M_PI - angle_diff); // Check whether the current plane model satisfies our angle threshold criterion with respect to the given axis if (angle_diff > eps_angle_) + { return (false); + } } return (true); diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_plane.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_plane.hpp index 5b99792f..c4efebe9 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_plane.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_plane.hpp @@ -49,11 +49,13 @@ ////////////////////////////////////////////////////////////////////////// template bool -pcl::SampleConsensusModelPlane::isSampleGood (const std::vector &samples) const +pcl::SampleConsensusModelPlane::isSampleGood (const Indices &samples) const { - // Need an extra check in case the sample selection is empty - if (samples.empty ()) + if (samples.size () != sample_size_) + { + PCL_ERROR ("[pcl::SampleConsensusModelPlane::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n", samples.size (), sample_size_); return (false); + } // Get the values at the two points pcl::Array4fMapConst p0 = input_->points[samples[0]].getArray4fMap (); pcl::Array4fMapConst p1 = input_->points[samples[1]].getArray4fMap (); @@ -67,7 +69,7 @@ pcl::SampleConsensusModelPlane::isSampleGood (const std::vector &sa ////////////////////////////////////////////////////////////////////////// template bool pcl::SampleConsensusModelPlane::computeModelCoefficients ( - const std::vector &samples, Eigen::VectorXf &model_coefficients) const + const Indices &samples, Eigen::VectorXf &model_coefficients) const { // Need 3 samples if (samples.size () != sample_size_) @@ -88,21 +90,23 @@ pcl::SampleConsensusModelPlane::computeModelCoefficients ( // Avoid some crashes by checking for collinearity here Eigen::Array4f dy1dy2 = p1p0 / p2p0; if ( (dy1dy2[0] == dy1dy2[1]) && (dy1dy2[2] == dy1dy2[1]) ) // Check for collinearity + { return (false); + } // Compute the plane coefficients from the 3 given points in a straightforward manner // calculate the plane normal n = (p2-p1) x (p3-p1) = cross (p2-p1, p3-p1) - model_coefficients.resize (4); + model_coefficients.resize (model_size_); model_coefficients[0] = p1p0[1] * p2p0[2] - p1p0[2] * p2p0[1]; model_coefficients[1] = p1p0[2] * p2p0[0] - p1p0[0] * p2p0[2]; model_coefficients[2] = p1p0[0] * p2p0[1] - p1p0[1] * p2p0[0]; - model_coefficients[3] = 0; + model_coefficients[3] = 0.0f; // Normalize model_coefficients.normalize (); // ... + d = 0 - model_coefficients[3] = -1 * (model_coefficients.template head<4>().dot (p0.matrix ())); + model_coefficients[3] = -1.0f * (model_coefficients.template head<4>().dot (p0.matrix ())); return (true); } @@ -113,9 +117,9 @@ pcl::SampleConsensusModelPlane::getDistancesToModel ( const Eigen::VectorXf &model_coefficients, std::vector &distances) const { // Needs a valid set of model coefficients - if (model_coefficients.size () != model_size_) + if (!isModelValid (model_coefficients)) { - PCL_ERROR ("[pcl::SampleConsensusModelPlane::getDistancesToModel] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelPlane::getDistancesToModel] Given model is invalid!\n"); return; } @@ -133,7 +137,7 @@ pcl::SampleConsensusModelPlane::getDistancesToModel ( Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, - 1); + 1.0f); distances[i] = std::abs (model_coefficients.dot (pt)); } } @@ -141,18 +145,19 @@ pcl::SampleConsensusModelPlane::getDistancesToModel ( ////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusModelPlane::selectWithinDistance ( - const Eigen::VectorXf &model_coefficients, const double threshold, std::vector &inliers) + const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers) { // Needs a valid set of model coefficients - if (model_coefficients.size () != model_size_) + if (!isModelValid (model_coefficients)) { - PCL_ERROR ("[pcl::SampleConsensusModelPlane::selectWithinDistance] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelPlane::selectWithinDistance] Given model is invalid!\n"); return; } - int nr_p = 0; - inliers.resize (indices_->size ()); - error_sqr_dists_.resize (indices_->size ()); + inliers.clear (); + error_sqr_dists_.clear (); + inliers.reserve (indices_->size ()); + error_sqr_dists_.reserve (indices_->size ()); // Iterate through the 3d points and calculate the distances from them to the plane for (std::size_t i = 0; i < indices_->size (); ++i) @@ -162,20 +167,17 @@ pcl::SampleConsensusModelPlane::selectWithinDistance ( Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, - 1); + 1.0f); float distance = std::abs (model_coefficients.dot (pt)); if (distance < threshold) { // Returns the indices of the points whose distances are smaller than the threshold - inliers[nr_p] = (*indices_)[i]; - error_sqr_dists_[nr_p] = static_cast (distance); - ++nr_p; + inliers.push_back ((*indices_)[i]); + error_sqr_dists_.push_back (static_cast (distance)); } } - inliers.resize (nr_p); - error_sqr_dists_.resize (nr_p); } ////////////////////////////////////////////////////////////////////////// @@ -184,9 +186,9 @@ pcl::SampleConsensusModelPlane::countWithinDistance ( const Eigen::VectorXf &model_coefficients, const double threshold) const { // Needs a valid set of model coefficients - if (model_coefficients.size () != model_size_) + if (!isModelValid (model_coefficients)) { - PCL_ERROR ("[pcl::SampleConsensusModelPlane::countWithinDistance] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelPlane::countWithinDistance] Given model is invalid!\n"); return (0); } @@ -200,9 +202,11 @@ pcl::SampleConsensusModelPlane::countWithinDistance ( Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, - 1); + 1.0f); if (std::abs (model_coefficients.dot (pt)) < threshold) + { nr_p++; + } } return (nr_p); } @@ -210,12 +214,12 @@ pcl::SampleConsensusModelPlane::countWithinDistance ( ////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusModelPlane::optimizeModelCoefficients ( - const std::vector &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const + const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const { // Needs a valid set of model coefficients - if (model_coefficients.size () != model_size_) + if (!isModelValid (model_coefficients)) { - PCL_ERROR ("[pcl::SampleConsensusModelPlane::optimizeModelCoefficients] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelPlane::optimizeModelCoefficients] Given model is invalid!\n"); optimized_coefficients = model_coefficients; return; } @@ -242,12 +246,12 @@ pcl::SampleConsensusModelPlane::optimizeModelCoefficients ( pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector); // Hessian form (D = nc . p_plane (centroid here) + p) - optimized_coefficients.resize (4); + optimized_coefficients.resize (model_size_); optimized_coefficients[0] = eigen_vector [0]; optimized_coefficients[1] = eigen_vector [1]; optimized_coefficients[2] = eigen_vector [2]; - optimized_coefficients[3] = 0; - optimized_coefficients[3] = -1 * optimized_coefficients.dot (xyz_centroid); + optimized_coefficients[3] = 0.0f; + optimized_coefficients[3] = -1.0f * optimized_coefficients.dot (xyz_centroid); // Make sure it results in a valid model if (!isModelValid (optimized_coefficients)) @@ -259,19 +263,19 @@ pcl::SampleConsensusModelPlane::optimizeModelCoefficients ( ////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusModelPlane::projectPoints ( - const std::vector &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields) const + const Indices &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields) const { // Needs a valid set of model coefficients - if (model_coefficients.size () != model_size_) + if (!isModelValid (model_coefficients)) { - PCL_ERROR ("[pcl::SampleConsensusModelPlane::projectPoints] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelPlane::projectPoints] Given model is invalid!\n"); return; } projected_points.header = input_->header; projected_points.is_dense = input_->is_dense; - Eigen::Vector4f mc (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); + Eigen::Vector4f mc (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f); // normalize the vector perpendicular to the plane... mc.normalize (); @@ -296,7 +300,7 @@ pcl::SampleConsensusModelPlane::projectPoints ( pcl::for_each_type (NdConcatenateFunctor (input_->points[i], projected_points.points[i])); // Iterate through the 3d points and calculate the distances from them to the plane - for (const int &inlier : inliers) + for (const auto &inlier : inliers) { // Calculate the distance from the point to the plane Eigen::Vector4f p (input_->points[inlier].x, @@ -320,8 +324,10 @@ pcl::SampleConsensusModelPlane::projectPoints ( using FieldList = typename pcl::traits::fieldList::type; // Iterate over each point for (std::size_t i = 0; i < inliers.size (); ++i) + { // Iterate over each dimension pcl::for_each_type (NdConcatenateFunctor (input_->points[inliers[i]], projected_points.points[i])); + } // Iterate through the 3d points and calculate the distances from them to the plane for (std::size_t i = 0; i < inliers.size (); ++i) @@ -330,7 +336,7 @@ pcl::SampleConsensusModelPlane::projectPoints ( Eigen::Vector4f p (input_->points[inliers[i]].x, input_->points[inliers[i]].y, input_->points[inliers[i]].z, - 1); + 1.0f); // use normalized coefficients to calculate the scalar projection float distance_to_plane = tmp_mc.dot (p); @@ -343,23 +349,25 @@ pcl::SampleConsensusModelPlane::projectPoints ( ////////////////////////////////////////////////////////////////////////// template bool pcl::SampleConsensusModelPlane::doSamplesVerifyModel ( - const std::set &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const + const std::set &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const { // Needs a valid set of model coefficients - if (model_coefficients.size () != model_size_) + if (!isModelValid (model_coefficients)) { - PCL_ERROR ("[pcl::SampleConsensusModelPlane::doSamplesVerifyModel] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelPlane::doSamplesVerifyModel] Given model is invalid!\n"); return (false); } - for (const int &index : indices) + for (const auto &index : indices) { Eigen::Vector4f pt (input_->points[index].x, input_->points[index].y, input_->points[index].z, - 1); + 1.0f); if (std::abs (model_coefficients.dot (pt)) > threshold) + { return (false); + } } return (true); diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_registration.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_registration.hpp index 5b9d1f7d..be1d34b7 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_registration.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_registration.hpp @@ -42,14 +42,18 @@ #define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_REGISTRATION_H_ #include -#include #include #include ////////////////////////////////////////////////////////////////////////// template bool -pcl::SampleConsensusModelRegistration::isSampleGood (const std::vector &samples) const +pcl::SampleConsensusModelRegistration::isSampleGood (const Indices &samples) const { + if (samples.size () != sample_size_) + { + PCL_ERROR ("[pcl::SampleConsensusModelRegistration::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n", samples.size (), sample_size_); + return (false); + } using namespace pcl::common; using namespace pcl::traits; @@ -64,7 +68,7 @@ pcl::SampleConsensusModelRegistration::isSampleGood (const std::vector bool -pcl::SampleConsensusModelRegistration::computeModelCoefficients (const std::vector &samples, Eigen::VectorXf &model_coefficients) const +pcl::SampleConsensusModelRegistration::computeModelCoefficients (const Indices &samples, Eigen::VectorXf &model_coefficients) const { if (!target_) { @@ -72,12 +76,16 @@ pcl::SampleConsensusModelRegistration::computeModelCoefficients (const s return (false); } // Need 3 samples - if (samples.size () != 3) + if (samples.size () != sample_size_) + { return (false); + } - std::vector indices_tgt (3); + Indices indices_tgt (3); for (int i = 0; i < 3; ++i) + { indices_tgt[i] = correspondences_.at (samples[i]); + } estimateRigidTransformationSVD (*input_, samples, *target_, indices_tgt, model_coefficients); return (true); @@ -117,10 +125,10 @@ pcl::SampleConsensusModelRegistration::getDistancesToModel (const Eigen: { Eigen::Vector4f pt_src (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, - input_->points[(*indices_)[i]].z, 1); + input_->points[(*indices_)[i]].z, 1.0f); Eigen::Vector4f pt_tgt (target_->points[(*indices_tgt_)[i]].x, target_->points[(*indices_tgt_)[i]].y, - target_->points[(*indices_tgt_)[i]].z, 1); + target_->points[(*indices_tgt_)[i]].z, 1.0f); Eigen::Vector4f p_tr (transform * pt_src); // Calculate the distance from the transformed point to its correspondence @@ -131,7 +139,7 @@ pcl::SampleConsensusModelRegistration::getDistancesToModel (const Eigen: ////////////////////////////////////////////////////////////////////////// template void -pcl::SampleConsensusModelRegistration::selectWithinDistance (const Eigen::VectorXf &model_coefficients, const double threshold, std::vector &inliers) +pcl::SampleConsensusModelRegistration::selectWithinDistance (const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers) { if (indices_->size () != indices_tgt_->size ()) { @@ -154,9 +162,10 @@ pcl::SampleConsensusModelRegistration::selectWithinDistance (const Eigen return; } - int nr_p = 0; - inliers.resize (indices_->size ()); - error_sqr_dists_.resize (indices_->size ()); + inliers.clear (); + error_sqr_dists_.clear (); + inliers.reserve (indices_->size ()); + error_sqr_dists_.reserve (indices_->size ()); Eigen::Matrix4f transform; transform.row (0).matrix () = model_coefficients.segment<4>(0); @@ -179,13 +188,10 @@ pcl::SampleConsensusModelRegistration::selectWithinDistance (const Eigen // Calculate the distance from the transformed point to its correspondence if (distance < thresh) { - inliers[nr_p] = (*indices_)[i]; - error_sqr_dists_[nr_p] = static_cast (distance); - ++nr_p; + inliers.push_back ((*indices_)[i]); + error_sqr_dists_.push_back (static_cast (distance)); } } - inliers.resize (nr_p); - error_sqr_dists_.resize (nr_p); } ////////////////////////////////////////////////////////////////////////// @@ -208,7 +214,9 @@ pcl::SampleConsensusModelRegistration::countWithinDistance ( // Check if the model is valid given the user constraints if (!isModelValid (model_coefficients)) + { return (0); + } Eigen::Matrix4f transform; transform.row (0).matrix () = model_coefficients.segment<4>(0); @@ -236,7 +244,7 @@ pcl::SampleConsensusModelRegistration::countWithinDistance ( ////////////////////////////////////////////////////////////////////////// template void -pcl::SampleConsensusModelRegistration::optimizeModelCoefficients (const std::vector &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const +pcl::SampleConsensusModelRegistration::optimizeModelCoefficients (const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const { if (indices_->size () != indices_tgt_->size ()) { @@ -252,8 +260,8 @@ pcl::SampleConsensusModelRegistration::optimizeModelCoefficients (const return; } - std::vector indices_src (inliers.size ()); - std::vector indices_tgt (inliers.size ()); + Indices indices_src (inliers.size ()); + Indices indices_tgt (inliers.size ()); for (std::size_t i = 0; i < inliers.size (); ++i) { indices_src[i] = inliers[i]; @@ -267,9 +275,9 @@ pcl::SampleConsensusModelRegistration::optimizeModelCoefficients (const template void pcl::SampleConsensusModelRegistration::estimateRigidTransformationSVD ( const pcl::PointCloud &cloud_src, - const std::vector &indices_src, + const Indices &indices_src, const pcl::PointCloud &cloud_tgt, - const std::vector &indices_tgt, + const Indices &indices_tgt, Eigen::VectorXf &transform) const { transform.resize (16); diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_registration_2d.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_registration_2d.hpp index 0a12544a..fc94efce 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_registration_2d.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_registration_2d.hpp @@ -39,13 +39,17 @@ #define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_REGISTRATION_2D_HPP_ #include -#include #include ////////////////////////////////////////////////////////////////////////// template bool -pcl::SampleConsensusModelRegistration2D::isSampleGood (const std::vector&) const +pcl::SampleConsensusModelRegistration2D::isSampleGood (const Indices&samples) const { + if (samples.size () != sample_size_) + { + PCL_ERROR ("[pcl::SampleConsensusModelRegistration2D::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n", samples.size (), sample_size_); + return (false); + } return (true); //using namespace pcl::common; //using namespace pcl::traits; @@ -89,7 +93,7 @@ pcl::SampleConsensusModelRegistration2D::getDistancesToModel (const Eige { Eigen::Vector4f pt_src (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, - input_->points[(*indices_)[i]].z, 1); + input_->points[(*indices_)[i]].z, 1.0f); Eigen::Vector4f p_tr (transform * pt_src); @@ -97,8 +101,10 @@ pcl::SampleConsensusModelRegistration2D::getDistancesToModel (const Eige Eigen::Vector3f p_tr3 (p_tr[0], p_tr[1], p_tr[2]); Eigen::Vector3f uv (projection_matrix_ * p_tr3); - if (uv[2] < 0) + if (uv[2] < 0.0f) + { continue; + } uv /= uv[2]; @@ -113,7 +119,7 @@ pcl::SampleConsensusModelRegistration2D::getDistancesToModel (const Eige ////////////////////////////////////////////////////////////////////////// template void -pcl::SampleConsensusModelRegistration2D::selectWithinDistance (const Eigen::VectorXf &model_coefficients, const double threshold, std::vector &inliers) +pcl::SampleConsensusModelRegistration2D::selectWithinDistance (const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers) { if (indices_->size () != indices_tgt_->size ()) { @@ -129,9 +135,10 @@ pcl::SampleConsensusModelRegistration2D::selectWithinDistance (const Eig double thresh = threshold * threshold; - int nr_p = 0; - inliers.resize (indices_->size ()); - error_sqr_dists_.resize (indices_->size ()); + inliers.clear (); + error_sqr_dists_.clear (); + inliers.reserve (indices_->size ()); + error_sqr_dists_.reserve (indices_->size ()); Eigen::Matrix4f transform; transform.row (0).matrix () = model_coefficients.segment<4>(0); @@ -143,7 +150,7 @@ pcl::SampleConsensusModelRegistration2D::selectWithinDistance (const Eig { Eigen::Vector4f pt_src (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, - input_->points[(*indices_)[i]].z, 1); + input_->points[(*indices_)[i]].z, 1.0f); Eigen::Vector4f p_tr (transform * pt_src); @@ -151,7 +158,7 @@ pcl::SampleConsensusModelRegistration2D::selectWithinDistance (const Eig Eigen::Vector3f p_tr3 (p_tr[0], p_tr[1], p_tr[2]); Eigen::Vector3f uv (projection_matrix_ * p_tr3); - if (uv[2] < 0) + if (uv[2] < 0.0f) continue; uv /= uv[2]; @@ -164,13 +171,10 @@ pcl::SampleConsensusModelRegistration2D::selectWithinDistance (const Eig // Calculate the distance from the transformed point to its correspondence if (distance < thresh) { - inliers[nr_p] = (*indices_)[i]; - error_sqr_dists_[nr_p] = distance; - ++nr_p; + inliers.push_back ((*indices_)[i]); + error_sqr_dists_.push_back (distance); } } - inliers.resize (nr_p); - error_sqr_dists_.resize (nr_p); } ////////////////////////////////////////////////////////////////////////// @@ -203,7 +207,7 @@ pcl::SampleConsensusModelRegistration2D::countWithinDistance ( { Eigen::Vector4f pt_src (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, - input_->points[(*indices_)[i]].z, 1); + input_->points[(*indices_)[i]].z, 1.0f); Eigen::Vector4f p_tr (transform * pt_src); @@ -211,8 +215,10 @@ pcl::SampleConsensusModelRegistration2D::countWithinDistance ( Eigen::Vector3f p_tr3 (p_tr[0], p_tr[1], p_tr[2]); Eigen::Vector3f uv (projection_matrix_ * p_tr3); - if (uv[2] < 0) + if (uv[2] < 0.0f) + { continue; + } uv /= uv[2]; @@ -221,7 +227,9 @@ pcl::SampleConsensusModelRegistration2D::countWithinDistance ( (uv[0] - target_->points[(*indices_tgt_)[i]].u) + (uv[1] - target_->points[(*indices_tgt_)[i]].v) * (uv[1] - target_->points[(*indices_tgt_)[i]].v)) < thresh) + { ++nr_p; + } } return (nr_p); } diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_sphere.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_sphere.hpp index fb392efc..b18a9a3a 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_sphere.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_sphere.hpp @@ -46,18 +46,23 @@ ////////////////////////////////////////////////////////////////////////// template bool -pcl::SampleConsensusModelSphere::isSampleGood (const std::vector &) const +pcl::SampleConsensusModelSphere::isSampleGood (const Indices &samples) const { + if (samples.size () != sample_size_) + { + PCL_ERROR ("[pcl::SampleConsensusModelSphere::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n", samples.size (), sample_size_); + return (false); + } return (true); } ////////////////////////////////////////////////////////////////////////// template bool pcl::SampleConsensusModelSphere::computeModelCoefficients ( - const std::vector &samples, Eigen::VectorXf &model_coefficients) const + const Indices &samples, Eigen::VectorXf &model_coefficients) const { // Need 4 samples - if (samples.size () != 4) + if (samples.size () != sample_size_) { PCL_ERROR ("[pcl::SampleConsensusModelSphere::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ()); return (false); @@ -73,12 +78,16 @@ pcl::SampleConsensusModelSphere::computeModelCoefficients ( } float m11 = temp.determinant (); if (m11 == 0) + { return (false); // the points don't define a sphere! + } for (int i = 0; i < 4; ++i) + { temp (i, 0) = (input_->points[samples[i]].x) * (input_->points[samples[i]].x) + (input_->points[samples[i]].y) * (input_->points[samples[i]].y) + (input_->points[samples[i]].z) * (input_->points[samples[i]].z); + } float m12 = temp.determinant (); for (int i = 0; i < 4; ++i) @@ -105,7 +114,7 @@ pcl::SampleConsensusModelSphere::computeModelCoefficients ( float m15 = temp.determinant (); // Center (x , y, z) - model_coefficients.resize (4); + model_coefficients.resize (model_size_); model_coefficients[0] = 0.5f * m12 / m11; model_coefficients[1] = 0.5f * m13 / m11; model_coefficients[2] = 0.5f * m14 / m11; @@ -132,6 +141,7 @@ pcl::SampleConsensusModelSphere::getDistancesToModel ( // Iterate through the 3d points and calculate the distances from them to the sphere for (std::size_t i = 0; i < indices_->size (); ++i) + { // Calculate the distance from the point to the sphere as the difference between //dist(point,sphere_origin) and sphere_radius distances[i] = std::abs (std::sqrt ( @@ -144,12 +154,13 @@ pcl::SampleConsensusModelSphere::getDistancesToModel ( ( input_->points[(*indices_)[i]].z - model_coefficients[2] ) * ( input_->points[(*indices_)[i]].z - model_coefficients[2] ) ) - model_coefficients[3]); + } } ////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusModelSphere::selectWithinDistance ( - const Eigen::VectorXf &model_coefficients, const double threshold, std::vector &inliers) + const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers) { // Check if the model is valid given the user constraints if (!isModelValid (model_coefficients)) @@ -158,9 +169,10 @@ pcl::SampleConsensusModelSphere::selectWithinDistance ( return; } - int nr_p = 0; - inliers.resize (indices_->size ()); - error_sqr_dists_.resize (indices_->size ()); + inliers.clear (); + error_sqr_dists_.clear (); + inliers.reserve (indices_->size ()); + error_sqr_dists_.reserve (indices_->size ()); // Iterate through the 3d points and calculate the distances from them to the sphere for (std::size_t i = 0; i < indices_->size (); ++i) @@ -180,13 +192,10 @@ pcl::SampleConsensusModelSphere::selectWithinDistance ( if (distance < threshold) { // Returns the indices of the points whose distances are smaller than the threshold - inliers[nr_p] = (*indices_)[i]; - error_sqr_dists_[nr_p] = static_cast (distance); - ++nr_p; + inliers.push_back ((*indices_)[i]); + error_sqr_dists_.push_back (static_cast (distance)); } } - inliers.resize (nr_p); - error_sqr_dists_.resize (nr_p); } ////////////////////////////////////////////////////////////////////////// @@ -223,21 +232,21 @@ pcl::SampleConsensusModelSphere::countWithinDistance ( ////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusModelSphere::optimizeModelCoefficients ( - const std::vector &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const + const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const { optimized_coefficients = model_coefficients; // Needs a set of valid model coefficients - if (model_coefficients.size () != 4) + if (!isModelValid (model_coefficients)) { - PCL_ERROR ("[pcl::SampleConsensusModelSphere::optimizeModelCoefficients] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelSphere::optimizeModelCoefficients] Given model is invalid!\n"); return; } - // Need at least 4 samples - if (inliers.size () <= 4) + // Need more than the minimum sample size to make a difference + if (inliers.size () <= sample_size_) { - PCL_ERROR ("[pcl::SampleConsensusModelSphere::optimizeModelCoefficients] Not enough inliers found to support a model (%lu)! Returning the same coefficients.\n", inliers.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelSphere::optimizeModelCoefficients] Not enough inliers to refine/optimize the model's coefficients (%lu)! Returning the same coefficients.\n", inliers.size ()); return; } @@ -254,12 +263,12 @@ pcl::SampleConsensusModelSphere::optimizeModelCoefficients ( ////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusModelSphere::projectPoints ( - const std::vector &, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool) const + const Indices &, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool) const { // Needs a valid model coefficients - if (model_coefficients.size () != 4) + if (!isModelValid (model_coefficients)) { - PCL_ERROR ("[pcl::SampleConsensusModelSphere::projectPoints] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelSphere::projectPoints] Given model is invalid!\n"); return; } @@ -277,16 +286,17 @@ pcl::SampleConsensusModelSphere::projectPoints ( ////////////////////////////////////////////////////////////////////////// template bool pcl::SampleConsensusModelSphere::doSamplesVerifyModel ( - const std::set &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const + const std::set &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const { // Needs a valid model coefficients - if (model_coefficients.size () != 4) + if (!isModelValid (model_coefficients)) { - PCL_ERROR ("[pcl::SampleConsensusModelSphere::doSamplesVerifyModel] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelSphere::doSamplesVerifyModel] Given model is invalid!\n"); return (false); } - for (const int &index : indices) + for (const auto &index : indices) + { // Calculate the distance from the point to the sphere as the difference between //dist(point,sphere_origin) and sphere_radius if (std::abs (sqrt ( @@ -297,7 +307,10 @@ pcl::SampleConsensusModelSphere::doSamplesVerifyModel ( ( input_->points[index].z - model_coefficients[2] ) * ( input_->points[index].z - model_coefficients[2] ) ) - model_coefficients[3]) > threshold) + { return (false); + } + } return (true); } diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_stick.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_stick.hpp index 8d030eaf..37655c7c 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_stick.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_stick.hpp @@ -47,15 +47,22 @@ ////////////////////////////////////////////////////////////////////////// template bool -pcl::SampleConsensusModelStick::isSampleGood (const std::vector &samples) const +pcl::SampleConsensusModelStick::isSampleGood (const Indices &samples) const { + if (samples.size () != sample_size_) + { + PCL_ERROR ("[pcl::SampleConsensusModelStick::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n", samples.size (), sample_size_); + return (false); + } if ( (input_->points[samples[0]].x != input_->points[samples[1]].x) && (input_->points[samples[0]].y != input_->points[samples[1]].y) && (input_->points[samples[0]].z != input_->points[samples[1]].z)) + { return (true); + } return (false); } @@ -63,16 +70,16 @@ pcl::SampleConsensusModelStick::isSampleGood (const std::vector &sa ////////////////////////////////////////////////////////////////////////// template bool pcl::SampleConsensusModelStick::computeModelCoefficients ( - const std::vector &samples, Eigen::VectorXf &model_coefficients) const + const Indices &samples, Eigen::VectorXf &model_coefficients) const { // Need 2 samples - if (samples.size () != 2) + if (samples.size () != sample_size_) { PCL_ERROR ("[pcl::SampleConsensusModelStick::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ()); return (false); } - model_coefficients.resize (7); + model_coefficients.resize (model_size_); model_coefficients[0] = input_->points[samples[0]].x; model_coefficients[1] = input_->points[samples[0]].y; model_coefficients[2] = input_->points[samples[0]].z; @@ -98,14 +105,17 @@ pcl::SampleConsensusModelStick::getDistancesToModel ( { // Needs a valid set of model coefficients if (!isModelValid (model_coefficients)) + { + PCL_ERROR ("[pcl::SampleConsensusModelStick::getDistancesToModel] Given model is invalid!\n"); return; + } float sqr_threshold = static_cast (radius_max_ * radius_max_); distances.resize (indices_->size ()); // Obtain the line point and direction - Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); - Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); + Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f); + Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f); line_dir.normalize (); // Iterate through the 3d points and calculate the distances from them to the line @@ -116,32 +126,40 @@ pcl::SampleConsensusModelStick::getDistancesToModel ( float sqr_distance = (line_pt - input_->points[(*indices_)[i]].getVector4fMap ()).cross3 (line_dir).squaredNorm (); if (sqr_distance < sqr_threshold) + { // Need to estimate sqrt here to keep MSAC and friends general distances[i] = sqrt (sqr_distance); + } else + { // Penalize outliers by doubling the distance distances[i] = 2 * sqrt (sqr_distance); + } } } ////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusModelStick::selectWithinDistance ( - const Eigen::VectorXf &model_coefficients, const double threshold, std::vector &inliers) + const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers) { // Needs a valid set of model coefficients if (!isModelValid (model_coefficients)) + { + PCL_ERROR ("[pcl::SampleConsensusModelStick::selectWithinDistance] Given model is invalid!\n"); return; + } float sqr_threshold = static_cast (threshold * threshold); - int nr_p = 0; - inliers.resize (indices_->size ()); - error_sqr_dists_.resize (indices_->size ()); + inliers.clear (); + error_sqr_dists_.clear (); + inliers.reserve (indices_->size ()); + error_sqr_dists_.reserve (indices_->size ()); // Obtain the line point and direction - Eigen::Vector4f line_pt1 (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); - Eigen::Vector4f line_pt2 (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); + Eigen::Vector4f line_pt1 (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f); + Eigen::Vector4f line_pt2 (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f); Eigen::Vector4f line_dir = line_pt2 - line_pt1; //Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); //Eigen::Vector4f line_dir (model_coefficients[3] - model_coefficients[0], model_coefficients[4] - model_coefficients[1], model_coefficients[5] - model_coefficients[2], 0); @@ -164,13 +182,10 @@ pcl::SampleConsensusModelStick::selectWithinDistance ( if (sqr_distance < sqr_threshold) { // Returns the indices of the points whose squared distances are smaller than the threshold - inliers[nr_p] = (*indices_)[i]; - error_sqr_dists_[nr_p] = static_cast (sqr_distance); - ++nr_p; + inliers.push_back ((*indices_)[i]); + error_sqr_dists_.push_back (static_cast (sqr_distance)); } } - inliers.resize (nr_p); - error_sqr_dists_.resize (nr_p); } /////////////////////////////////////////////////////////////////////////// @@ -180,15 +195,18 @@ pcl::SampleConsensusModelStick::countWithinDistance ( { // Needs a valid set of model coefficients if (!isModelValid (model_coefficients)) + { + PCL_ERROR ("[pcl::SampleConsensusModelStick::countWithinDistance] Given model is invalid!\n"); return (0); + } float sqr_threshold = static_cast (threshold * threshold); std::size_t nr_i = 0, nr_o = 0; // Obtain the line point and direction - Eigen::Vector4f line_pt1 (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); - Eigen::Vector4f line_pt2 (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); + Eigen::Vector4f line_pt1 (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f); + Eigen::Vector4f line_pt2 (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f); Eigen::Vector4f line_dir = line_pt2 - line_pt1; line_dir.normalize (); @@ -210,9 +228,13 @@ pcl::SampleConsensusModelStick::countWithinDistance ( float sqr_distance = dir.cross3 (line_dir).squaredNorm (); // Use a larger threshold (4 times the radius) to get more points in if (sqr_distance < sqr_threshold) + { nr_i++; - else if (sqr_distance < 4 * sqr_threshold) + } + else if (sqr_distance < 4.0f * sqr_threshold) + { nr_o++; + } } return (nr_i <= nr_o ? 0 : nr_i - nr_o); @@ -221,7 +243,7 @@ pcl::SampleConsensusModelStick::countWithinDistance ( ////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusModelStick::optimizeModelCoefficients ( - const std::vector &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const + const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const { // Needs a valid set of model coefficients if (!isModelValid (model_coefficients)) @@ -230,15 +252,15 @@ pcl::SampleConsensusModelStick::optimizeModelCoefficients ( return; } - // Need at least 2 points to estimate a line - if (inliers.size () <= 2) + // Need more than the minimum sample size to make a difference + if (inliers.size () <= sample_size_) { - PCL_ERROR ("[pcl::SampleConsensusModelStick::optimizeModelCoefficients] Not enough inliers found to support a model (%lu)! Returning the same coefficients.\n", inliers.size ()); + PCL_ERROR ("[pcl::SampleConsensusModelStick::optimizeModelCoefficients] Not enough inliers to refine/optimize the model's coefficients (%lu)! Returning the same coefficients.\n", inliers.size ()); optimized_coefficients = model_coefficients; return; } - optimized_coefficients.resize (7); + optimized_coefficients.resize (model_size_); // Compute the 3x3 covariance matrix Eigen::Vector4f centroid; @@ -262,15 +284,18 @@ pcl::SampleConsensusModelStick::optimizeModelCoefficients ( ////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusModelStick::projectPoints ( - const std::vector &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields) const + const Indices &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields) const { // Needs a valid model coefficients if (!isModelValid (model_coefficients)) + { + PCL_ERROR ("[pcl::SampleConsensusModelStick::projectPoints] Given model is invalid!\n"); return; + } // Obtain the line point and direction - Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); - Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); + Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f); + Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f); projected_points.header = input_->header; projected_points.is_dense = input_->is_dense; @@ -286,13 +311,15 @@ pcl::SampleConsensusModelStick::projectPoints ( using FieldList = typename pcl::traits::fieldList::type; // Iterate over each point for (std::size_t i = 0; i < projected_points.points.size (); ++i) + { // Iterate over each dimension pcl::for_each_type (NdConcatenateFunctor (input_->points[i], projected_points.points[i])); + } // Iterate through the 3d points and calculate the distances from them to the line - for (const int &inlier : inliers) + for (const auto &inlier : inliers) { - Eigen::Vector4f pt (input_->points[inlier].x, input_->points[inlier].y, input_->points[inlier].z, 0); + Eigen::Vector4f pt (input_->points[inlier].x, input_->points[inlier].y, input_->points[inlier].z, 0.0f); // double k = (DOT_PROD_3D (points[i], p21) - dotA_B) / dotB_B; float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir); @@ -313,13 +340,15 @@ pcl::SampleConsensusModelStick::projectPoints ( using FieldList = typename pcl::traits::fieldList::type; // Iterate over each point for (std::size_t i = 0; i < inliers.size (); ++i) + { // Iterate over each dimension pcl::for_each_type (NdConcatenateFunctor (input_->points[inliers[i]], projected_points.points[i])); + } // Iterate through the 3d points and calculate the distances from them to the line for (std::size_t i = 0; i < inliers.size (); ++i) { - Eigen::Vector4f pt (input_->points[inliers[i]].x, input_->points[inliers[i]].y, input_->points[inliers[i]].z, 0); + Eigen::Vector4f pt (input_->points[inliers[i]].x, input_->points[inliers[i]].y, input_->points[inliers[i]].z, 0.0f); // double k = (DOT_PROD_3D (points[i], p21) - dotA_B) / dotB_B; float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir); @@ -335,26 +364,31 @@ pcl::SampleConsensusModelStick::projectPoints ( ////////////////////////////////////////////////////////////////////////// template bool pcl::SampleConsensusModelStick::doSamplesVerifyModel ( - const std::set &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const + const std::set &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const { // Needs a valid set of model coefficients if (!isModelValid (model_coefficients)) + { + PCL_ERROR ("[pcl::SampleConsensusModelStick::doSamplesVerifyModel] Given model is invalid!\n"); return (false); + } // Obtain the line point and direction - Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); - Eigen::Vector4f line_dir (model_coefficients[3] - model_coefficients[0], model_coefficients[4] - model_coefficients[1], model_coefficients[5] - model_coefficients[2], 0); + Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f); + Eigen::Vector4f line_dir (model_coefficients[3] - model_coefficients[0], model_coefficients[4] - model_coefficients[1], model_coefficients[5] - model_coefficients[2], 0.0f); //Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); line_dir.normalize (); float sqr_threshold = static_cast (threshold * threshold); // Iterate through the 3d points and calculate the distances from them to the line - for (const int &index : indices) + for (const auto &index : indices) { // Calculate the distance from the point to the line // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p2-p0)) / norm(p2-p1) if ((line_pt - input_->points[index].getVector4fMap ()).cross3 (line_dir).squaredNorm () > sqr_threshold) + { return (false); + } } return (true); diff --git a/sample_consensus/include/pcl/sample_consensus/sac.h b/sample_consensus/include/pcl/sample_consensus/sac.h index 4489a88a..c42d3696 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac.h +++ b/sample_consensus/include/pcl/sample_consensus/sac.h @@ -197,7 +197,7 @@ namespace pcl double sigma_sqr = sigma * sigma; unsigned int refine_iterations = 0; bool inlier_changed = false, oscillating = false; - std::vector new_inliers, prev_inliers = inliers_; + Indices new_inliers, prev_inliers = inliers_; std::vector inliers_sizes; Eigen::VectorXf new_model_coefficients = model_coefficients_; do @@ -287,25 +287,25 @@ namespace pcl inline void getRandomSamples (const IndicesPtr &indices, std::size_t nr_samples, - std::set &indices_subset) + std::set &indices_subset) { indices_subset.clear (); while (indices_subset.size () < nr_samples) - //indices_subset.insert ((*indices)[(int) (indices->size () * (rand () / (RAND_MAX + 1.0)))]); - indices_subset.insert ((*indices)[static_cast (static_cast(indices->size ()) * rnd ())]); + //indices_subset.insert ((*indices)[(index_t) (indices->size () * (rand () / (RAND_MAX + 1.0)))]); + indices_subset.insert ((*indices)[static_cast (static_cast(indices->size ()) * rnd ())]); } /** \brief Return the best model found so far. * \param[out] model the resultant model */ inline void - getModel (std::vector &model) const { model = model_; } + getModel (Indices &model) const { model = model_; } /** \brief Return the best set of inliers found so far for this model. * \param[out] inliers the resultant set of inliers */ inline void - getInliers (std::vector &inliers) const { inliers = inliers_; } + getInliers (Indices &inliers) const { inliers = inliers_; } /** \brief Return the model coefficients of the best model found so far. * \param[out] model_coefficients the resultant model coefficients, as documented in \ref sample_consensus @@ -318,10 +318,10 @@ namespace pcl SampleConsensusModelPtr sac_model_; /** \brief The model found after the last computeModel () as point cloud indices. */ - std::vector model_; + Indices model_; /** \brief The indices of the points that were chosen as inliers after the last computeModel () call. */ - std::vector inliers_; + Indices inliers_; /** \brief The coefficients of our model computed directly from the model found. */ Eigen::VectorXf model_coefficients_; diff --git a/sample_consensus/include/pcl/sample_consensus/sac_model.h b/sample_consensus/include/pcl/sample_consensus/sac_model.h index edd75231..655d93e7 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model.h @@ -46,10 +46,12 @@ #include #include +#include #include #include #include #include +#include // for index_t, Indices #include #include @@ -128,10 +130,10 @@ namespace pcl * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) */ SampleConsensusModel (const PointCloudConstPtr &cloud, - const std::vector &indices, + const Indices &indices, bool random = false) : input_ (cloud) - , indices_ (new std::vector (indices)) + , indices_ (new Indices (indices)) , radius_min_ (-std::numeric_limits::max ()) , radius_max_ (std::numeric_limits::max ()) , samples_radius_ (0.) @@ -163,7 +165,7 @@ namespace pcl * \param[out] samples the resultant model samples */ virtual void - getSamples (int &iterations, std::vector &samples) + getSamples (int &iterations, Indices &samples) { // We're assuming that indices_ have already been set in the constructor if (indices_->size () < getSampleSize ()) @@ -206,7 +208,7 @@ namespace pcl * \param[out] model_coefficients the computed model coefficients */ virtual bool - computeModelCoefficients (const std::vector &samples, + computeModelCoefficients (const Indices &samples, Eigen::VectorXf &model_coefficients) const = 0; /** \brief Recompute the model coefficients using the given inlier set @@ -220,7 +222,7 @@ namespace pcl * \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization */ virtual void - optimizeModelCoefficients (const std::vector &inliers, + optimizeModelCoefficients (const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const = 0; @@ -244,7 +246,7 @@ namespace pcl virtual void selectWithinDistance (const Eigen::VectorXf &model_coefficients, const double threshold, - std::vector &inliers) = 0; + Indices &inliers) = 0; /** \brief Count all the points which respect the given model * coefficients as inliers. Pure virtual. @@ -268,7 +270,7 @@ namespace pcl * the point projections on the plane model */ virtual void - projectPoints (const std::vector &inliers, + projectPoints (const Indices &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields = true) const = 0; @@ -282,7 +284,7 @@ namespace pcl * determining the inliers from the outliers */ virtual bool - doSamplesVerifyModel (const std::set &indices, + doSamplesVerifyModel (const std::set &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const = 0; @@ -294,13 +296,13 @@ namespace pcl { input_ = cloud; if (!indices_) - indices_.reset (new std::vector ()); + indices_.reset (new Indices ()); if (indices_->empty ()) { // Prepare a set of indices to be used (entire cloud) indices_->resize (cloud->points.size ()); for (std::size_t i = 0; i < cloud->points.size (); ++i) - (*indices_)[i] = static_cast (i); + (*indices_)[i] = static_cast (i); } shuffled_indices_ = *indices_; } @@ -323,9 +325,9 @@ namespace pcl * \param[out] indices the vector of indices that represents the input data. */ inline void - setIndices (const std::vector &indices) + setIndices (const Indices &indices) { - indices_.reset (new std::vector (indices)); + indices_.reset (new Indices (indices)); shuffled_indices_ = indices; } @@ -441,7 +443,7 @@ namespace pcl * \param[out] sample the set of indices of target_ to analyze */ inline void - drawIndexSample (std::vector &sample) + drawIndexSample (Indices &sample) { std::size_t sample_size = sample.size (); std::size_t index_size = shuffled_indices_.size (); @@ -458,7 +460,7 @@ namespace pcl * \param[out] sample the set of indices of target_ to analyze */ inline void - drawIndexSampleRadius (std::vector &sample) + drawIndexSampleRadius (Indices &sample) { std::size_t sample_size = sample.size (); std::size_t index_size = shuffled_indices_.size (); @@ -466,7 +468,7 @@ namespace pcl std::swap (shuffled_indices_[0], shuffled_indices_[0 + (rnd () % (index_size - 0))]); //const PointT& pt0 = (*input_)[shuffled_indices_[0]]; - std::vector indices; + Indices indices; std::vector sqr_dists; // If indices have been set when the search object was constructed, @@ -506,7 +508,7 @@ namespace pcl { if (model_coefficients.size () != model_size_) { - PCL_ERROR ("[pcl::%s::isModelValid] Invalid number of model coefficients given (%lu)!\n", getClassName ().c_str (), model_coefficients.size ()); + PCL_ERROR ("[pcl::%s::isModelValid] Invalid number of model coefficients given (is %lu, should be %lu)!\n", getClassName ().c_str (), model_coefficients.size (), model_size_); return (false); } return (true); @@ -517,7 +519,7 @@ namespace pcl * \param[in] samples the resultant index samples */ virtual bool - isSampleGood (const std::vector &samples) const = 0; + isSampleGood (const Indices &samples) const = 0; /** \brief The model name. */ std::string model_name_; @@ -543,7 +545,7 @@ namespace pcl SearchPtr samples_radius_search_; /** Data containing a shuffled version of the indices. This is used and modified when drawing samples. */ - std::vector shuffled_indices_; + Indices shuffled_indices_; /** \brief Boost-based random number generator algorithm. */ boost::mt19937 rng_alg_; diff --git a/sample_consensus/include/pcl/sample_consensus/sac_model_circle.h b/sample_consensus/include/pcl/sample_consensus/sac_model_circle.h index b1ef12b0..2f841015 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model_circle.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model_circle.h @@ -91,7 +91,7 @@ namespace pcl * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) */ SampleConsensusModelCircle2D (const PointCloudConstPtr &cloud, - const std::vector &indices, + const Indices &indices, bool random = false) : SampleConsensusModel (cloud, indices, random) { @@ -129,7 +129,7 @@ namespace pcl * \param[out] model_coefficients the resultant model coefficients */ bool - computeModelCoefficients (const std::vector &samples, + computeModelCoefficients (const Indices &samples, Eigen::VectorXf &model_coefficients) const override; /** \brief Compute all distances from the cloud data to a given 2D circle model. @@ -148,7 +148,7 @@ namespace pcl void selectWithinDistance (const Eigen::VectorXf &model_coefficients, const double threshold, - std::vector &inliers) override; + Indices &inliers) override; /** \brief Count all the points which respect the given model coefficients as inliers. * @@ -167,7 +167,7 @@ namespace pcl * \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization */ void - optimizeModelCoefficients (const std::vector &inliers, + optimizeModelCoefficients (const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const override; @@ -178,7 +178,7 @@ namespace pcl * \param[in] copy_data_fields set to true if we need to copy the other data fields */ void - projectPoints (const std::vector &inliers, + projectPoints (const Indices &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields = true) const override; @@ -189,7 +189,7 @@ namespace pcl * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers */ bool - doSamplesVerifyModel (const std::set &indices, + doSamplesVerifyModel (const std::set &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const override; @@ -211,7 +211,7 @@ namespace pcl * \param[in] samples the resultant index samples */ bool - isSampleGood(const std::vector &samples) const override; + isSampleGood(const Indices &samples) const override; private: /** \brief Functor for the optimization function */ @@ -221,7 +221,7 @@ namespace pcl * \param[in] indices the indices of data points to evaluate * \param[in] estimator pointer to the estimator object */ - OptimizationFunctor (const pcl::SampleConsensusModelCircle2D *model, const std::vector& indices) : + OptimizationFunctor (const pcl::SampleConsensusModelCircle2D *model, const Indices& indices) : pcl::Functor (indices.size ()), model_ (model), indices_ (indices) {} /** Cost function to be minimized @@ -245,7 +245,7 @@ namespace pcl } const pcl::SampleConsensusModelCircle2D *model_; - const std::vector &indices_; + const Indices &indices_; }; }; } diff --git a/sample_consensus/include/pcl/sample_consensus/sac_model_circle3d.h b/sample_consensus/include/pcl/sample_consensus/sac_model_circle3d.h index 28d8a335..966abd05 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model_circle3d.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model_circle3d.h @@ -92,7 +92,7 @@ namespace pcl * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) */ SampleConsensusModelCircle3D (const PointCloudConstPtr &cloud, - const std::vector &indices, + const Indices &indices, bool random = false) : SampleConsensusModel (cloud, indices, random) { @@ -130,7 +130,7 @@ namespace pcl * \param[out] model_coefficients the resultant model coefficients */ bool - computeModelCoefficients (const std::vector &samples, + computeModelCoefficients (const Indices &samples, Eigen::VectorXf &model_coefficients) const override; /** \brief Compute all distances from the cloud data to a given 3D circle model. @@ -149,7 +149,7 @@ namespace pcl void selectWithinDistance (const Eigen::VectorXf &model_coefficients, const double threshold, - std::vector &inliers) override; + Indices &inliers) override; /** \brief Count all the points which respect the given model coefficients as inliers. * @@ -168,7 +168,7 @@ namespace pcl * \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization */ void - optimizeModelCoefficients (const std::vector &inliers, + optimizeModelCoefficients (const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const override; @@ -179,7 +179,7 @@ namespace pcl * \param[in] copy_data_fields set to true if we need to copy the other data fields */ void - projectPoints (const std::vector &inliers, + projectPoints (const Indices &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields = true) const override; @@ -190,7 +190,7 @@ namespace pcl * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers */ bool - doSamplesVerifyModel (const std::set &indices, + doSamplesVerifyModel (const std::set &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const override; @@ -212,7 +212,7 @@ namespace pcl * \param[in] samples the resultant index samples */ bool - isSampleGood(const std::vector &samples) const override; + isSampleGood(const Indices &samples) const override; private: /** \brief Functor for the optimization function */ @@ -222,7 +222,7 @@ namespace pcl * \param[in] indices the indices of data points to evaluate * \param[in] estimator pointer to the estimator object */ - OptimizationFunctor (const pcl::SampleConsensusModelCircle3D *model, const std::vector& indices) : + OptimizationFunctor (const pcl::SampleConsensusModelCircle3D *model, const Indices& indices) : pcl::Functor (indices.size ()), model_ (model), indices_ (indices) {} /** Cost function to be minimized @@ -262,7 +262,7 @@ namespace pcl } const pcl::SampleConsensusModelCircle3D *model_; - const std::vector &indices_; + const Indices &indices_; }; }; } diff --git a/sample_consensus/include/pcl/sample_consensus/sac_model_cone.h b/sample_consensus/include/pcl/sample_consensus/sac_model_cone.h index a607e0da..230da26a 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model_cone.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model_cone.h @@ -103,7 +103,7 @@ namespace pcl * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) */ SampleConsensusModelCone (const PointCloudConstPtr &cloud, - const std::vector &indices, + const Indices &indices, bool random = false) : SampleConsensusModel (cloud, indices, random) , SampleConsensusModelFromNormals () @@ -197,7 +197,7 @@ namespace pcl * \param[out] model_coefficients the resultant model coefficients */ bool - computeModelCoefficients (const std::vector &samples, + computeModelCoefficients (const Indices &samples, Eigen::VectorXf &model_coefficients) const override; /** \brief Compute all distances from the cloud data to a given cone model. @@ -216,7 +216,7 @@ namespace pcl void selectWithinDistance (const Eigen::VectorXf &model_coefficients, const double threshold, - std::vector &inliers) override; + Indices &inliers) override; /** \brief Count all the points which respect the given model coefficients as inliers. * @@ -236,7 +236,7 @@ namespace pcl * \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization */ void - optimizeModelCoefficients (const std::vector &inliers, + optimizeModelCoefficients (const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const override; @@ -248,7 +248,7 @@ namespace pcl * \param[in] copy_data_fields set to true if we need to copy the other data fields */ void - projectPoints (const std::vector &inliers, + projectPoints (const Indices &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields = true) const override; @@ -259,7 +259,7 @@ namespace pcl * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers */ bool - doSamplesVerifyModel (const std::set &indices, + doSamplesVerifyModel (const std::set &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const override; @@ -289,7 +289,7 @@ namespace pcl * \param[in] samples the resultant index samples */ bool - isSampleGood (const std::vector &samples) const override; + isSampleGood (const Indices &samples) const override; private: /** \brief The axis along which we need to search for a cone direction. */ @@ -309,7 +309,7 @@ namespace pcl * \param[in] indices the indices of data points to evaluate * \param[in] estimator pointer to the estimator object */ - OptimizationFunctor (const pcl::SampleConsensusModelCone *model, const std::vector& indices) : + OptimizationFunctor (const pcl::SampleConsensusModelCone *model, const Indices& indices) : pcl::Functor (indices.size ()), model_ (model), indices_ (indices) {} /** Cost function to be minimized @@ -348,7 +348,7 @@ namespace pcl } const pcl::SampleConsensusModelCone *model_; - const std::vector &indices_; + const Indices &indices_; }; }; } diff --git a/sample_consensus/include/pcl/sample_consensus/sac_model_cylinder.h b/sample_consensus/include/pcl/sample_consensus/sac_model_cylinder.h index eda0f89f..98800841 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model_cylinder.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model_cylinder.h @@ -101,7 +101,7 @@ namespace pcl * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) */ SampleConsensusModelCylinder (const PointCloudConstPtr &cloud, - const std::vector &indices, + const Indices &indices, bool random = false) : SampleConsensusModel (cloud, indices, random) , SampleConsensusModelFromNormals () @@ -169,7 +169,7 @@ namespace pcl * \param[out] model_coefficients the resultant model coefficients */ bool - computeModelCoefficients (const std::vector &samples, + computeModelCoefficients (const Indices &samples, Eigen::VectorXf &model_coefficients) const override; /** \brief Compute all distances from the cloud data to a given cylinder model. @@ -188,7 +188,7 @@ namespace pcl void selectWithinDistance (const Eigen::VectorXf &model_coefficients, const double threshold, - std::vector &inliers) override; + Indices &inliers) override; /** \brief Count all the points which respect the given model coefficients as inliers. * @@ -207,7 +207,7 @@ namespace pcl * \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization */ void - optimizeModelCoefficients (const std::vector &inliers, + optimizeModelCoefficients (const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const override; @@ -219,7 +219,7 @@ namespace pcl * \param[in] copy_data_fields set to true if we need to copy the other data fields */ void - projectPoints (const std::vector &inliers, + projectPoints (const Indices &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields = true) const override; @@ -230,7 +230,7 @@ namespace pcl * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers */ bool - doSamplesVerifyModel (const std::set &indices, + doSamplesVerifyModel (const std::set &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const override; @@ -288,7 +288,7 @@ namespace pcl * \param[in] samples the resultant index samples */ bool - isSampleGood (const std::vector &samples) const override; + isSampleGood (const Indices &samples) const override; private: /** \brief The axis along which we need to search for a cylinder direction. */ @@ -304,7 +304,7 @@ namespace pcl * \param[in] indices the indices of data points to evaluate * \param[in] estimator pointer to the estimator object */ - OptimizationFunctor (const pcl::SampleConsensusModelCylinder *model, const std::vector& indices) : + OptimizationFunctor (const pcl::SampleConsensusModelCylinder *model, const Indices& indices) : pcl::Functor (indices.size ()), model_ (model), indices_ (indices) {} /** Cost function to be minimized @@ -331,7 +331,7 @@ namespace pcl } const pcl::SampleConsensusModelCylinder *model_; - const std::vector &indices_; + const Indices &indices_; }; }; } diff --git a/sample_consensus/include/pcl/sample_consensus/sac_model_line.h b/sample_consensus/include/pcl/sample_consensus/sac_model_line.h index db66a67b..d948276a 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model_line.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model_line.h @@ -93,7 +93,7 @@ namespace pcl * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) */ SampleConsensusModelLine (const PointCloudConstPtr &cloud, - const std::vector &indices, + const Indices &indices, bool random = false) : SampleConsensusModel (cloud, indices, random) { @@ -112,7 +112,7 @@ namespace pcl * \param[out] model_coefficients the resultant model coefficients */ bool - computeModelCoefficients (const std::vector &samples, + computeModelCoefficients (const Indices &samples, Eigen::VectorXf &model_coefficients) const override; /** \brief Compute all squared distances from the cloud data to a given line model. @@ -131,7 +131,7 @@ namespace pcl void selectWithinDistance (const Eigen::VectorXf &model_coefficients, const double threshold, - std::vector &inliers) override; + Indices &inliers) override; /** \brief Count all the points which respect the given model coefficients as inliers. * @@ -150,7 +150,7 @@ namespace pcl * \param[out] optimized_coefficients the resultant recomputed coefficients after optimization */ void - optimizeModelCoefficients (const std::vector &inliers, + optimizeModelCoefficients (const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const override; @@ -161,7 +161,7 @@ namespace pcl * \param[in] copy_data_fields set to true if we need to copy the other data fields */ void - projectPoints (const std::vector &inliers, + projectPoints (const Indices &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields = true) const override; @@ -172,7 +172,7 @@ namespace pcl * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers */ bool - doSamplesVerifyModel (const std::set &indices, + doSamplesVerifyModel (const std::set &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const override; @@ -189,7 +189,7 @@ namespace pcl * \param[in] samples the resultant index samples */ bool - isSampleGood (const std::vector &samples) const override; + isSampleGood (const Indices &samples) const override; }; } diff --git a/sample_consensus/include/pcl/sample_consensus/sac_model_normal_parallel_plane.h b/sample_consensus/include/pcl/sample_consensus/sac_model_normal_parallel_plane.h index e6944d95..d92cf870 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model_normal_parallel_plane.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model_normal_parallel_plane.h @@ -42,6 +42,7 @@ #include #include +#include #include namespace pcl @@ -126,7 +127,7 @@ namespace pcl * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) */ SampleConsensusModelNormalParallelPlane (const PointCloudConstPtr &cloud, - const std::vector &indices, + const Indices &indices, bool random = false) : SampleConsensusModelNormalPlane (cloud, indices, random) , axis_ (Eigen::Vector4f::Zero ()) diff --git a/sample_consensus/include/pcl/sample_consensus/sac_model_normal_plane.h b/sample_consensus/include/pcl/sample_consensus/sac_model_normal_plane.h index 6ae57f9e..fb532501 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model_normal_plane.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model_normal_plane.h @@ -40,6 +40,7 @@ #pragma once +#include #include #include #include @@ -114,7 +115,7 @@ namespace pcl * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) */ SampleConsensusModelNormalPlane (const PointCloudConstPtr &cloud, - const std::vector &indices, + const Indices &indices, bool random = false) : SampleConsensusModelPlane (cloud, indices, random) , SampleConsensusModelFromNormals () @@ -135,7 +136,7 @@ namespace pcl void selectWithinDistance (const Eigen::VectorXf &model_coefficients, const double threshold, - std::vector &inliers) override; + Indices &inliers) override; /** \brief Count all the points which respect the given model coefficients as inliers. * diff --git a/sample_consensus/include/pcl/sample_consensus/sac_model_normal_sphere.h b/sample_consensus/include/pcl/sample_consensus/sac_model_normal_sphere.h index 91bb0a10..7b149b82 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model_normal_sphere.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model_normal_sphere.h @@ -44,6 +44,7 @@ #include #include #include +#include #include namespace pcl @@ -108,7 +109,7 @@ namespace pcl * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) */ SampleConsensusModelNormalSphere (const PointCloudConstPtr &cloud, - const std::vector &indices, + const Indices &indices, bool random = false) : SampleConsensusModelSphere (cloud, indices, random) , SampleConsensusModelFromNormals () @@ -129,7 +130,7 @@ namespace pcl void selectWithinDistance (const Eigen::VectorXf &model_coefficients, const double threshold, - std::vector &inliers) override; + Indices &inliers) override; /** \brief Count all the points which respect the given model coefficients as inliers. * \param[in] model_coefficients the coefficients of a model that we need to compute distances to @@ -157,13 +158,7 @@ namespace pcl protected: using SampleConsensusModel::sample_size_; using SampleConsensusModel::model_size_; - - /** \brief Check whether a model is valid given the user constraints. - * \param[in] model_coefficients the set of model coefficients - */ - bool - isModelValid (const Eigen::VectorXf &model_coefficients) const override; - + using SampleConsensusModelSphere::isModelValid; }; } diff --git a/sample_consensus/include/pcl/sample_consensus/sac_model_parallel_line.h b/sample_consensus/include/pcl/sample_consensus/sac_model_parallel_line.h index 6e0d18cc..c3cd3deb 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model_parallel_line.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model_parallel_line.h @@ -95,7 +95,7 @@ namespace pcl * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) */ SampleConsensusModelParallelLine (const PointCloudConstPtr &cloud, - const std::vector &indices, + const Indices &indices, bool random = false) : SampleConsensusModelLine (cloud, indices, random) , axis_ (Eigen::Vector3f::Zero ()) @@ -136,7 +136,7 @@ namespace pcl void selectWithinDistance (const Eigen::VectorXf &model_coefficients, const double threshold, - std::vector &inliers) override; + Indices &inliers) override; /** \brief Count all the points which respect the given model coefficients as inliers. * diff --git a/sample_consensus/include/pcl/sample_consensus/sac_model_parallel_plane.h b/sample_consensus/include/pcl/sample_consensus/sac_model_parallel_plane.h index 3697adf0..8d17ad20 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model_parallel_plane.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model_parallel_plane.h @@ -97,7 +97,7 @@ namespace pcl * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) */ SampleConsensusModelParallelPlane (const PointCloudConstPtr &cloud, - const std::vector &indices, + const Indices &indices, bool random = false) : SampleConsensusModelPlane (cloud, indices, random) , axis_ (Eigen::Vector3f::Zero ()) @@ -141,7 +141,7 @@ namespace pcl void selectWithinDistance (const Eigen::VectorXf &model_coefficients, const double threshold, - std::vector &inliers) override; + Indices &inliers) override; /** \brief Count all the points which respect the given model coefficients as inliers. * diff --git a/sample_consensus/include/pcl/sample_consensus/sac_model_perpendicular_plane.h b/sample_consensus/include/pcl/sample_consensus/sac_model_perpendicular_plane.h index a4497072..ebf061b9 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model_perpendicular_plane.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model_perpendicular_plane.h @@ -101,7 +101,7 @@ namespace pcl * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) */ SampleConsensusModelPerpendicularPlane (const PointCloudConstPtr &cloud, - const std::vector &indices, + const Indices &indices, bool random = false) : SampleConsensusModelPlane (cloud, indices, random) , axis_ (Eigen::Vector3f::Zero ()) @@ -144,7 +144,7 @@ namespace pcl void selectWithinDistance (const Eigen::VectorXf &model_coefficients, const double threshold, - std::vector &inliers) override; + Indices &inliers) override; /** \brief Count all the points which respect the given model coefficients as inliers. * diff --git a/sample_consensus/include/pcl/sample_consensus/sac_model_plane.h b/sample_consensus/include/pcl/sample_consensus/sac_model_plane.h index bd447b8e..6bcfedd2 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model_plane.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model_plane.h @@ -166,7 +166,7 @@ namespace pcl * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) */ SampleConsensusModelPlane (const PointCloudConstPtr &cloud, - const std::vector &indices, + const Indices &indices, bool random = false) : SampleConsensusModel (cloud, indices, random) { @@ -185,7 +185,7 @@ namespace pcl * \param[out] model_coefficients the resultant model coefficients */ bool - computeModelCoefficients (const std::vector &samples, + computeModelCoefficients (const Indices &samples, Eigen::VectorXf &model_coefficients) const override; /** \brief Compute all distances from the cloud data to a given plane model. @@ -204,7 +204,7 @@ namespace pcl void selectWithinDistance (const Eigen::VectorXf &model_coefficients, const double threshold, - std::vector &inliers) override; + Indices &inliers) override; /** \brief Count all the points which respect the given model coefficients as inliers. * @@ -223,7 +223,7 @@ namespace pcl * \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization */ void - optimizeModelCoefficients (const std::vector &inliers, + optimizeModelCoefficients (const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const override; @@ -234,7 +234,7 @@ namespace pcl * \param[in] copy_data_fields set to true if we need to copy the other data fields */ void - projectPoints (const std::vector &inliers, + projectPoints (const Indices &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields = true) const override; @@ -245,7 +245,7 @@ namespace pcl * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers */ bool - doSamplesVerifyModel (const std::set &indices, + doSamplesVerifyModel (const std::set &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const override; @@ -263,7 +263,7 @@ namespace pcl * \param[in] samples the resultant index samples */ bool - isSampleGood (const std::vector &samples) const override; + isSampleGood (const Indices &samples) const override; }; } diff --git a/sample_consensus/include/pcl/sample_consensus/sac_model_registration.h b/sample_consensus/include/pcl/sample_consensus/sac_model_registration.h index 04dae537..f649a726 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model_registration.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model_registration.h @@ -40,6 +40,7 @@ #pragma once +#include #include #include #include @@ -95,7 +96,7 @@ namespace pcl * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) */ SampleConsensusModelRegistration (const PointCloudConstPtr &cloud, - const std::vector &indices, + const Indices &indices, bool random = false) : SampleConsensusModel (cloud, indices, random) , target_ () @@ -129,7 +130,7 @@ namespace pcl setInputTarget (const PointCloudConstPtr &target) { target_ = target; - indices_tgt_.reset (new std::vector); + indices_tgt_.reset (new Indices); // Cache the size and fill the target indices int target_size = static_cast (target->size ()); indices_tgt_->resize (target_size); @@ -144,10 +145,10 @@ namespace pcl * \param[in] indices_tgt a vector of point indices to be used from \a target */ inline void - setInputTarget (const PointCloudConstPtr &target, const std::vector &indices_tgt) + setInputTarget (const PointCloudConstPtr &target, const Indices &indices_tgt) { target_ = target; - indices_tgt_.reset (new std::vector (indices_tgt)); + indices_tgt_.reset (new Indices (indices_tgt)); computeOriginalIndexMapping (); } @@ -156,7 +157,7 @@ namespace pcl * \param[out] model_coefficients the resultant model coefficients */ bool - computeModelCoefficients (const std::vector &samples, + computeModelCoefficients (const Indices &samples, Eigen::VectorXf &model_coefficients) const override; /** \brief Compute all distances from the transformed points to their correspondences @@ -175,7 +176,7 @@ namespace pcl void selectWithinDistance (const Eigen::VectorXf &model_coefficients, const double threshold, - std::vector &inliers) override; + Indices &inliers) override; /** \brief Count all the points which respect the given model coefficients as inliers. * @@ -193,19 +194,19 @@ namespace pcl * \param[out] optimized_coefficients the resultant recomputed transformation */ void - optimizeModelCoefficients (const std::vector &inliers, + optimizeModelCoefficients (const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const override; void - projectPoints (const std::vector &, + projectPoints (const Indices &, const Eigen::VectorXf &, PointCloud &, bool = true) const override { }; bool - doSamplesVerifyModel (const std::set &, + doSamplesVerifyModel (const std::set &, const Eigen::VectorXf &, const double) const override { @@ -225,7 +226,7 @@ namespace pcl * \param[in] samples the resultant index samples */ bool - isSampleGood (const std::vector &samples) const override; + isSampleGood (const Indices &samples) const override; /** \brief Computes an "optimal" sample distance threshold based on the * principal directions of the input cloud. @@ -262,7 +263,7 @@ namespace pcl */ inline void computeSampleDistanceThreshold (const PointCloudConstPtr &cloud, - const std::vector &indices) + const Indices &indices) { // Compute the principal directions via PCA Eigen::Vector4f xyz_centroid; @@ -297,9 +298,9 @@ namespace pcl */ void estimateRigidTransformationSVD (const pcl::PointCloud &cloud_src, - const std::vector &indices_src, + const Indices &indices_src, const pcl::PointCloud &cloud_tgt, - const std::vector &indices_tgt, + const Indices &indices_tgt, Eigen::VectorXf &transform) const; /** \brief Compute mappings between original indices of the input_/target_ clouds. */ diff --git a/sample_consensus/include/pcl/sample_consensus/sac_model_registration_2d.h b/sample_consensus/include/pcl/sample_consensus/sac_model_registration_2d.h index 2f508d0b..82f16ade 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model_registration_2d.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model_registration_2d.h @@ -39,6 +39,7 @@ #pragma once #include +#include #include namespace pcl @@ -91,7 +92,7 @@ namespace pcl * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) */ SampleConsensusModelRegistration2D (const PointCloudConstPtr &cloud, - const std::vector &indices, + const Indices &indices, bool random = false) : pcl::SampleConsensusModelRegistration (cloud, indices, random) , projection_matrix_ (Eigen::Matrix3f::Identity ()) @@ -122,7 +123,7 @@ namespace pcl void selectWithinDistance (const Eigen::VectorXf &model_coefficients, const double threshold, - std::vector &inliers); + Indices &inliers); /** \brief Count all the points which respect the given model coefficients as inliers. * @@ -155,7 +156,7 @@ namespace pcl * \param[in] samples the resultant index samples */ bool - isSampleGood (const std::vector &samples) const; + isSampleGood (const Indices &samples) const; /** \brief Computes an "optimal" sample distance threshold based on the * principal directions of the input cloud. @@ -189,7 +190,7 @@ namespace pcl */ inline void computeSampleDistanceThreshold (const PointCloudConstPtr&, - const std::vector&) + const Indices&) { //// Compute the principal directions via PCA //Eigen::Vector4f xyz_centroid; diff --git a/sample_consensus/include/pcl/sample_consensus/sac_model_sphere.h b/sample_consensus/include/pcl/sample_consensus/sac_model_sphere.h index ae00f814..c6f460a4 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model_sphere.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model_sphere.h @@ -92,7 +92,7 @@ namespace pcl * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) */ SampleConsensusModelSphere (const PointCloudConstPtr &cloud, - const std::vector &indices, + const Indices &indices, bool random = false) : SampleConsensusModel (cloud, indices, random) { @@ -131,7 +131,7 @@ namespace pcl * \param[out] model_coefficients the resultant model coefficients */ bool - computeModelCoefficients (const std::vector &samples, + computeModelCoefficients (const Indices &samples, Eigen::VectorXf &model_coefficients) const override; /** \brief Compute all distances from the cloud data to a given sphere model. @@ -150,7 +150,7 @@ namespace pcl void selectWithinDistance (const Eigen::VectorXf &model_coefficients, const double threshold, - std::vector &inliers) override; + Indices &inliers) override; /** \brief Count all the points which respect the given model coefficients as inliers. * @@ -169,7 +169,7 @@ namespace pcl * \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization */ void - optimizeModelCoefficients (const std::vector &inliers, + optimizeModelCoefficients (const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const override; @@ -181,7 +181,7 @@ namespace pcl * \todo implement this. */ void - projectPoints (const std::vector &inliers, + projectPoints (const Indices &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields = true) const override; @@ -192,7 +192,7 @@ namespace pcl * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers */ bool - doSamplesVerifyModel (const std::set &indices, + doSamplesVerifyModel (const std::set &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const override; @@ -225,7 +225,7 @@ namespace pcl * \param[in] samples the resultant index samples */ bool - isSampleGood(const std::vector &samples) const override; + isSampleGood(const Indices &samples) const override; private: struct OptimizationFunctor : pcl::Functor @@ -234,7 +234,7 @@ namespace pcl * \param[in] indices the indices of data points to evaluate * \param[in] estimator pointer to the estimator object */ - OptimizationFunctor (const pcl::SampleConsensusModelSphere *model, const std::vector& indices) : + OptimizationFunctor (const pcl::SampleConsensusModelSphere *model, const Indices& indices) : pcl::Functor (indices.size ()), model_ (model), indices_ (indices) {} /** Cost function to be minimized @@ -261,7 +261,7 @@ namespace pcl } const pcl::SampleConsensusModelSphere *model_; - const std::vector &indices_; + const Indices &indices_; }; }; } diff --git a/sample_consensus/include/pcl/sample_consensus/sac_model_stick.h b/sample_consensus/include/pcl/sample_consensus/sac_model_stick.h index 865997f9..e3821a25 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model_stick.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model_stick.h @@ -98,7 +98,7 @@ namespace pcl * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) */ SampleConsensusModelStick (const PointCloudConstPtr &cloud, - const std::vector &indices, + const Indices &indices, bool random = false) : SampleConsensusModel (cloud, indices, random) { @@ -117,7 +117,7 @@ namespace pcl * \param[out] model_coefficients the resultant model coefficients */ bool - computeModelCoefficients (const std::vector &samples, + computeModelCoefficients (const Indices &samples, Eigen::VectorXf &model_coefficients) const override; /** \brief Compute all squared distances from the cloud data to a given stick model. @@ -136,7 +136,7 @@ namespace pcl void selectWithinDistance (const Eigen::VectorXf &model_coefficients, const double threshold, - std::vector &inliers) override; + Indices &inliers) override; /** \brief Count all the points which respect the given model coefficients as inliers. * @@ -155,7 +155,7 @@ namespace pcl * \param[out] optimized_coefficients the resultant recomputed coefficients after optimization */ void - optimizeModelCoefficients (const std::vector &inliers, + optimizeModelCoefficients (const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const override; @@ -166,7 +166,7 @@ namespace pcl * \param[in] copy_data_fields set to true if we need to copy the other data fields */ void - projectPoints (const std::vector &inliers, + projectPoints (const Indices &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields = true) const override; @@ -177,7 +177,7 @@ namespace pcl * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers */ bool - doSamplesVerifyModel (const std::set &indices, + doSamplesVerifyModel (const std::set &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const override; @@ -194,7 +194,7 @@ namespace pcl * \param[in] samples the resultant index samples */ bool - isSampleGood (const std::vector &samples) const override; + isSampleGood (const Indices &samples) const override; }; } diff --git a/search/include/pcl/search/brute_force.h b/search/include/pcl/search/brute_force.h index ec97f4a6..4104e1fc 100644 --- a/search/include/pcl/search/brute_force.h +++ b/search/include/pcl/search/brute_force.h @@ -53,8 +53,8 @@ namespace pcl using PointCloud = typename Search::PointCloud; using PointCloudConstPtr = typename Search::PointCloudConstPtr; - using IndicesPtr = shared_ptr >; - using IndicesConstPtr = shared_ptr >; + using IndicesPtr = pcl::IndicesPtr; + using IndicesConstPtr = pcl::IndicesConstPtr; using pcl::search::Search::input_; using pcl::search::Search::indices_; @@ -62,10 +62,10 @@ namespace pcl struct Entry { - Entry (int idx, float dist) : index (idx), distance (dist) {} + Entry (index_t idx, float dist) : index (idx), distance (dist) {} Entry () : index (0), distance (0) {} - unsigned index; + index_t index; float distance; inline bool @@ -104,7 +104,7 @@ namespace pcl * \return number of neighbors found */ int - nearestKSearch (const PointT &point, int k, std::vector &k_indices, std::vector &k_distances) const override; + nearestKSearch (const PointT &point, int k, Indices &k_indices, std::vector &k_distances) const override; /** \brief Search for all the nearest neighbors of the query point in a given radius. * \param[in] point the given query point @@ -118,24 +118,24 @@ namespace pcl */ int radiusSearch (const PointT& point, double radius, - std::vector &k_indices, std::vector &k_sqr_distances, + Indices &k_indices, std::vector &k_sqr_distances, unsigned int max_nn = 0) const override; private: int - denseKSearch (const PointT &point, int k, std::vector &k_indices, std::vector &k_distances) const; + denseKSearch (const PointT &point, int k, Indices &k_indices, std::vector &k_distances) const; int - sparseKSearch (const PointT &point, int k, std::vector &k_indices, std::vector &k_distances) const; + sparseKSearch (const PointT &point, int k, Indices &k_indices, std::vector &k_distances) const; int denseRadiusSearch (const PointT& point, double radius, - std::vector &k_indices, std::vector &k_sqr_distances, + Indices &k_indices, std::vector &k_sqr_distances, unsigned int max_nn = 0) const; int sparseRadiusSearch (const PointT& point, double radius, - std::vector &k_indices, std::vector &k_sqr_distances, + Indices &k_indices, std::vector &k_sqr_distances, unsigned int max_nn = 0) const; }; } diff --git a/search/include/pcl/search/flann_search.h b/search/include/pcl/search/flann_search.h index 95f61056..80b448c4 100644 --- a/search/include/pcl/search/flann_search.h +++ b/search/include/pcl/search/flann_search.h @@ -87,9 +87,9 @@ namespace pcl * search.setInputCloud (target); * * // Do search - * std::vector > k_indices; + * std::vector k_indices; * std::vector > k_sqr_distances; - * search.nearestKSearch (*query, std::vector (), 2, k_indices, k_sqr_distances); + * search.nearestKSearch (*query, Indices (), 2, k_indices, k_sqr_distances); * \endcode * * \author Andreas Muetzel @@ -110,9 +110,6 @@ namespace pcl using PointCloud = typename Search::PointCloud; using PointCloudConstPtr = typename Search::PointCloudConstPtr; - using typename Search::IndicesPtr; - using typename Search::IndicesConstPtr; - using MatrixPtr = shared_ptr >; using MatrixConstPtr = shared_ptr >; @@ -260,6 +257,8 @@ namespace pcl void setInputCloud (const PointCloudConstPtr& cloud, const IndicesConstPtr& indices = IndicesConstPtr ()) override; + using Search::nearestKSearch; + /** \brief Search for the k-nearest neighbors for the given query point. * \param[in] point the given query point * \param[in] k the number of neighbors to search for @@ -269,7 +268,7 @@ namespace pcl * \return number of neighbors found */ int - nearestKSearch (const PointT &point, int k, std::vector &k_indices, std::vector &k_sqr_distances) const override; + nearestKSearch (const PointT &point, int k, Indices &k_indices, std::vector &k_sqr_distances) const override; /** \brief Search for the k-nearest neighbors for the given query point. @@ -280,8 +279,8 @@ namespace pcl * \param[out] k_sqr_distances the resultant squared distances to the neighboring points, k_sqr_distances[i] corresponds to the neighbors of the query point i */ void - nearestKSearch (const PointCloud& cloud, const std::vector& indices, int k, - std::vector< std::vector >& k_indices, std::vector< std::vector >& k_sqr_distances) const override; + nearestKSearch (const PointCloud& cloud, const Indices& indices, int k, + std::vector& k_indices, std::vector< std::vector >& k_sqr_distances) const override; /** \brief Search for all the nearest neighbors of the query point in a given radius. * \param[in] point the given query point @@ -295,7 +294,7 @@ namespace pcl */ int radiusSearch (const PointT& point, double radius, - std::vector &k_indices, std::vector &k_sqr_distances, + Indices &k_indices, std::vector &k_sqr_distances, unsigned int max_nn = 0) const override; /** \brief Search for the k-nearest neighbors for the given query point. @@ -307,7 +306,7 @@ namespace pcl * \param[in] max_nn if given, bounds the maximum returned neighbors to this value */ void - radiusSearch (const PointCloud& cloud, const std::vector& indices, double radius, std::vector< std::vector >& k_indices, + radiusSearch (const PointCloud& cloud, const Indices& indices, double radius, std::vector& k_indices, std::vector< std::vector >& k_sqr_distances, unsigned int max_nn=0) const override; /** \brief Provide a pointer to the point representation to use to convert points into k-D vectors. @@ -361,7 +360,7 @@ namespace pcl int dim_; - std::vector index_mapping_; + Indices index_mapping_; bool identity_mapping_; }; diff --git a/search/include/pcl/search/impl/brute_force.hpp b/search/include/pcl/search/impl/brute_force.hpp index 118d238c..8461e594 100644 --- a/search/include/pcl/search/impl/brute_force.hpp +++ b/search/include/pcl/search/impl/brute_force.hpp @@ -35,9 +35,9 @@ * */ -#ifndef PCL_SEARCH_IMPL_BRUTE_FORCE_SEARCH_H_ -#define PCL_SEARCH_IMPL_BRUTE_FORCE_SEARCH_H_ +#pragma once +#include // for pcl::isFinite #include #include @@ -52,7 +52,7 @@ pcl::search::BruteForce::getDistSqr ( ////////////////////////////////////////////////////////////////////////////////////////////// template int pcl::search::BruteForce::nearestKSearch ( - const PointT& point, int k, std::vector& k_indices, std::vector& k_distances) const + const PointT& point, int k, Indices& k_indices, std::vector& k_distances) const { assert (isFinite (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"); @@ -69,7 +69,7 @@ pcl::search::BruteForce::nearestKSearch ( ////////////////////////////////////////////////////////////////////////////////////////////// template int pcl::search::BruteForce::denseKSearch ( - const PointT &point, int k, std::vector &k_indices, std::vector &k_distances) const + const PointT &point, int k, Indices &k_indices, std::vector &k_distances) const { // container for first k elements -> O(1) for insertion, since order not required here std::vector result; @@ -77,8 +77,8 @@ pcl::search::BruteForce::denseKSearch ( std::priority_queue queue; if (indices_) { - std::vector::const_iterator iIt =indices_->begin (); - std::vector::const_iterator iEnd = indices_->begin () + std::min (static_cast (k), static_cast (indices_->size ())); + auto iIt = indices_->cbegin (); + auto iEnd = indices_->cbegin () + std::min (static_cast (k), static_cast (indices_->size ())); for (; iIt != iEnd; ++iIt) result.push_back (Entry (*iIt, getDistSqr (input_->points[*iIt], point))); @@ -86,7 +86,7 @@ pcl::search::BruteForce::denseKSearch ( // add the rest Entry entry; - for (; iIt != indices_->end (); ++iIt) + for (; iIt != indices_->cend (); ++iIt) { entry.distance = getDistSqr (input_->points[*iIt], point); if (queue.top ().distance > entry.distance) @@ -100,7 +100,7 @@ pcl::search::BruteForce::denseKSearch ( else { Entry entry; - for (entry.index = 0; entry.index < std::min (static_cast (k), static_cast (input_->size ())); ++entry.index) + for (entry.index = 0; entry.index < std::min (k, input_->size ()); ++entry.index) { entry.distance = getDistSqr (input_->points[entry.index], point); result.push_back (entry); @@ -109,7 +109,7 @@ pcl::search::BruteForce::denseKSearch ( queue = std::priority_queue (result.begin (), result.end ()); // add the rest - for (; entry.index < input_->size (); ++entry.index) + for (; entry.index < static_cast(input_->size ()); ++entry.index) { entry.distance = getDistSqr (input_->points[entry.index], point); if (queue.top ().distance > entry.distance) @@ -137,7 +137,7 @@ pcl::search::BruteForce::denseKSearch ( ////////////////////////////////////////////////////////////////////////////////////////////// template int pcl::search::BruteForce::sparseKSearch ( - const PointT &point, int k, std::vector &k_indices, std::vector &k_distances) const + const PointT &point, int k, Indices &k_indices, std::vector &k_distances) const { // result used to collect the first k neighbors -> unordered std::vector result; @@ -146,8 +146,8 @@ pcl::search::BruteForce::sparseKSearch ( std::priority_queue queue; if (indices_) { - std::vector::const_iterator iIt =indices_->begin (); - for (; iIt != indices_->end () && result.size () < static_cast (k); ++iIt) + auto iIt =indices_->cbegin (); + for (; iIt != indices_->cend () && result.size () < static_cast (k); ++iIt) { if (std::isfinite (input_->points[*iIt].x)) result.push_back (Entry (*iIt, getDistSqr (input_->points[*iIt], point))); @@ -158,7 +158,7 @@ pcl::search::BruteForce::sparseKSearch ( // either we have k elements, or there are none left to iterate >in either case we're fine // add the rest Entry entry; - for (; iIt != indices_->end (); ++iIt) + for (; iIt != indices_->cend (); ++iIt) { if (!std::isfinite (input_->points[*iIt].x)) continue; @@ -175,7 +175,7 @@ pcl::search::BruteForce::sparseKSearch ( else { Entry entry; - for (entry.index = 0; entry.index < input_->size () && result.size () < static_cast (k); ++entry.index) + for (entry.index = 0; (entry.index < static_cast(input_->size ())) && (result.size () < static_cast (k)); ++entry.index) { if (std::isfinite (input_->points[entry.index].x)) { @@ -186,7 +186,7 @@ pcl::search::BruteForce::sparseKSearch ( queue = std::priority_queue (result.begin (), result.end ()); // add the rest - for (; entry.index < input_->size (); ++entry.index) + for (; entry.index < static_cast(input_->size ()); ++entry.index) { if (!std::isfinite (input_->points[entry.index].x)) continue; @@ -217,7 +217,7 @@ pcl::search::BruteForce::sparseKSearch ( template int pcl::search::BruteForce::denseRadiusSearch ( const PointT& point, double radius, - std::vector &k_indices, std::vector &k_sqr_distances, + Indices &k_indices, std::vector &k_sqr_distances, unsigned int max_nn) const { radius *= radius; @@ -235,12 +235,12 @@ pcl::search::BruteForce::denseRadiusSearch ( float distance; if (indices_) { - for (std::vector::const_iterator iIt =indices_->begin (); iIt != indices_->end (); ++iIt) + for (const auto& idx : *indices_) { - distance = getDistSqr (input_->points[*iIt], point); + distance = getDistSqr (input_->points[idx], point); if (distance <= radius) { - k_indices.push_back (*iIt); + k_indices.push_back (idx); k_sqr_distances.push_back (distance); if (k_indices.size () == max_nn) // max_nn = 0 -> never true break; @@ -272,7 +272,7 @@ pcl::search::BruteForce::denseRadiusSearch ( template int pcl::search::BruteForce::sparseRadiusSearch ( const PointT& point, double radius, - std::vector &k_indices, std::vector &k_sqr_distances, + Indices &k_indices, std::vector &k_sqr_distances, unsigned int max_nn) const { radius *= radius; @@ -291,15 +291,15 @@ pcl::search::BruteForce::sparseRadiusSearch ( float distance; if (indices_) { - for (std::vector::const_iterator iIt =indices_->begin (); iIt != indices_->end (); ++iIt) + for (const auto& idx : *indices_) { - if (!std::isfinite (input_->points[*iIt].x)) + if (!std::isfinite (input_->points[idx].x)) continue; - distance = getDistSqr (input_->points[*iIt], point); + distance = getDistSqr (input_->points[idx], point); if (distance <= radius) { - k_indices.push_back (*iIt); + k_indices.push_back (idx); k_sqr_distances.push_back (distance); if (k_indices.size () == max_nn) // never true if max_nn = 0 break; @@ -332,7 +332,7 @@ pcl::search::BruteForce::sparseRadiusSearch ( ////////////////////////////////////////////////////////////////////////////////////////////// template int pcl::search::BruteForce::radiusSearch ( - const PointT& point, double radius, std::vector &k_indices, + const PointT& point, double radius, Indices &k_indices, std::vector &k_sqr_distances, unsigned int max_nn) const { assert (isFinite (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"); @@ -348,5 +348,3 @@ pcl::search::BruteForce::radiusSearch ( } #define PCL_INSTANTIATE_BruteForce(T) template class PCL_EXPORTS pcl::search::BruteForce; - -#endif //PCL_SEARCH_IMPL_BRUTE_FORCE_SEARCH_H_ diff --git a/search/include/pcl/search/impl/flann_search.hpp b/search/include/pcl/search/impl/flann_search.hpp index baf01e1e..1b38f6cf 100644 --- a/search/include/pcl/search/impl/flann_search.hpp +++ b/search/include/pcl/search/impl/flann_search.hpp @@ -100,7 +100,7 @@ pcl::search::FlannSearch::setInputCloud (const PointCloud ////////////////////////////////////////////////////////////////////////////////////////////// template int -pcl::search::FlannSearch::nearestKSearch (const PointT &point, int k, std::vector &indices, std::vector &dists) const +pcl::search::FlannSearch::nearestKSearch (const PointT &point, int k, Indices &indices, std::vector &dists) const { assert (point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"); // remove this check as soon as FLANN does NaN checks internally bool can_cast = point_representation_->isTrivial (); @@ -123,7 +123,7 @@ pcl::search::FlannSearch::nearestKSearch (const PointT &p indices.resize (k,-1); if (dists.size() != static_cast (k)) dists.resize (k); - flann::Matrix i (&indices[0],1,k); + flann::Matrix i (&indices[0],1,k); flann::Matrix d (&dists[0],1,k); int result = index_->knnSearch (m,i,d,k, p); @@ -133,7 +133,7 @@ pcl::search::FlannSearch::nearestKSearch (const PointT &p { for (std::size_t i = 0; i < static_cast (k); ++i) { - int& neighbor_index = indices[i]; + auto& neighbor_index = indices[i]; neighbor_index = index_mapping_[neighbor_index]; } } @@ -143,7 +143,7 @@ pcl::search::FlannSearch::nearestKSearch (const PointT &p ////////////////////////////////////////////////////////////////////////////////////////////// template void pcl::search::FlannSearch::nearestKSearch ( - const PointCloud& cloud, const std::vector& indices, int k, std::vector< std::vector >& k_indices, + const PointCloud& cloud, const Indices& indices, int k, std::vector& k_indices, std::vector< std::vector >& k_sqr_distances) const { if (indices.empty ()) @@ -219,7 +219,7 @@ pcl::search::FlannSearch::nearestKSearch ( { for (auto &k_index : k_indices) { - for (int &neighbor_index : k_index) + for (auto &neighbor_index : k_index) { neighbor_index = index_mapping_[neighbor_index]; } @@ -230,7 +230,7 @@ pcl::search::FlannSearch::nearestKSearch ( ////////////////////////////////////////////////////////////////////////////////////////////// template int pcl::search::FlannSearch::radiusSearch (const PointT& point, double radius, - std::vector &indices, std::vector &distances, + Indices &indices, std::vector &distances, unsigned int max_nn) const { assert (point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to radiusSearch!"); // remove this check as soon as FLANN does NaN checks internally @@ -251,7 +251,7 @@ pcl::search::FlannSearch::radiusSearch (const PointT& poi p.eps = eps_; p.max_neighbors = max_nn > 0 ? max_nn : -1; p.checks = checks_; - std::vector > i (1); + std::vector i (1); std::vector > d (1); int result = index_->radiusSearch (m,i,d,static_cast (radius * radius), p); @@ -261,7 +261,7 @@ pcl::search::FlannSearch::radiusSearch (const PointT& poi if (!identity_mapping_) { - for (int &neighbor_index : indices) + for (auto &neighbor_index : indices) { neighbor_index = index_mapping_ [neighbor_index]; } @@ -272,7 +272,7 @@ pcl::search::FlannSearch::radiusSearch (const PointT& poi ////////////////////////////////////////////////////////////////////////////////////////////// template void pcl::search::FlannSearch::radiusSearch ( - const PointCloud& cloud, const std::vector& indices, double radius, std::vector< std::vector >& k_indices, + const PointCloud& cloud, const Indices& indices, double radius, std::vector& k_indices, std::vector< std::vector >& k_sqr_distances, unsigned int max_nn) const { if (indices.empty ()) // full point cloud + trivial copy operation = no need to do any conversion/copying to the flann matrix! @@ -349,7 +349,7 @@ pcl::search::FlannSearch::radiusSearch ( { for (auto &k_index : k_indices) { - for (int &neighbor_index : k_index) + for (auto &neighbor_index : k_index) { neighbor_index = index_mapping_[neighbor_index]; } @@ -396,7 +396,7 @@ pcl::search::FlannSearch::convertInputToFlannMatrix () continue; } - index_mapping_.push_back (static_cast (i)); // If the returned index should be for the indices vector + index_mapping_.push_back (static_cast (i)); // If the returned index should be for the indices vector point_representation_->vectorize (point, cloud_ptr); cloud_ptr += dim_; @@ -410,7 +410,7 @@ pcl::search::FlannSearch::convertInputToFlannMatrix () float* cloud_ptr = input_flann_->ptr(); for (std::size_t indices_index = 0; indices_index < original_no_of_points; ++indices_index) { - int cloud_index = (*indices_)[indices_index]; + index_t cloud_index = (*indices_)[indices_index]; const PointT& point = (*input_)[cloud_index]; // Check if the point is invalid if (!point_representation_->isValid (point)) @@ -419,7 +419,7 @@ pcl::search::FlannSearch::convertInputToFlannMatrix () continue; } - index_mapping_.push_back (static_cast (indices_index)); // If the returned index should be for the indices vector + index_mapping_.push_back (static_cast (indices_index)); // If the returned index should be for the indices vector point_representation_->vectorize (point, cloud_ptr); cloud_ptr += dim_; diff --git a/search/include/pcl/search/impl/kdtree.hpp b/search/include/pcl/search/impl/kdtree.hpp index a6d7fb22..1b79c67d 100644 --- a/search/include/pcl/search/impl/kdtree.hpp +++ b/search/include/pcl/search/impl/kdtree.hpp @@ -86,7 +86,7 @@ pcl::search::KdTree::setInputCloud ( /////////////////////////////////////////////////////////////////////////////////////////// template int pcl::search::KdTree::nearestKSearch ( - const PointT &point, int k, std::vector &k_indices, + const PointT &point, int k, Indices &k_indices, std::vector &k_sqr_distances) const { return (tree_->nearestKSearch (point, k, k_indices, k_sqr_distances)); @@ -96,7 +96,7 @@ pcl::search::KdTree::nearestKSearch ( template int pcl::search::KdTree::radiusSearch ( const PointT& point, double radius, - std::vector &k_indices, std::vector &k_sqr_distances, + Indices &k_indices, std::vector &k_sqr_distances, unsigned int max_nn) const { return (tree_->radiusSearch (point, radius, k_indices, k_sqr_distances, max_nn)); diff --git a/search/include/pcl/search/impl/organized.hpp b/search/include/pcl/search/impl/organized.hpp index 85438a30..173d7f1e 100644 --- a/search/include/pcl/search/impl/organized.hpp +++ b/search/include/pcl/search/impl/organized.hpp @@ -37,11 +37,11 @@ * */ -#ifndef PCL_SEARCH_IMPL_ORGANIZED_NEIGHBOR_SEARCH_H_ -#define PCL_SEARCH_IMPL_ORGANIZED_NEIGHBOR_SEARCH_H_ +#pragma once #include #include +#include // for pcl::isFinite #include #include @@ -49,7 +49,7 @@ template int pcl::search::OrganizedNeighbor::radiusSearch (const PointT &query, const double radius, - std::vector &k_indices, + Indices &k_indices, std::vector &k_sqr_distances, unsigned int max_nn) const { @@ -116,7 +116,7 @@ pcl::search::OrganizedNeighbor::radiusSearch (const PointT template int pcl::search::OrganizedNeighbor::nearestKSearch (const PointT &query, int k, - std::vector &k_indices, + Indices &k_indices, std::vector &k_sqr_distances) const { assert (isFinite (query) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"); @@ -195,8 +195,8 @@ pcl::search::OrganizedNeighbor::nearestKSearch (const PointT &query, // if upper line of the rectangle is visible and x-extend is not 0 if (yBegin >= 0 && yBegin < static_cast (input_->height)) { - int idx = yBegin * input_->width + xFrom; - int idxTo = idx + xTo - xFrom; + index_t idx = yBegin * input_->width + xFrom; + index_t idxTo = idx + xTo - xFrom; for (; idx < idxTo; ++idx) stop = testPoint (query, k, results, idx) || stop; } @@ -206,8 +206,8 @@ pcl::search::OrganizedNeighbor::nearestKSearch (const PointT &query, // if lower line of the rectangle is visible if (yEnd > 0 && yEnd <= static_cast (input_->height)) { - int idx = (yEnd - 1) * input_->width + xFrom; - int idxTo = idx + xTo - xFrom; + index_t idx = (yEnd - 1) * input_->width + xFrom; + index_t idxTo = idx + xTo - xFrom; for (; idx < idxTo; ++idx) stop = testPoint (query, k, results, idx) || stop; @@ -223,8 +223,8 @@ pcl::search::OrganizedNeighbor::nearestKSearch (const PointT &query, { if (xBegin >= 0 && xBegin < static_cast (input_->width)) { - int idx = yFrom * input_->width + xBegin; - int idxTo = yTo * input_->width + xBegin; + index_t idx = yFrom * input_->width + xBegin; + index_t idxTo = yTo * input_->width + xBegin; for (; idx < idxTo; idx += input_->width) stop = testPoint (query, k, results, idx) || stop; @@ -232,8 +232,8 @@ pcl::search::OrganizedNeighbor::nearestKSearch (const PointT &query, if (xEnd > 0 && xEnd <= static_cast (input_->width)) { - int idx = yFrom * input_->width + xEnd - 1; - int idxTo = yTo * input_->width + xEnd - 1; + index_t idx = yFrom * input_->width + xEnd - 1; + index_t idxTo = yTo * input_->width + xEnd - 1; for (; idx < idxTo; idx += input_->width) stop = testPoint (query, k, results, idx) || stop; @@ -347,7 +347,7 @@ pcl::search::OrganizedNeighbor::estimateProjectionMatrix () const unsigned ySkip = (std::max) (input_->height >> pyramid_level_, unsigned (1)); const unsigned xSkip = (std::max) (input_->width >> pyramid_level_, unsigned (1)); - std::vector indices; + Indices indices; indices.reserve (input_->size () >> (pyramid_level_ << 1)); for (unsigned yIdx = 0, idx = 0; yIdx < input_->height; yIdx += ySkip, idx += input_->width * ySkip) @@ -365,7 +365,7 @@ pcl::search::OrganizedNeighbor::estimateProjectionMatrix () if (std::abs (residual_sqr) > eps_ * float (indices.size ())) { - PCL_ERROR ("[pcl::%s::radiusSearch] Input dataset is not from a projective device!\nResidual (MSE) %f, using %d valid points\n", this->getName ().c_str (), residual_sqr / double (indices.size()), indices.size ()); + PCL_ERROR ("[pcl::%s::estimateProjectionMatrix] Input dataset is not from a projective device!\nResidual (MSE) %f, using %d valid points\n", this->getName ().c_str (), residual_sqr / double (indices.size()), indices.size ()); return; } @@ -388,4 +388,3 @@ pcl::search::OrganizedNeighbor::projectPoint (const PointT& point, pcl:: } #define PCL_INSTANTIATE_OrganizedNeighbor(T) template class PCL_EXPORTS pcl::search::OrganizedNeighbor; -#endif diff --git a/search/include/pcl/search/impl/search.hpp b/search/include/pcl/search/impl/search.hpp index dbb68804..600accf5 100644 --- a/search/include/pcl/search/impl/search.hpp +++ b/search/include/pcl/search/impl/search.hpp @@ -83,27 +83,27 @@ pcl::search::Search::setInputCloud ( /////////////////////////////////////////////////////////////////////////////////////////// template int pcl::search::Search::nearestKSearch ( - const PointCloud &cloud, int index, int k, - std::vector &k_indices, std::vector &k_sqr_distances) const + const PointCloud &cloud, index_t index, int k, + Indices &k_indices, std::vector &k_sqr_distances) const { - assert (index >= 0 && index < static_cast (cloud.points.size ()) && "Out-of-bounds error in nearestKSearch!"); + assert (index >= 0 && index < static_cast (cloud.points.size ()) && "Out-of-bounds error in nearestKSearch!"); return (nearestKSearch (cloud.points[index], k, k_indices, k_sqr_distances)); } /////////////////////////////////////////////////////////////////////////////////////////// template int pcl::search::Search::nearestKSearch ( - int index, int k, - std::vector &k_indices, + index_t index, int k, + Indices &k_indices, std::vector &k_sqr_distances) const { if (!indices_) { - assert (index >= 0 && index < static_cast (input_->points.size ()) && "Out-of-bounds error in nearestKSearch!"); + assert (index >= 0 && index < static_cast (input_->points.size ()) && "Out-of-bounds error in nearestKSearch!"); return (nearestKSearch (input_->points[index], k, k_indices, k_sqr_distances)); } - assert (index >= 0 && index < static_cast (indices_->size ()) && "Out-of-bounds error in nearestKSearch!"); - if (index >= static_cast (indices_->size ()) || index < 0) + assert (index >= 0 && index < static_cast (indices_->size ()) && "Out-of-bounds error in nearestKSearch!"); + if (index >= static_cast (indices_->size ()) || index < 0) return (0); return (nearestKSearch (input_->points[(*indices_)[index]], k, k_indices, k_sqr_distances)); } @@ -111,8 +111,8 @@ pcl::search::Search::nearestKSearch ( /////////////////////////////////////////////////////////////////////////////////////////// template void pcl::search::Search::nearestKSearch ( - const PointCloud& cloud, const std::vector& indices, - int k, std::vector< std::vector >& k_indices, + const PointCloud& cloud, const Indices& indices, + int k, std::vector& k_indices, std::vector< std::vector >& k_sqr_distances) const { if (indices.empty ()) @@ -120,7 +120,7 @@ pcl::search::Search::nearestKSearch ( k_indices.resize (cloud.size ()); k_sqr_distances.resize (cloud.size ()); for (std::size_t i = 0; i < cloud.size (); i++) - nearestKSearch (cloud, static_cast (i), k, k_indices[i], k_sqr_distances[i]); + nearestKSearch (cloud, static_cast (i), k, k_indices[i], k_sqr_distances[i]); } else { @@ -134,26 +134,26 @@ pcl::search::Search::nearestKSearch ( /////////////////////////////////////////////////////////////////////////////////////////// template int pcl::search::Search::radiusSearch ( - const PointCloud &cloud, int index, double radius, - std::vector &k_indices, std::vector &k_sqr_distances, + const PointCloud &cloud, index_t index, double radius, + Indices &k_indices, std::vector &k_sqr_distances, unsigned int max_nn) const { - assert (index >= 0 && index < static_cast (cloud.points.size ()) && "Out-of-bounds error in radiusSearch!"); + assert (index >= 0 && index < static_cast (cloud.points.size ()) && "Out-of-bounds error in radiusSearch!"); return (radiusSearch(cloud.points[index], radius, k_indices, k_sqr_distances, max_nn)); } /////////////////////////////////////////////////////////////////////////////////////////// template int pcl::search::Search::radiusSearch ( - int index, double radius, std::vector &k_indices, + index_t index, double radius, Indices &k_indices, std::vector &k_sqr_distances, unsigned int max_nn ) const { if (!indices_) { - assert (index >= 0 && index < static_cast (input_->points.size ()) && "Out-of-bounds error in radiusSearch!"); + assert (index >= 0 && index < static_cast (input_->points.size ()) && "Out-of-bounds error in radiusSearch!"); return (radiusSearch (input_->points[index], radius, k_indices, k_sqr_distances, max_nn)); } - assert (index >= 0 && index < static_cast (indices_->size ()) && "Out-of-bounds error in radiusSearch!"); + assert (index >= 0 && index < static_cast (indices_->size ()) && "Out-of-bounds error in radiusSearch!"); return (radiusSearch (input_->points[(*indices_)[index]], radius, k_indices, k_sqr_distances, max_nn)); } @@ -161,9 +161,9 @@ pcl::search::Search::radiusSearch ( template void pcl::search::Search::radiusSearch ( const PointCloud& cloud, - const std::vector& indices, + const Indices& indices, double radius, - std::vector< std::vector >& k_indices, + std::vector& k_indices, std::vector< std::vector > &k_sqr_distances, unsigned int max_nn) const { @@ -172,7 +172,7 @@ pcl::search::Search::radiusSearch ( k_indices.resize (cloud.size ()); k_sqr_distances.resize (cloud.size ()); for (std::size_t i = 0; i < cloud.size (); i++) - radiusSearch (cloud, static_cast (i), radius,k_indices[i], k_sqr_distances[i], max_nn); + radiusSearch (cloud, static_cast (i), radius,k_indices[i], k_sqr_distances[i], max_nn); } else { @@ -186,16 +186,16 @@ pcl::search::Search::radiusSearch ( /////////////////////////////////////////////////////////////////////////////////////////// template void pcl::search::Search::sortResults ( - std::vector& indices, std::vector& distances) const + Indices& indices, std::vector& distances) const { - std::vector order (indices.size ()); + Indices order (indices.size ()); for (std::size_t idx = 0; idx < order.size (); ++idx) - order [idx] = static_cast (idx); + order [idx] = static_cast (idx); Compare compare (distances); sort (order.begin (), order.end (), compare); - std::vector sorted (indices.size ()); + Indices sorted (indices.size ()); for (std::size_t idx = 0; idx < order.size (); ++idx) sorted [idx] = indices[order [idx]]; diff --git a/search/include/pcl/search/kdtree.h b/search/include/pcl/search/kdtree.h index 25d4e864..b0e48a08 100644 --- a/search/include/pcl/search/kdtree.h +++ b/search/include/pcl/search/kdtree.h @@ -64,9 +64,6 @@ namespace pcl using PointCloud = typename Search::PointCloud; using PointCloudConstPtr = typename Search::PointCloudConstPtr; - using typename Search::IndicesPtr; - using typename Search::IndicesConstPtr; - using pcl::search::Search::indices_; using pcl::search::Search::input_; using pcl::search::Search::getIndices; @@ -147,7 +144,7 @@ namespace pcl */ int nearestKSearch (const PointT &point, int k, - std::vector &k_indices, + Indices &k_indices, std::vector &k_sqr_distances) const override; /** \brief Search for all the nearest neighbors of the query point in a given radius. @@ -162,7 +159,7 @@ namespace pcl */ int radiusSearch (const PointT& point, double radius, - std::vector &k_indices, + Indices &k_indices, std::vector &k_sqr_distances, unsigned int max_nn = 0) const override; protected: diff --git a/search/include/pcl/search/octree.h b/search/include/pcl/search/octree.h index 0b0a312b..ad160be1 100644 --- a/search/include/pcl/search/octree.h +++ b/search/include/pcl/search/octree.h @@ -72,9 +72,6 @@ namespace pcl using Ptr = shared_ptr >; using ConstPtr = shared_ptr >; - using typename Search::IndicesPtr; - using typename Search::IndicesConstPtr; - using PointCloud = pcl::PointCloud; using PointCloudPtr = typename PointCloud::Ptr; using PointCloudConstPtr = typename PointCloud::ConstPtr; @@ -139,7 +136,7 @@ namespace pcl * \return number of neighbors found */ inline int - nearestKSearch (const PointCloud &cloud, int index, int k, std::vector &k_indices, + nearestKSearch (const PointCloud &cloud, index_t index, int k, Indices &k_indices, std::vector &k_sqr_distances) const override { return (tree_->nearestKSearch (cloud, index, k, k_indices, k_sqr_distances)); @@ -154,7 +151,7 @@ namespace pcl * \return number of neighbors found */ inline int - nearestKSearch (const PointT &point, int k, std::vector &k_indices, + nearestKSearch (const PointT &point, int k, Indices &k_indices, std::vector &k_sqr_distances) const override { return (tree_->nearestKSearch (point, k, k_indices, k_sqr_distances)); @@ -172,7 +169,7 @@ namespace pcl * \return number of neighbors found */ inline int - nearestKSearch (int index, int k, std::vector &k_indices, std::vector &k_sqr_distances) const override + nearestKSearch (index_t index, int k, Indices &k_indices, std::vector &k_sqr_distances) const override { return (tree_->nearestKSearch (index, k, k_indices, k_sqr_distances)); } @@ -188,9 +185,9 @@ namespace pcl */ inline int radiusSearch (const PointCloud &cloud, - int index, + index_t index, double radius, - std::vector &k_indices, + Indices &k_indices, std::vector &k_sqr_distances, unsigned int max_nn = 0) const override { @@ -211,7 +208,7 @@ namespace pcl inline int radiusSearch (const PointT &p_q, double radius, - std::vector &k_indices, + Indices &k_indices, std::vector &k_sqr_distances, unsigned int max_nn = 0) const override { @@ -231,7 +228,7 @@ namespace pcl * \return number of neighbors found in radius */ inline int - radiusSearch (int index, double radius, std::vector &k_indices, + radiusSearch (index_t index, double radius, Indices &k_indices, std::vector &k_sqr_distances, unsigned int max_nn = 0) const override { tree_->radiusSearch (index, radius, k_indices, k_sqr_distances, max_nn); @@ -249,7 +246,7 @@ namespace pcl * \return number of neighbors found */ inline void - approxNearestSearch (const PointCloudConstPtr &cloud, int query_index, int &result_index, + approxNearestSearch (const PointCloudConstPtr &cloud, index_t query_index, index_t &result_index, float &sqr_distance) { return (tree_->approxNearestSearch (cloud->points[query_index], result_index, sqr_distance)); @@ -261,7 +258,7 @@ namespace pcl * \param[out] sqr_distance the resultant squared distance to the neighboring point */ inline void - approxNearestSearch (const PointT &p_q, int &result_index, float &sqr_distance) + approxNearestSearch (const PointT &p_q, index_t &result_index, float &sqr_distance) { return (tree_->approxNearestSearch (p_q, result_index, sqr_distance)); } @@ -274,7 +271,7 @@ namespace pcl * \return number of neighbors found */ inline void - approxNearestSearch (int query_index, int &result_index, float &sqr_distance) + approxNearestSearch (index_t query_index, index_t &result_index, float &sqr_distance) { return (tree_->approxNearestSearch (query_index, result_index, sqr_distance)); } diff --git a/search/include/pcl/search/organized.h b/search/include/pcl/search/organized.h index d819a1ac..a6d40bbf 100644 --- a/search/include/pcl/search/organized.h +++ b/search/include/pcl/search/organized.h @@ -39,6 +39,7 @@ #pragma once +#include #include #include #include @@ -68,7 +69,6 @@ namespace pcl using PointCloudPtr = typename PointCloud::Ptr; using PointCloudConstPtr = typename PointCloud::ConstPtr; - using typename Search::IndicesConstPtr; using Ptr = shared_ptr >; using ConstPtr = shared_ptr >; @@ -136,8 +136,8 @@ namespace pcl if (indices_ && !indices_->empty()) { mask_.assign (input_->size (), 0); - for (std::vector::const_iterator iIt = indices_->begin (); iIt != indices_->end (); ++iIt) - mask_[*iIt] = 1; + for (const auto& idx : *indices_) + mask_[idx] = 1; } else mask_.assign (input_->size (), 1); @@ -158,7 +158,7 @@ namespace pcl int radiusSearch (const PointT &p_q, double radius, - std::vector &k_indices, + Indices &k_indices, std::vector &k_sqr_distances, unsigned int max_nn = 0) const override; @@ -178,7 +178,7 @@ namespace pcl int nearestKSearch (const PointT &p_q, int k, - std::vector &k_indices, + Indices &k_indices, std::vector &k_sqr_distances) const override; /** \brief projects a point into the image @@ -192,9 +192,9 @@ namespace pcl struct Entry { - Entry (int idx, float dist) : index (idx), distance (dist) {} + Entry (index_t idx, float dist) : index (idx), distance (dist) {} Entry () : index (0), distance (0) {} - unsigned index; + index_t index; float distance; inline bool @@ -212,7 +212,7 @@ namespace pcl * \return whether the top element changed or not. */ inline bool - testPoint (const PointT& query, unsigned k, std::priority_queue& queue, unsigned index) const + testPoint (const PointT& query, unsigned k, std::priority_queue& queue, index_t index) const { const PointT& point = input_->points [index]; if (mask_ [index] && std::isfinite (point.x)) diff --git a/search/include/pcl/search/search.h b/search/include/pcl/search/search.h index f90c64ad..afa4f456 100644 --- a/search/include/pcl/search/search.h +++ b/search/include/pcl/search/search.h @@ -38,6 +38,7 @@ #pragma once +#include // for IndicesConstPtr #include #include #include @@ -80,8 +81,8 @@ namespace pcl using Ptr = shared_ptr >; using ConstPtr = shared_ptr >; - using IndicesPtr = shared_ptr >; - using IndicesConstPtr = shared_ptr >; + using IndicesPtr = pcl::IndicesPtr; + using IndicesConstPtr = pcl::IndicesConstPtr; /** Constructor. */ Search (const std::string& name = "", bool sorted = false); @@ -142,7 +143,7 @@ namespace pcl * \return number of neighbors found */ virtual int - nearestKSearch (const PointT &point, int k, std::vector &k_indices, + nearestKSearch (const PointT &point, int k, Indices &k_indices, std::vector &k_sqr_distances) const = 0; /** \brief Search for k-nearest neighbors for the given query point. @@ -156,7 +157,7 @@ namespace pcl */ template inline int nearestKSearchT (const PointTDiff &point, int k, - std::vector &k_indices, std::vector &k_sqr_distances) const + Indices &k_indices, std::vector &k_sqr_distances) const { PointT p; copyPoint (point, p); @@ -180,8 +181,8 @@ namespace pcl * \exception asserts in debug mode if the index is not between 0 and the maximum number of points */ virtual int - nearestKSearch (const PointCloud &cloud, int index, int k, - std::vector &k_indices, + nearestKSearch (const PointCloud &cloud, index_t index, int k, + Indices &k_indices, std::vector &k_sqr_distances) const; /** \brief Search for k-nearest neighbors for the given query point (zero-copy). @@ -202,8 +203,8 @@ namespace pcl * \exception asserts in debug mode if the index is not between 0 and the maximum number of points */ virtual int - nearestKSearch (int index, int k, - std::vector &k_indices, + nearestKSearch (index_t index, int k, + Indices &k_indices, std::vector &k_sqr_distances) const; /** \brief Search for the k-nearest neighbors for the given query point. @@ -214,8 +215,8 @@ namespace pcl * \param[out] k_sqr_distances the resultant squared distances to the neighboring points, k_sqr_distances[i] corresponds to the neighbors of the query point i */ virtual void - nearestKSearch (const PointCloud& cloud, const std::vector& indices, - int k, std::vector< std::vector >& k_indices, + nearestKSearch (const PointCloud& cloud, const Indices& indices, + int k, std::vector& k_indices, std::vector< std::vector >& k_sqr_distances) const; /** \brief Search for the k-nearest neighbors for the given query point. Use this method if the query points are of a different type than the points in the data set (e.g. PointXYZRGBA instead of PointXYZ). @@ -227,7 +228,7 @@ namespace pcl * \note This method copies the input point cloud of type PointTDiff to a temporary cloud of type PointT and performs the batch search on the new cloud. You should prefer the single-point search if you don't use a search algorithm that accelerates batch NN search. */ template void - nearestKSearchT (const pcl::PointCloud &cloud, const std::vector& indices, int k, std::vector< std::vector > &k_indices, + nearestKSearchT (const pcl::PointCloud &cloud, const Indices& indices, int k, std::vector &k_indices, std::vector< std::vector > &k_sqr_distances) const { // Copy all the data fields from the input cloud to the output one @@ -244,7 +245,7 @@ namespace pcl pcl::for_each_type (pcl::NdConcatenateFunctor ( cloud[i], pc[i])); } - nearestKSearch (pc,std::vector(),k,k_indices,k_sqr_distances); + nearestKSearch (pc,Indices(),k,k_indices,k_sqr_distances); } else { @@ -254,7 +255,7 @@ namespace pcl pcl::for_each_type (pcl::NdConcatenateFunctor ( cloud[indices[i]], pc[i])); } - nearestKSearch (pc,std::vector(),k,k_indices,k_sqr_distances); + nearestKSearch (pc,Indices(),k,k_indices,k_sqr_distances); } } @@ -269,7 +270,7 @@ namespace pcl * \return number of neighbors found in radius */ virtual int - radiusSearch (const PointT& point, double radius, std::vector& k_indices, + radiusSearch (const PointT& point, double radius, Indices& k_indices, std::vector& k_sqr_distances, unsigned int max_nn = 0) const = 0; /** \brief Search for all the nearest neighbors of the query point in a given radius. @@ -283,7 +284,7 @@ namespace pcl * \return number of neighbors found in radius */ template inline int - radiusSearchT (const PointTDiff &point, double radius, std::vector &k_indices, + radiusSearchT (const PointTDiff &point, double radius, Indices &k_indices, std::vector &k_sqr_distances, unsigned int max_nn = 0) const { PointT p; @@ -309,8 +310,8 @@ namespace pcl * \exception asserts in debug mode if the index is not between 0 and the maximum number of points */ virtual int - radiusSearch (const PointCloud &cloud, int index, double radius, - std::vector &k_indices, std::vector &k_sqr_distances, + radiusSearch (const PointCloud &cloud, index_t index, double radius, + Indices &k_indices, std::vector &k_sqr_distances, unsigned int max_nn = 0) const; /** \brief Search for all the nearest neighbors of the query point in a given radius (zero-copy). @@ -333,7 +334,7 @@ namespace pcl * \exception asserts in debug mode if the index is not between 0 and the maximum number of points */ virtual int - radiusSearch (int index, double radius, std::vector &k_indices, + radiusSearch (index_t index, double radius, Indices &k_indices, std::vector &k_sqr_distances, unsigned int max_nn = 0) const; /** \brief Search for all the nearest neighbors of the query point in a given radius. @@ -348,9 +349,9 @@ namespace pcl */ virtual void radiusSearch (const PointCloud& cloud, - const std::vector& indices, + const Indices& indices, double radius, - std::vector< std::vector >& k_indices, + std::vector& k_indices, std::vector< std::vector > &k_sqr_distances, unsigned int max_nn = 0) const; @@ -367,9 +368,9 @@ namespace pcl */ template void radiusSearchT (const pcl::PointCloud &cloud, - const std::vector& indices, + const Indices& indices, double radius, - std::vector< std::vector > &k_indices, + std::vector &k_indices, std::vector< std::vector > &k_sqr_distances, unsigned int max_nn = 0) const { @@ -384,20 +385,20 @@ namespace pcl pc.resize (cloud.size ()); for (std::size_t i = 0; i < cloud.size (); ++i) pcl::for_each_type (pcl::NdConcatenateFunctor (cloud[i], pc[i])); - radiusSearch (pc, std::vector (), radius, k_indices, k_sqr_distances, max_nn); + radiusSearch (pc, Indices (), radius, k_indices, k_sqr_distances, max_nn); } else { pc.resize (indices.size ()); for (std::size_t i = 0; i < indices.size (); ++i) pcl::for_each_type (pcl::NdConcatenateFunctor (cloud[indices[i]], pc[i])); - radiusSearch (pc, std::vector(), radius, k_indices, k_sqr_distances, max_nn); + radiusSearch (pc, Indices(), radius, k_indices, k_sqr_distances, max_nn); } } protected: void - sortResults (std::vector& indices, std::vector& distances) const; + sortResults (Indices& indices, std::vector& distances) const; PointCloudConstPtr input_; IndicesConstPtr indices_; @@ -413,7 +414,7 @@ namespace pcl } bool - operator () (int first, int second) const + operator () (index_t first, index_t second) const { return (distances_ [first] < distances_[second]); } diff --git a/segmentation/include/pcl/segmentation/boost.h b/segmentation/include/pcl/segmentation/boost.h index 19e75080..40c3aac5 100644 --- a/segmentation/include/pcl/segmentation/boost.h +++ b/segmentation/include/pcl/segmentation/boost.h @@ -47,7 +47,6 @@ #ifndef Q_MOC_RUN // Marking all Boost headers as system headers to remove warnings #include -#include #include #include #include diff --git a/segmentation/include/pcl/segmentation/comparator.h b/segmentation/include/pcl/segmentation/comparator.h index 122745a0..a08c4857 100644 --- a/segmentation/include/pcl/segmentation/comparator.h +++ b/segmentation/include/pcl/segmentation/comparator.h @@ -39,6 +39,7 @@ #pragma once +#include #include #include diff --git a/segmentation/include/pcl/segmentation/conditional_euclidean_clustering.h b/segmentation/include/pcl/segmentation/conditional_euclidean_clustering.h index 387f1237..55d96b2c 100644 --- a/segmentation/include/pcl/segmentation/conditional_euclidean_clustering.h +++ b/segmentation/include/pcl/segmentation/conditional_euclidean_clustering.h @@ -37,6 +37,7 @@ #pragma once +#include #include #include #include @@ -109,18 +110,18 @@ namespace pcl /** \brief Provide a pointer to the search object. * \param[in] tree a pointer to the spatial search object. */ - inline void - setSearchMethod (const SearcherPtr &tree) - { - searcher_ = tree; + inline void + setSearchMethod (const SearcherPtr &tree) + { + searcher_ = tree; } - /** \brief Get a pointer to the search method used. + /** \brief Get a pointer to the search method used. */ - inline const SearcherPtr& - getSearchMethod () const - { - return searcher_; + inline const SearcherPtr& + getSearchMethod () const + { + return searcher_; } /** \brief Set the condition that needs to hold for neighboring points to be considered part of the same cluster. @@ -141,7 +142,7 @@ namespace pcl * \param[in] condition_function The condition function that needs to hold for clustering */ inline void - setConditionFunction (bool (*condition_function) (const PointT&, const PointT&, float)) + setConditionFunction (bool (*condition_function) (const PointT&, const PointT&, float)) { condition_function_ = condition_function; } diff --git a/segmentation/include/pcl/segmentation/cpc_segmentation.h b/segmentation/include/pcl/segmentation/cpc_segmentation.h index e7f76abc..de38bd26 100644 --- a/segmentation/include/pcl/segmentation/cpc_segmentation.h +++ b/segmentation/include/pcl/segmentation/cpc_segmentation.h @@ -53,7 +53,7 @@ #define PCL_INSTANTIATE_CPCSegmentation(T) template class PCL_EXPORTS pcl::CPCSegmentation; namespace pcl -{ +{ /** \brief A segmentation algorithm partitioning a supervoxel graph. It uses planar cuts induced by local concavities for the recursive segmentation. Cuts are estimated using locally constrained directed RANSAC. * \note If you use this in a scientific work please cite the following paper: * M. Schoeler, J. Papon, F. Woergoetter @@ -88,11 +88,11 @@ namespace pcl public: CPCSegmentation (); - + ~CPCSegmentation (); /** \brief Merge supervoxels using cuts through local convexities. The input parameters are generated by using the \ref SupervoxelClustering class. To retrieve the output use the \ref relabelCloud method. - * \note There are three ways to retrieve the segmentation afterwards (inherited from \ref LCCPSegmentation): \ref relabelCloud, \ref getSegmentSupervoxelMap and \ref getSupervoxelSegmentMap*/ + * \note There are three ways to retrieve the segmentation afterwards (inherited from \ref LCCPSegmentation): \ref relabelCloud, \ref getSupervoxelToSegmentMap and \ref getSupervoxelToSegmentMap */ void segment (); @@ -122,13 +122,13 @@ namespace pcl /** \brief Set the number of iterations for the weighted RANSAC step (best cut estimations) * \param[in] ransac_iterations The number of iterations */ - inline void + inline void setRANSACIterations (const std::uint32_t ransac_iterations) { ransac_itrs_ = ransac_iterations; } - private: + private: /** \brief Used in for CPC to find and fit cutting planes to the pointcloud. * \note Is used recursively @@ -155,12 +155,12 @@ namespace pcl /** \brief Use clean cutting */ bool use_clean_cutting_; - + /** \brief Iterations for RANSAC */ std::uint32_t ransac_itrs_; - - -/******************************************* Directional weighted RANSAC declarations ******************************************************************/ + + +/******************************************* Directional weighted RANSAC declarations ******************************************************************/ /** \brief @b WeightedRandomSampleConsensus represents an implementation of the Directionally Weighted RANSAC algorithm, as described in: "Constrained Planar Cuts - Part Segmentation for Point Clouds", CVPR 2015, M. Schoeler, J. Papon, F. Wörgötter. * \note It only uses points with a weight > 0 for the model calculation, but uses all points for the evaluation (scoring of the model) * Only use in conjunction with sac_model_plane @@ -171,7 +171,7 @@ namespace pcl * \author Markus Schoeler (mschoeler@web.de) * \ingroup segmentation */ - + class WeightedRandomSampleConsensus : public SampleConsensus { using SampleConsensusModelPtr = SampleConsensusModel::Ptr; @@ -184,7 +184,7 @@ namespace pcl * \param[in] model a Sample Consensus model * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) */ - WeightedRandomSampleConsensus (const SampleConsensusModelPtr &model, + WeightedRandomSampleConsensus (const SampleConsensusModelPtr &model, bool random = false) : SampleConsensus (model, random) { @@ -253,26 +253,26 @@ namespace pcl full_cloud_pt_indices_.reset (new std::vector (* (sac_model_->getIndices ()))); point_cloud_ptr_ = sac_model_->getInputCloud (); } - + /** \brief weight each positive weight point by the inner product between the normal and the plane normal */ bool use_directed_weights_; - + /** \brief vector of weights assigned to points. Set by the setWeights-method */ std::vector weights_; - + /** \brief The indices used for estimating the RANSAC model. Only those whose weight is > 0 */ pcl::IndicesPtr model_pt_indices_; - + /** \brief The complete list of indices used for the model evaluation */ pcl::IndicesPtr full_cloud_pt_indices_; - + /** \brief Pointer to the input PointCloud */ pcl::PointCloud::ConstPtr point_cloud_ptr_; - + /** \brief Highest score found so far */ double best_score_; }; - + }; } @@ -281,5 +281,5 @@ namespace pcl #elif defined(PCL_ONLY_CORE_POINT_TYPES) //pcl::PointXYZINormal is not a core point type (so we cannot use the precompiled classes here) #include - #include + #include #endif // PCL_NO_PRECOMPILE / PCL_ONLY_CORE_POINT_TYPES diff --git a/segmentation/include/pcl/segmentation/crf_normal_segmentation.h b/segmentation/include/pcl/segmentation/crf_normal_segmentation.h index 71be7ce2..6eb2663c 100644 --- a/segmentation/include/pcl/segmentation/crf_normal_segmentation.h +++ b/segmentation/include/pcl/segmentation/crf_normal_segmentation.h @@ -36,6 +36,7 @@ #pragma once +#include #include #include diff --git a/segmentation/include/pcl/segmentation/crf_segmentation.h b/segmentation/include/pcl/segmentation/crf_segmentation.h index f06434df..c9701840 100644 --- a/segmentation/include/pcl/segmentation/crf_segmentation.h +++ b/segmentation/include/pcl/segmentation/crf_segmentation.h @@ -39,6 +39,7 @@ #pragma once +#include #include #include #include diff --git a/segmentation/include/pcl/segmentation/edge_aware_plane_comparator.h b/segmentation/include/pcl/segmentation/edge_aware_plane_comparator.h index acb483cf..98f3bde3 100644 --- a/segmentation/include/pcl/segmentation/edge_aware_plane_comparator.h +++ b/segmentation/include/pcl/segmentation/edge_aware_plane_comparator.h @@ -44,7 +44,7 @@ namespace pcl { - /** \brief EdgeAwarePlaneComparator is a Comparator that operates on plane coefficients, + /** \brief EdgeAwarePlaneComparator is a Comparator that operates on plane coefficients, * for use in planar segmentation. * In conjunction with OrganizedConnectedComponentSegmentation, this allows planes to be segmented from organized data. * @@ -56,11 +56,11 @@ namespace pcl public: using PointCloud = typename Comparator::PointCloud; using PointCloudConstPtr = typename Comparator::PointCloudConstPtr; - + using PointCloudN = pcl::PointCloud; using PointCloudNPtr = typename PointCloudN::Ptr; using PointCloudNConstPtr = typename PointCloudN::ConstPtr; - + using Ptr = shared_ptr >; using ConstPtr = shared_ptr >; @@ -80,10 +80,10 @@ namespace pcl { } - /** \brief Empty constructor for PlaneCoefficientComparator. + /** \brief Empty constructor for PlaneCoefficientComparator. * \param[in] distance_map the distance map to use */ - EdgeAwarePlaneComparator (const float *distance_map) : + EdgeAwarePlaneComparator (const float *distance_map) : distance_map_ (distance_map), distance_map_threshold_ (5), curvature_threshold_ (0.04f), @@ -92,13 +92,13 @@ namespace pcl } /** \brief Destructor for PlaneCoefficientComparator. */ - + ~EdgeAwarePlaneComparator () { } - /** \brief Set a distance map to use. For an example of a valid distance map see - * \ref OrganizedIntegralImageNormalEstimation + /** \brief Set a distance map to use. For an example of a valid distance map see + * IntegralImageNormalEstimation::getDistanceMap * \param[in] distance_map the distance map to use */ inline void @@ -161,7 +161,7 @@ namespace pcl { return (euclidean_distance_threshold_); } - + protected: /** \brief Compare two neighboring points, by using normal information, curvature, and euclidean distance information. * \param[in] idx1 The index of the first point. @@ -184,7 +184,7 @@ namespace pcl dist_threshold *= z * z; euclidean_dist_threshold *= z * z; } - + float dx = input_->points[idx1].x - input_->points[idx2].x; float dy = input_->points[idx1].y - input_->points[idx2].y; float dz = input_->points[idx1].z - input_->points[idx2].z; @@ -195,10 +195,10 @@ namespace pcl bool curvature_ok = normals_->points[idx1].curvature < curvature_threshold_; bool plane_d_ok = std::abs ((*plane_coeff_d_)[idx1] - (*plane_coeff_d_)[idx2]) < dist_threshold; - - if (distance_map_[idx1] < distance_map_threshold_) + + if (distance_map_[idx1] < distance_map_threshold_) curvature_ok = false; - + return (dist_ok && normal_ok && curvature_ok && plane_d_ok); } diff --git a/segmentation/include/pcl/segmentation/euclidean_cluster_comparator.h b/segmentation/include/pcl/segmentation/euclidean_cluster_comparator.h index 7295978e..a924d218 100644 --- a/segmentation/include/pcl/segmentation/euclidean_cluster_comparator.h +++ b/segmentation/include/pcl/segmentation/euclidean_cluster_comparator.h @@ -39,10 +39,12 @@ #pragma once +#include #include +#include #include #include -#include + namespace pcl { @@ -211,7 +213,7 @@ namespace pcl using experimental::EuclideanClusterComparator::setExcludeLabels; /** \brief Default constructor for EuclideanClusterComparator. */ - PCL_DEPRECATED("remove PointNT from template parameters") + PCL_DEPRECATED(1, 12, "remove PointNT from template parameters") EuclideanClusterComparator () : normals_ () , angular_threshold_ (0.0f) @@ -220,19 +222,19 @@ namespace pcl /** \brief Provide a pointer to the input normals. * \param[in] normals the input normal cloud */ - 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.") + PCL_DEPRECATED(1, 12, "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. */ - 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.") + PCL_DEPRECATED(1, 12, "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 */ - 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.") + PCL_DEPRECATED(1, 12, "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) { @@ -240,18 +242,18 @@ 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. */ - 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.") + PCL_DEPRECATED(1, 12, "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 */ - PCL_DEPRECATED("use setExcludeLabels(const ExcludeLabelSetConstPtr &) instead") + PCL_DEPRECATED(1, 12, "use setExcludeLabels(const ExcludeLabelSetConstPtr &) instead") void setExcludeLabels (const std::vector& exclude_labels) { - exclude_labels_ = boost::make_shared > (); + exclude_labels_ = pcl::make_shared > (); for (std::size_t i = 0; i < exclude_labels.size (); ++i) if (exclude_labels[i]) exclude_labels_->insert (i); diff --git a/segmentation/include/pcl/segmentation/grabcut_segmentation.h b/segmentation/include/pcl/segmentation/grabcut_segmentation.h index d8ecbf5b..f31580d9 100644 --- a/segmentation/include/pcl/segmentation/grabcut_segmentation.h +++ b/segmentation/include/pcl/segmentation/grabcut_segmentation.h @@ -39,6 +39,7 @@ #pragma once +#include #include #include #include diff --git a/segmentation/include/pcl/segmentation/ground_plane_comparator.h b/segmentation/include/pcl/segmentation/ground_plane_comparator.h index 3ca912b4..2f3cbee4 100644 --- a/segmentation/include/pcl/segmentation/ground_plane_comparator.h +++ b/segmentation/include/pcl/segmentation/ground_plane_comparator.h @@ -39,10 +39,11 @@ #pragma once -#include +#include #include +#include #include -#include + namespace pcl { @@ -139,7 +140,7 @@ namespace pcl void setPlaneCoeffD (std::vector& plane_coeff_d) { - plane_coeff_d_ = boost::make_shared >(plane_coeff_d); + plane_coeff_d_ = pcl::make_shared >(plane_coeff_d); } /** \brief Get a pointer to the vector of the d-coefficient of the planes' hessian normal form. */ diff --git a/segmentation/include/pcl/segmentation/impl/approximate_progressive_morphological_filter.hpp b/segmentation/include/pcl/segmentation/impl/approximate_progressive_morphological_filter.hpp index 47335f74..9ac78328 100644 --- a/segmentation/include/pcl/segmentation/impl/approximate_progressive_morphological_filter.hpp +++ b/segmentation/include/pcl/segmentation/impl/approximate_progressive_morphological_filter.hpp @@ -83,25 +83,17 @@ pcl::ApproximateProgressiveMorphologicalFilter::extract (std::vector window_sizes; std::vector half_sizes; int iteration = 0; - int half_size = 0.0f; - float window_size = 0.0f; - float height_threshold = 0.0f; + float window_size = 0.0f; while (window_size < max_window_size_) { // Determine the initial window size. - if (exponential_) - half_size = static_cast (std::pow (static_cast (base_), iteration)); - else - half_size = (iteration+1) * base_; + int half_size = (exponential_) ? (static_cast (std::pow (static_cast (base_), iteration))) : ((iteration+1) * base_); window_size = 2 * half_size + 1; // Calculate the height threshold to be used in the next iteration. - if (iteration == 0) - height_threshold = initial_distance_; - else - height_threshold = slope_ * (window_size - window_sizes[iteration-1]) * cell_size_ + initial_distance_; + float height_threshold = (iteration == 0) ? (initial_distance_) : (slope_ * (window_size - window_sizes[iteration-1]) * cell_size_ + initial_distance_); // Enforce max distance on height threshold if (height_threshold > max_distance_) @@ -133,9 +125,10 @@ pcl::ApproximateProgressiveMorphologicalFilter::extract (std::vector::quiet_NaN ()); -#ifdef _OPENMP -#pragma omp parallel for num_threads(threads_) -#endif +#pragma omp parallel for \ + default(none) \ + shared(A, global_min) \ + num_threads(threads_) for (int i = 0; i < (int)input_->points.size (); ++i) { // ...then test for lower points within the cell @@ -164,9 +157,10 @@ pcl::ApproximateProgressiveMorphologicalFilter::extract (std::vector (*input_, ground, *cloud); // Apply the morphological opening operation at the current window size. -#ifdef _OPENMP -#pragma omp parallel for num_threads(threads_) -#endif +#pragma omp parallel for \ + default(none) \ + shared(A, cols, half_sizes, i, rows, Z) \ + num_threads(threads_) for (int row = 0; row < rows; ++row) { int rs, re; @@ -198,9 +192,10 @@ pcl::ApproximateProgressiveMorphologicalFilter::extract (std::vector::segmentPoints (pcl::PointCloud // ----------------------------------// // -------- -------------------// - pcl::PointCloud tmp_cloud; - tmp_cloud = *filtered_anno_; - // create dense CRF DenseCrf crf (N, n_labels); @@ -549,6 +546,9 @@ pcl::CrfSegmentation::segmentPoints (pcl::PointCloud /* + pcl::PointCloud tmp_cloud; + tmp_cloud = *filtered_anno_; + bool c = true; for (std::size_t i = 0; i < tmp_cloud.points.size (); i++) { diff --git a/segmentation/include/pcl/segmentation/impl/grabcut_segmentation.hpp b/segmentation/include/pcl/segmentation/impl/grabcut_segmentation.hpp index e87d883b..72fbe5e5 100644 --- a/segmentation/include/pcl/segmentation/impl/grabcut_segmentation.hpp +++ b/segmentation/include/pcl/segmentation/impl/grabcut_segmentation.hpp @@ -35,25 +35,29 @@ * */ -#ifndef PCL_SEGMENTATION_IMPL_GRABCUT_HPP -#define PCL_SEGMENTATION_IMPL_GRABCUT_HPP +#pragma once +#include +#include // for pcl::isFinite #include #include -#include + namespace pcl { - template <> - float squaredEuclideanDistance (const pcl::segmentation::grabcut::Color &c1, - const pcl::segmentation::grabcut::Color &c2) - { - return ((c1.r-c2.r)*(c1.r-c2.r)+(c1.g-c2.g)*(c1.g-c2.g)+(c1.b-c2.b)*(c1.b-c2.b)); - } + +template <> +float squaredEuclideanDistance (const pcl::segmentation::grabcut::Color &c1, + const pcl::segmentation::grabcut::Color &c2) +{ + return ((c1.r-c2.r)*(c1.r-c2.r)+(c1.g-c2.g)*(c1.g-c2.g)+(c1.b-c2.b)*(c1.b-c2.b)); } +namespace segmentation +{ + template -pcl::segmentation::grabcut::Color::Color (const PointT& p) +grabcut::Color::Color (const PointT& p) { r = static_cast (p.r) / 255.0; g = static_cast (p.g) / 255.0; @@ -61,7 +65,7 @@ pcl::segmentation::grabcut::Color::Color (const PointT& p) } template -pcl::segmentation::grabcut::Color::operator PointT () const +grabcut::Color::operator PointT () const { PointT p; p.r = static_cast (r * 255); @@ -70,14 +74,16 @@ pcl::segmentation::grabcut::Color::operator PointT () const return (p); } +} // namespace segmentation + template void -pcl::GrabCut::setInputCloud (const PointCloudConstPtr &cloud) +GrabCut::setInputCloud (const PointCloudConstPtr &cloud) { input_ = cloud; } template bool -pcl::GrabCut::initCompute () +GrabCut::initCompute () { using namespace pcl::segmentation::grabcut; if (!pcl::PCLBase::initCompute ()) @@ -140,20 +146,20 @@ pcl::GrabCut::initCompute () } template void -pcl::GrabCut::addEdge (vertex_descriptor v1, vertex_descriptor v2, float capacity, float rev_capacity) +GrabCut::addEdge (vertex_descriptor v1, vertex_descriptor v2, float capacity, float rev_capacity) { graph_.addEdge (v1, v2, capacity, rev_capacity); } template void -pcl::GrabCut::setTerminalWeights (vertex_descriptor v, float source_capacity, float sink_capacity) +GrabCut::setTerminalWeights (vertex_descriptor v, float source_capacity, float sink_capacity) { graph_.addSourceEdge (v, source_capacity); graph_.addTargetEdge (v, sink_capacity); } template void -pcl::GrabCut::setBackgroundPointsIndices (const PointIndicesConstPtr &indices) +GrabCut::setBackgroundPointsIndices (const PointIndicesConstPtr &indices) { using namespace pcl::segmentation::grabcut; if (!initCompute ()) @@ -175,7 +181,7 @@ pcl::GrabCut::setBackgroundPointsIndices (const PointIndicesConstPtr &in } template void -pcl::GrabCut::fitGMMs () +GrabCut::fitGMMs () { // Step 3: Build GMMs using Orchard-Bouman clustering algorithm buildGMMs (*image_, *indices_, hard_segmentation_, GMM_component_, background_GMM_, foreground_GMM_); @@ -185,7 +191,7 @@ pcl::GrabCut::fitGMMs () } template int -pcl::GrabCut::refineOnce () +GrabCut::refineOnce () { // Steps 4 and 5: Learn new GMMs from current segmentation learnGMMs (*image_, *indices_, hard_segmentation_, GMM_component_, background_GMM_, foreground_GMM_); @@ -202,7 +208,7 @@ pcl::GrabCut::refineOnce () } template void -pcl::GrabCut::refine () +GrabCut::refine () { std::size_t changed = indices_->size (); @@ -211,7 +217,7 @@ pcl::GrabCut::refine () } template int -pcl::GrabCut::updateHardSegmentation () +GrabCut::updateHardSegmentation () { using namespace pcl::segmentation::grabcut; @@ -242,7 +248,7 @@ pcl::GrabCut::updateHardSegmentation () } template void -pcl::GrabCut::setTrimap (const PointIndicesConstPtr &indices, segmentation::grabcut::TrimapValue t) +GrabCut::setTrimap (const PointIndicesConstPtr &indices, segmentation::grabcut::TrimapValue t) { using namespace pcl::segmentation::grabcut; for (const int &index : indices->indices) @@ -259,7 +265,7 @@ pcl::GrabCut::setTrimap (const PointIndicesConstPtr &indices, segmentati } template void -pcl::GrabCut::initGraph () +GrabCut::initGraph () { using namespace pcl::segmentation::grabcut; const int number_of_indices = static_cast (indices_->size ()); @@ -324,7 +330,7 @@ pcl::GrabCut::initGraph () } template void -pcl::GrabCut::computeNLinksNonOrganized () +GrabCut::computeNLinksNonOrganized () { const int number_of_indices = static_cast (indices_->size ()); for (int i_point = 0; i_point < number_of_indices; ++i_point) @@ -350,7 +356,7 @@ pcl::GrabCut::computeNLinksNonOrganized () } template void -pcl::GrabCut::computeNLinksOrganized () +GrabCut::computeNLinksOrganized () { for( unsigned int y = 0; y < image_->height; ++y ) { @@ -377,7 +383,7 @@ pcl::GrabCut::computeNLinksOrganized () } template void -pcl::GrabCut::computeBetaNonOrganized () +GrabCut::computeBetaNonOrganized () { float result = 0; std::size_t edges = 0; @@ -416,7 +422,7 @@ pcl::GrabCut::computeBetaNonOrganized () } template void -pcl::GrabCut::computeBetaOrganized () +GrabCut::computeBetaOrganized () { float result = 0; std::size_t edges = 0; @@ -486,13 +492,13 @@ pcl::GrabCut::computeBetaOrganized () } template void -pcl::GrabCut::computeL () +GrabCut::computeL () { L_ = 8*lambda_ + 1; } template void -pcl::GrabCut::extract (std::vector& clusters) +GrabCut::extract (std::vector& clusters) { using namespace pcl::segmentation::grabcut; clusters.clear (); @@ -509,4 +515,5 @@ pcl::GrabCut::extract (std::vector& clusters) clusters[0].indices.push_back (i); } -#endif +} // namespace pcl + diff --git a/segmentation/include/pcl/segmentation/impl/lccp_segmentation.hpp b/segmentation/include/pcl/segmentation/impl/lccp_segmentation.hpp index a43243f9..f2a8730b 100644 --- a/segmentation/include/pcl/segmentation/impl/lccp_segmentation.hpp +++ b/segmentation/include/pcl/segmentation/impl/lccp_segmentation.hpp @@ -138,22 +138,20 @@ pcl::LCCPSegmentation::computeSegmentAdjacency () { seg_label_to_neighbor_set_map_.clear (); - //The vertices in the supervoxel adjacency list are the supervoxel centroids - std::pair vertex_iterator_range; - vertex_iterator_range = boost::vertices (sv_adjacency_list_); - std::uint32_t current_segLabel; std::uint32_t neigh_segLabel; + VertexIterator sv_itr, sv_itr_end; + //The vertices in the supervoxel adjacency list are the supervoxel centroids // For every Supervoxel.. - for (VertexIterator sv_itr = vertex_iterator_range.first; sv_itr != vertex_iterator_range.second; ++sv_itr) // For all supervoxels + for(std::tie(sv_itr, sv_itr_end) = boost::vertices(sv_adjacency_list_); sv_itr != sv_itr_end; ++sv_itr) // For all supervoxels { const std::uint32_t& sv_label = sv_adjacency_list_[*sv_itr]; current_segLabel = sv_label_to_seg_label_map_[sv_label]; + AdjacencyIterator itr_neighbor, itr_neighbor_end; // ..look at all neighbors and insert their labels into the neighbor set - std::pair neighbors = boost::adjacent_vertices (*sv_itr, sv_adjacency_list_); - for (AdjacencyIterator itr_neighbor = neighbors.first; itr_neighbor != neighbors.second; ++itr_neighbor) + for (std::tie(itr_neighbor, itr_neighbor_end) = boost::adjacent_vertices (*sv_itr, sv_adjacency_list_); itr_neighbor != itr_neighbor_end; ++itr_neighbor) { const std::uint32_t& neigh_label = sv_adjacency_list_[*itr_neighbor]; neigh_segLabel = sv_label_to_seg_label_map_[neigh_label]; @@ -176,13 +174,6 @@ pcl::LCCPSegmentation::mergeSmallSegments () std::set filteredSegLabels; - std::uint32_t largest_neigh_size = 0; - std::uint32_t largest_neigh_seg_label = 0; - std::uint32_t current_seg_label; - - std::pair vertex_iterator_range; - vertex_iterator_range = boost::vertices (sv_adjacency_list_); - bool continue_filtering = true; while (continue_filtering) @@ -190,13 +181,14 @@ pcl::LCCPSegmentation::mergeSmallSegments () continue_filtering = false; unsigned int nr_filtered = 0; + VertexIterator sv_itr, sv_itr_end; // Iterate through all supervoxels, check if they are in a "small" segment -> change label to largest neighborID - for (VertexIterator sv_itr = vertex_iterator_range.first; sv_itr != vertex_iterator_range.second; ++sv_itr) // For all supervoxels + for (std::tie(sv_itr, sv_itr_end) = boost::vertices (sv_adjacency_list_); sv_itr != sv_itr_end; ++sv_itr) // For all supervoxels { const std::uint32_t& sv_label = sv_adjacency_list_[*sv_itr]; - current_seg_label = sv_label_to_seg_label_map_[sv_label]; - largest_neigh_seg_label = current_seg_label; - largest_neigh_size = seg_label_to_sv_list_map_[current_seg_label].size (); + std::uint32_t current_seg_label = sv_label_to_seg_label_map_[sv_label]; + std::uint32_t largest_neigh_seg_label = current_seg_label; + std::uint32_t largest_neigh_size = seg_label_to_sv_list_map_[current_seg_label].size (); const std::uint32_t& nr_neighbors = seg_label_to_neighbor_set_map_[current_seg_label].size (); if (nr_neighbors == 0) @@ -311,14 +303,12 @@ pcl::LCCPSegmentation::doGrouping () sv_label_to_seg_label_map_[sv_label] = 0; } + VertexIterator sv_itr, sv_itr_end; // Perform depth search on the graph and recursively group all supervoxels with convex connections //The vertices in the supervoxel adjacency list are the supervoxel centroids - std::pair< VertexIterator, VertexIterator> vertex_iterator_range; - vertex_iterator_range = boost::vertices (sv_adjacency_list_); - // Note: *sv_itr is of type " boost::graph_traits::vertex_descriptor " which it nothing but a typedef of std::size_t.. unsigned int segment_label = 1; // This starts at 1, because 0 is reserved for errors - for (VertexIterator sv_itr = vertex_iterator_range.first; sv_itr != vertex_iterator_range.second; ++sv_itr) // For all supervoxels + for (std::tie(sv_itr, sv_itr_end) = boost::vertices (sv_adjacency_list_); sv_itr != sv_itr_end; ++sv_itr) // For all supervoxels { const VertexID sv_vertex_id = *sv_itr; const std::uint32_t& sv_label = sv_adjacency_list_[sv_vertex_id]; @@ -343,10 +333,10 @@ pcl::LCCPSegmentation::recursiveSegmentGrowing (const VertexID &query_po sv_label_to_seg_label_map_[sv_label] = segment_label; seg_label_to_sv_list_map_[segment_label].insert (sv_label); + OutEdgeIterator out_Edge_itr, out_Edge_itr_end; // Iterate through all neighbors of this supervoxel and check whether they should be merged with the current supervoxel - std::pair out_edge_iterator_range; - out_edge_iterator_range = boost::out_edges (query_point_id, sv_adjacency_list_); // adjacent vertices to node (*itr) in graph sv_adjacency_list_ - for (OutEdgeIterator out_Edge_itr = out_edge_iterator_range.first; out_Edge_itr != out_edge_iterator_range.second; ++out_Edge_itr) + // boost::out_edges (query_point_id, sv_adjacency_list_): adjacent vertices to node (*itr) in graph sv_adjacency_list_ + for (std::tie(out_Edge_itr, out_Edge_itr_end) = boost::out_edges (query_point_id, sv_adjacency_list_); out_Edge_itr != out_Edge_itr_end; ++out_Edge_itr) { const VertexID neighbor_ID = boost::target (*out_Edge_itr, sv_adjacency_list_); const std::uint32_t& neighbor_label = sv_adjacency_list_[neighbor_ID]; @@ -367,37 +357,29 @@ pcl::LCCPSegmentation::applyKconvexity (const unsigned int k_arg) if (k_arg == 0) return; - unsigned int kcount = 0; - EdgeIterator edge_itr, edge_itr_end, next_edge; - boost::tie (edge_itr, edge_itr_end) = boost::edges (sv_adjacency_list_); - - std::pair source_neighbors_range; - std::pair target_neighbors_range; - // Check all edges in the graph for k-convexity - for (next_edge = edge_itr; edge_itr != edge_itr_end; edge_itr = next_edge) + for (std::tie (edge_itr, edge_itr_end) = boost::edges (sv_adjacency_list_), next_edge = edge_itr; edge_itr != edge_itr_end; edge_itr = next_edge) { - next_edge++; // next_edge iterator is necessary, because removing an edge invalidates the iterator to the current edge + ++next_edge; // next_edge iterator is necessary, because removing an edge invalidates the iterator to the current edge bool is_convex = sv_adjacency_list_[*edge_itr].is_convex; if (is_convex) // If edge is (0-)convex { - kcount = 0; + unsigned int kcount = 0; const VertexID source = boost::source (*edge_itr, sv_adjacency_list_); const VertexID target = boost::target (*edge_itr, sv_adjacency_list_); - source_neighbors_range = boost::out_edges (source, sv_adjacency_list_); - target_neighbors_range = boost::out_edges (target, sv_adjacency_list_); - + OutEdgeIterator source_neighbors_itr, source_neighbors_itr_end; // Find common neighbors, check their connection - for (OutEdgeIterator source_neighbors_itr = source_neighbors_range.first; source_neighbors_itr != source_neighbors_range.second; ++source_neighbors_itr) // For all supervoxels + for (std::tie(source_neighbors_itr, source_neighbors_itr_end) = boost::out_edges (source, sv_adjacency_list_); source_neighbors_itr != source_neighbors_itr_end; ++source_neighbors_itr) // For all supervoxels { VertexID source_neighbor_ID = boost::target (*source_neighbors_itr, sv_adjacency_list_); - for (OutEdgeIterator target_neighbors_itr = target_neighbors_range.first; target_neighbors_itr != target_neighbors_range.second; ++target_neighbors_itr) // For all supervoxels + OutEdgeIterator target_neighbors_itr, target_neighbors_itr_end; + for (std::tie(target_neighbors_itr, target_neighbors_itr_end) = boost::out_edges (target, sv_adjacency_list_); target_neighbors_itr != target_neighbors_itr_end; ++target_neighbors_itr) // For all supervoxels { VertexID target_neighbor_ID = boost::target (*target_neighbors_itr, sv_adjacency_list_); if (source_neighbor_ID == target_neighbor_ID) // Common neighbor @@ -431,11 +413,9 @@ pcl::LCCPSegmentation::calculateConvexConnections (SupervoxelAdjacencyLi { EdgeIterator edge_itr, edge_itr_end, next_edge; - boost::tie (edge_itr, edge_itr_end) = boost::edges (adjacency_list_arg); - - for (next_edge = edge_itr; edge_itr != edge_itr_end; edge_itr = next_edge) + for (std::tie(edge_itr, edge_itr_end) = boost::edges (adjacency_list_arg), next_edge = edge_itr; edge_itr != edge_itr_end; edge_itr = next_edge) { - next_edge++; // next_edge iterator is necessary, because removing an edge invalidates the iterator to the current edge + ++next_edge; // next_edge iterator is necessary, because removing an edge invalidates the iterator to the current edge std::uint32_t source_sv_label = adjacency_list_arg[boost::source (*edge_itr, adjacency_list_arg)]; std::uint32_t target_sv_label = adjacency_list_arg[boost::target (*edge_itr, adjacency_list_arg)]; diff --git a/segmentation/include/pcl/segmentation/impl/min_cut_segmentation.hpp b/segmentation/include/pcl/segmentation/impl/min_cut_segmentation.hpp index c95aa40d..34abc08a 100644 --- a/segmentation/include/pcl/segmentation/impl/min_cut_segmentation.hpp +++ b/segmentation/include/pcl/segmentation/impl/min_cut_segmentation.hpp @@ -238,11 +238,10 @@ pcl::MinCutSegmentation::extract (std::vector & clust } clusters_.clear (); - bool success = true; if ( !graph_is_valid_ ) { - success = buildGraph (); + bool success = buildGraph (); if (!success) { deinitCompute (); @@ -255,7 +254,7 @@ pcl::MinCutSegmentation::extract (std::vector & clust if ( !unary_potentials_are_valid_ ) { - success = recalculateUnaryPotentials (); + bool success = recalculateUnaryPotentials (); if (!success) { deinitCompute (); @@ -266,7 +265,7 @@ pcl::MinCutSegmentation::extract (std::vector & clust if ( !binary_potentials_are_valid_ ) { - success = recalculateBinaryPotentials (); + bool success = recalculateBinaryPotentials (); if (!success) { deinitCompute (); @@ -339,7 +338,7 @@ pcl::MinCutSegmentation::buildGraph () for (int i_point = 0; i_point < number_of_indices; i_point++) { - int point_index = (*indices_)[i_point]; + index_t point_index = (*indices_)[i_point]; double source_weight = 0.0; double sink_weight = 0.0; calculateUnaryPotential (point_index, source_weight, sink_weight); @@ -352,7 +351,7 @@ pcl::MinCutSegmentation::buildGraph () search_->setInputCloud (input_, indices_); for (int i_point = 0; i_point < number_of_indices; i_point++) { - int point_index = (*indices_)[i_point]; + index_t point_index = (*indices_)[i_point]; search_->nearestKSearch (i_point, number_of_neighbours_, neighbours, distances); for (std::size_t i_nghbr = 1; i_nghbr < neighbours.size (); i_nghbr++) { @@ -373,8 +372,6 @@ pcl::MinCutSegmentation::calculateUnaryPotential (int point, double& sou { double min_dist_to_foreground = std::numeric_limits::max (); //double min_dist_to_background = std::numeric_limits::max (); - double closest_foreground_point[2]; - closest_foreground_point[0] = closest_foreground_point[1] = 0; //double closest_background_point[] = {0.0, 0.0}; double initial_point[] = {0.0, 0.0}; @@ -389,8 +386,6 @@ pcl::MinCutSegmentation::calculateUnaryPotential (int point, double& sou if (min_dist_to_foreground > dist) { min_dist_to_foreground = dist; - closest_foreground_point[0] = foreground_points_[i_point].x; - closest_foreground_point[1] = foreground_points_[i_point].y; } } @@ -587,10 +582,9 @@ pcl::MinCutSegmentation::getColoredCloud () colored_cloud->is_dense = input_->is_dense; pcl::PointXYZRGB point; - int point_index = 0; for (int i_point = 0; i_point < num_of_pts_in_first_cluster; i_point++) { - point_index = clusters_[0].indices[i_point]; + index_t point_index = clusters_[0].indices[i_point]; point.x = *(input_->points[point_index].data); point.y = *(input_->points[point_index].data + 1); point.z = *(input_->points[point_index].data + 2); @@ -602,7 +596,7 @@ pcl::MinCutSegmentation::getColoredCloud () for (int i_point = 0; i_point < num_of_pts_in_second_cluster; i_point++) { - point_index = clusters_[1].indices[i_point]; + index_t point_index = clusters_[1].indices[i_point]; point.x = *(input_->points[point_index].data); point.y = *(input_->points[point_index].data + 1); point.z = *(input_->points[point_index].data + 2); diff --git a/segmentation/include/pcl/segmentation/impl/organized_multi_plane_segmentation.hpp b/segmentation/include/pcl/segmentation/impl/organized_multi_plane_segmentation.hpp index 5928e92c..be0a6a7b 100644 --- a/segmentation/include/pcl/segmentation/impl/organized_multi_plane_segmentation.hpp +++ b/segmentation/include/pcl/segmentation/impl/organized_multi_plane_segmentation.hpp @@ -91,6 +91,13 @@ pcl::OrganizedMultiPlaneSegmentation::segment (std::ve if (!initCompute ()) return; + // Check that the normals are present + if (!normals_) + { + PCL_ERROR( "[pcl::%s::segment] Must specify normals.\n", getClassName().c_str()); + return; + } + // Check that we got the same number of points and normals if (static_cast (normals_->points.size ()) != static_cast (input_->points.size ())) { diff --git a/segmentation/include/pcl/segmentation/impl/region_growing.hpp b/segmentation/include/pcl/segmentation/impl/region_growing.hpp index f65ad66c..f1e22d37 100644 --- a/segmentation/include/pcl/segmentation/impl/region_growing.hpp +++ b/segmentation/include/pcl/segmentation/impl/region_growing.hpp @@ -37,15 +37,15 @@ * */ -#ifndef PCL_SEGMENTATION_REGION_GROWING_HPP_ -#define PCL_SEGMENTATION_REGION_GROWING_HPP_ +#pragma once #include -#include -#include #include #include +#include // for pcl::isFinite +#include +#include #include #include @@ -741,4 +741,3 @@ pcl::RegionGrowing::getColoredCloudRGBA () #define PCL_INSTANTIATE_RegionGrowing(T) template class pcl::RegionGrowing; -#endif // PCL_SEGMENTATION_REGION_GROWING_HPP_ diff --git a/segmentation/include/pcl/segmentation/impl/sac_segmentation.hpp b/segmentation/include/pcl/segmentation/impl/sac_segmentation.hpp index 0b36c56c..ac77f22d 100644 --- a/segmentation/include/pcl/segmentation/impl/sac_segmentation.hpp +++ b/segmentation/include/pcl/segmentation/impl/sac_segmentation.hpp @@ -70,6 +70,8 @@ #include #include +#include // for static_pointer_cast + ////////////////////////////////////////////////////////////////////////////////////////////// template void pcl::SACSegmentation::segment (PointIndices &inliers, ModelCoefficients &model_coefficients) @@ -167,7 +169,7 @@ pcl::SACSegmentation::initSACModel (const int model_type) { PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_CIRCLE2D\n", getClassName ().c_str ()); model_.reset (new SampleConsensusModelCircle2D (input_, *indices_, random_)); - typename SampleConsensusModelCircle2D::Ptr model_circle = boost::static_pointer_cast > (model_); + typename SampleConsensusModelCircle2D::Ptr model_circle = static_pointer_cast > (model_); double min_radius, max_radius; model_circle->getRadiusLimits (min_radius, max_radius); if (radius_min_ != min_radius && radius_max_ != max_radius) @@ -181,7 +183,7 @@ pcl::SACSegmentation::initSACModel (const int model_type) { PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_CIRCLE3D\n", getClassName ().c_str ()); model_.reset (new SampleConsensusModelCircle3D (input_, *indices_)); - typename SampleConsensusModelCircle3D::Ptr model_circle3d = boost::static_pointer_cast > (model_); + typename SampleConsensusModelCircle3D::Ptr model_circle3d = static_pointer_cast > (model_); double min_radius, max_radius; model_circle3d->getRadiusLimits (min_radius, max_radius); if (radius_min_ != min_radius && radius_max_ != max_radius) @@ -195,7 +197,7 @@ pcl::SACSegmentation::initSACModel (const int model_type) { PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_SPHERE\n", getClassName ().c_str ()); model_.reset (new SampleConsensusModelSphere (input_, *indices_, random_)); - typename SampleConsensusModelSphere::Ptr model_sphere = boost::static_pointer_cast > (model_); + typename SampleConsensusModelSphere::Ptr model_sphere = static_pointer_cast > (model_); double min_radius, max_radius; model_sphere->getRadiusLimits (min_radius, max_radius); if (radius_min_ != min_radius && radius_max_ != max_radius) @@ -209,7 +211,7 @@ pcl::SACSegmentation::initSACModel (const int model_type) { PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PARALLEL_LINE\n", getClassName ().c_str ()); model_.reset (new SampleConsensusModelParallelLine (input_, *indices_, random_)); - typename SampleConsensusModelParallelLine::Ptr model_parallel = boost::static_pointer_cast > (model_); + typename SampleConsensusModelParallelLine::Ptr model_parallel = static_pointer_cast > (model_); if (axis_ != Eigen::Vector3f::Zero () && model_parallel->getAxis () != axis_) { PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]); @@ -226,7 +228,7 @@ pcl::SACSegmentation::initSACModel (const int model_type) { PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PERPENDICULAR_PLANE\n", getClassName ().c_str ()); model_.reset (new SampleConsensusModelPerpendicularPlane (input_, *indices_, random_)); - typename SampleConsensusModelPerpendicularPlane::Ptr model_perpendicular = boost::static_pointer_cast > (model_); + typename SampleConsensusModelPerpendicularPlane::Ptr model_perpendicular = static_pointer_cast > (model_); if (axis_ != Eigen::Vector3f::Zero () && model_perpendicular->getAxis () != axis_) { PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]); @@ -243,7 +245,7 @@ pcl::SACSegmentation::initSACModel (const int model_type) { PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PARALLEL_PLANE\n", getClassName ().c_str ()); model_.reset (new SampleConsensusModelParallelPlane (input_, *indices_, random_)); - typename SampleConsensusModelParallelPlane::Ptr model_parallel = boost::static_pointer_cast > (model_); + typename SampleConsensusModelParallelPlane::Ptr model_parallel = static_pointer_cast > (model_); if (axis_ != Eigen::Vector3f::Zero () && model_parallel->getAxis () != axis_) { PCL_DEBUG ("[pcl::%s::initSACModel] Setting the axis to %f, %f, %f\n", getClassName ().c_str (), axis_[0], axis_[1], axis_[2]); @@ -368,7 +370,7 @@ pcl::SACSegmentationFromNormals::initSACModel (const int model_ { PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_CYLINDER\n", getClassName ().c_str ()); model_.reset (new SampleConsensusModelCylinder (input_, *indices_, random_)); - typename SampleConsensusModelCylinder::Ptr model_cylinder = boost::static_pointer_cast > (model_); + typename SampleConsensusModelCylinder::Ptr model_cylinder = static_pointer_cast > (model_); // Set the input normals model_cylinder->setInputNormals (normals_); @@ -400,7 +402,7 @@ pcl::SACSegmentationFromNormals::initSACModel (const int model_ { PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_NORMAL_PLANE\n", getClassName ().c_str ()); model_.reset (new SampleConsensusModelNormalPlane (input_, *indices_, random_)); - typename SampleConsensusModelNormalPlane::Ptr model_normals = boost::static_pointer_cast > (model_); + typename SampleConsensusModelNormalPlane::Ptr model_normals = static_pointer_cast > (model_); // Set the input normals model_normals->setInputNormals (normals_); if (distance_weight_ != model_normals->getNormalDistanceWeight ()) @@ -414,7 +416,7 @@ pcl::SACSegmentationFromNormals::initSACModel (const int model_ { PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_NORMAL_PARALLEL_PLANE\n", getClassName ().c_str ()); model_.reset (new SampleConsensusModelNormalParallelPlane (input_, *indices_, random_)); - typename SampleConsensusModelNormalParallelPlane::Ptr model_normals = boost::static_pointer_cast > (model_); + typename SampleConsensusModelNormalParallelPlane::Ptr model_normals = static_pointer_cast > (model_); // Set the input normals model_normals->setInputNormals (normals_); if (distance_weight_ != model_normals->getNormalDistanceWeight ()) @@ -443,7 +445,7 @@ pcl::SACSegmentationFromNormals::initSACModel (const int model_ { PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_CONE\n", getClassName ().c_str ()); model_.reset (new SampleConsensusModelCone (input_, *indices_, random_)); - typename SampleConsensusModelCone::Ptr model_cone = boost::static_pointer_cast > (model_); + typename SampleConsensusModelCone::Ptr model_cone = static_pointer_cast > (model_); // Set the input normals model_cone->setInputNormals (normals_); @@ -476,7 +478,7 @@ pcl::SACSegmentationFromNormals::initSACModel (const int model_ { PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_NORMAL_SPHERE\n", getClassName ().c_str ()); model_.reset (new SampleConsensusModelNormalSphere (input_, *indices_, random_)); - typename SampleConsensusModelNormalSphere::Ptr model_normals_sphere = boost::static_pointer_cast > (model_); + typename SampleConsensusModelNormalSphere::Ptr model_normals_sphere = static_pointer_cast > (model_); // Set the input normals model_normals_sphere->setInputNormals (normals_); double min_radius, max_radius; diff --git a/segmentation/include/pcl/segmentation/impl/segment_differences.hpp b/segmentation/include/pcl/segmentation/impl/segment_differences.hpp index 32c27bf7..7c499593 100755 --- a/segmentation/include/pcl/segmentation/impl/segment_differences.hpp +++ b/segmentation/include/pcl/segmentation/impl/segment_differences.hpp @@ -35,11 +35,13 @@ * */ -#ifndef PCL_SEGMENTATION_IMPL_SEGMENT_DIFFERENCES_H_ -#define PCL_SEGMENTATION_IMPL_SEGMENT_DIFFERENCES_H_ +#pragma once #include + #include +#include // for pcl::isFinite + ////////////////////////////////////////////////////////////////////////// template void @@ -121,4 +123,3 @@ pcl::SegmentDifferences::segment (PointCloud &output) #define PCL_INSTANTIATE_SegmentDifferences(T) template class PCL_EXPORTS pcl::SegmentDifferences; #define PCL_INSTANTIATE_getPointCloudDifference(T) template PCL_EXPORTS void pcl::getPointCloudDifference(const pcl::PointCloud &, double, const typename pcl::search::Search::Ptr &, pcl::PointCloud &); -#endif // PCL_SEGMENTATION_IMPL_SEGMENT_DIFFERENCES_H_ diff --git a/segmentation/include/pcl/segmentation/lccp_segmentation.h b/segmentation/include/pcl/segmentation/lccp_segmentation.h index 5c66eb45..ad7a2cc7 100644 --- a/segmentation/include/pcl/segmentation/lccp_segmentation.h +++ b/segmentation/include/pcl/segmentation/lccp_segmentation.h @@ -62,18 +62,18 @@ namespace pcl { /** \brief Describes the difference of normals of the two supervoxels being connected*/ float normal_difference; - + /** \brief Describes if a connection is convex or concave*/ bool is_convex; - + /** \brief Describes if a connection is valid for the segment growing. Usually convex connections are and concave connection are not. Due to k-concavity a convex connection can be invalidated*/ bool is_valid; - + /** \brief Additional member used for the CPC algorithm. If edge has already induced a cut, it should be ignored for further cutting.*/ bool used_for_cutting; - + EdgeProperties () : - normal_difference (0), is_convex (false), is_valid (false), used_for_cutting (false) + normal_difference (0), is_convex (false), is_valid (false), used_for_cutting (false) { } }; @@ -98,10 +98,10 @@ namespace pcl void reset (); - + /** \brief Set the supervoxel clusters as well as the adjacency graph for the segmentation.Those parameters are generated by using the \ref SupervoxelClustering class. To retrieve the output use the \ref segment method. * \param[in] supervoxel_clusters_arg Map of < supervoxel labels, supervoxels > - * \param[in] label_adjacency_arg The graph defining the supervoxel adjacency relations + * \param[in] label_adjacency_arg The graph defining the supervoxel adjacency relations * \note Implicitly calls \ref reset */ inline void setInputSupervoxels (const std::map::Ptr> &supervoxel_clusters_arg, @@ -111,17 +111,17 @@ namespace pcl prepareSegmentation (supervoxel_clusters_arg, label_adjacency_arg); // after this, sv_adjacency_list_ can be used to access adjacency list supervoxels_set_ = true; } - + /** \brief Merge supervoxels using local convexity. The input parameters are generated by using the \ref SupervoxelClustering class. To retrieve the output use the \ref relabelCloud method. - * \note There are three ways to retrieve the segmentation afterwards: \ref relabelCloud, \ref getSegmentSupervoxelMap and \ref getSupervoxelSegmentMap*/ + * \note There are three ways to retrieve the segmentation afterwards: \ref relabelCloud, \ref getSegmentToSupervoxelMap and \ref getSupervoxelToSegmentMap. */ void segment (); - /** \brief Relabels cloud with supervoxel labels with the computed segment labels. labeled_cloud_arg should be created using the \ref getLabeledCloud method of the \ref SupervoxelClustering class. + /** \brief Relabels cloud with supervoxel labels with the computed segment labels. labeled_cloud_arg should be created using SupervoxelClustering::getLabeledCloud. * \param[in,out] labeled_cloud_arg Cloud to relabel */ void relabelCloud (pcl::PointCloud &labeled_cloud_arg); - + /** \brief Get map > * \param[out] segment_supervoxel_map_arg The output container. On error the map is empty. */ inline void @@ -137,7 +137,7 @@ namespace pcl segment_supervoxel_map_arg = std::map > (); } } - + /** \brief Get map * \param[out] supervoxel_segment_map_arg The output container. On error the map is empty. */ inline void @@ -153,7 +153,7 @@ namespace pcl supervoxel_segment_map_arg = std::map (); } } - + /** \brief Get map > * \param[out] segment_adjacency_map_arg map < SegmentID, std::set< Neighboring SegmentIDs> >. On error the map is empty. */ inline void @@ -171,7 +171,7 @@ namespace pcl segment_adjacency_map_arg = std::map > (); } } - + /** \brief Get normal threshold * \return The concavity tolerance angle in [deg] that is currently set */ inline float @@ -179,7 +179,7 @@ namespace pcl { return (concavity_tolerance_threshold_); } - + /** \brief Get the supervoxel adjacency graph with classified edges (boost::adjacency_list). * \param[out] adjacency_list_arg The supervoxel adjacency list with classified (convex/concave) edges. On error the list is empty. */ inline void @@ -195,7 +195,7 @@ namespace pcl adjacency_list_arg = pcl::LCCPSegmentation::SupervoxelAdjacencyList (); } } - + /** \brief Set normal threshold * \param[in] concavity_tolerance_threshold_arg the concavity tolerance angle in [deg] to set */ inline void @@ -236,7 +236,7 @@ namespace pcl { k_factor_ = k_factor_arg; } - + /** \brief Set the value \ref min_segment_size_ used in \ref mergeSmallSegments * \param[in] min_segment_size_arg Segments smaller than this size will be merged */ inline void @@ -267,7 +267,7 @@ namespace pcl * \note The vertices in the supervoxel adjacency list are the supervoxel centroids */ void doGrouping (); - + /** \brief Assigns neighbors of the query point to the same group as the query point. Recursive part of \ref doGrouping. Grouping is done by a depth-search of nodes in the adjacency-graph. * \param[in] queryPointID ID of point whose neighbors will be considered for grouping * \param[in] group_label ID of the group/segment the queried point belongs to */ @@ -302,7 +302,7 @@ namespace pcl /** \brief Marks if valid grouping data (\ref sv_adjacency_list_, \ref sv_label_to_seg_label_map_, \ref processed_) is available */ bool grouping_data_valid_; - + /** \brief Marks if supervoxels have been set by calling \ref setInputSupervoxels */ bool supervoxels_set_; @@ -314,7 +314,7 @@ namespace pcl /** \brief Determines if we use the sanity check which tries to find and invalidate singular connected patches*/ bool use_sanity_check_; - + /** \brief Seed resolution of the supervoxels (used only for smoothness check) */ float seed_resolution_; @@ -323,11 +323,11 @@ namespace pcl /** \brief Factor used for k-convexity */ std::uint32_t k_factor_; - + /** \brief Minimum segment size */ std::uint32_t min_segment_size_; - /** \brief Stores which supervoxel labels were already visited during recursive grouping. + /** \brief Stores which supervoxel labels were already visited during recursive grouping. * \note processed_[sv_Label] = false (default)/true (already processed) */ std::map processed_; @@ -337,11 +337,11 @@ namespace pcl /** \brief map from the supervoxel labels to the supervoxel objects */ std::map::Ptr> sv_label_to_supervoxel_map_; - /** \brief Storing relation between original SuperVoxel Labels and new segmantion labels. + /** \brief Storing relation between original SuperVoxel Labels and new segmantion labels. * \note sv_label_to_seg_label_map_[old_labelID] = new_labelID */ std::map sv_label_to_seg_label_map_; - /** \brief map > */ + /** \brief map Segment Label to a set of Supervoxel Labels */ std::map > seg_label_to_sv_list_map_; /** \brief map < SegmentID, std::set< Neighboring segment labels> > */ diff --git a/segmentation/include/pcl/segmentation/min_cut_segmentation.h b/segmentation/include/pcl/segmentation/min_cut_segmentation.h index 5307e32f..f2a26897 100644 --- a/segmentation/include/pcl/segmentation/min_cut_segmentation.h +++ b/segmentation/include/pcl/segmentation/min_cut_segmentation.h @@ -39,6 +39,7 @@ #pragma once #include +#include #include #include #include @@ -156,7 +157,7 @@ namespace pcl /** \brief Allows to set search method for finding KNN. * The graph is build such way that it contains the edges that connect point and its KNN. - * \param[in] search search method that will be used for finding KNN. + * \param[in] tree search method that will be used for finding KNN. */ void setSearchMethod (const KdTreePtr& tree); @@ -166,7 +167,7 @@ namespace pcl getNumberOfNeighbours () const; /** \brief Allows to set the number of neighbours to find. - * \param[in] number_of_neighbours new number of neighbours + * \param[in] neighbour_number new number of neighbours */ void setNumberOfNeighbours (unsigned int neighbour_number); diff --git a/segmentation/include/pcl/segmentation/organized_multi_plane_segmentation.h b/segmentation/include/pcl/segmentation/organized_multi_plane_segmentation.h index e73b7a24..26edba5c 100644 --- a/segmentation/include/pcl/segmentation/organized_multi_plane_segmentation.h +++ b/segmentation/include/pcl/segmentation/organized_multi_plane_segmentation.h @@ -43,6 +43,7 @@ #include #include #include +#include #include #include #include @@ -285,15 +286,17 @@ namespace pcl * \param [in] labels The labels produced by the initial segmentation * \param [in] label_indices The list of indices corresponding to each label */ - PCL_DEPRECATED("centroids and covariances parameters are not used; they are deprecated and will be removed in future releases") + PCL_DEPRECATED(1, 12, "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, - std::vector >& /*centroids*/, - std::vector >& /*covariances*/, + std::vector >& centroids, + std::vector >& covariances, PointCloudLPtr& labels, std::vector& label_indices) { + pcl::utils::ignore(centroids); + pcl::utils::ignore(covariances); refine(model_coefficients, inlier_indices, labels, label_indices); } diff --git a/segmentation/include/pcl/segmentation/planar_region.h b/segmentation/include/pcl/segmentation/planar_region.h index 69f1ec29..5897ce27 100644 --- a/segmentation/include/pcl/segmentation/planar_region.h +++ b/segmentation/include/pcl/segmentation/planar_region.h @@ -37,6 +37,7 @@ #pragma once +#include #include #include #include diff --git a/segmentation/include/pcl/segmentation/plane_coefficient_comparator.h b/segmentation/include/pcl/segmentation/plane_coefficient_comparator.h index d7933974..a7f6ba9d 100644 --- a/segmentation/include/pcl/segmentation/plane_coefficient_comparator.h +++ b/segmentation/include/pcl/segmentation/plane_coefficient_comparator.h @@ -40,8 +40,8 @@ #pragma once #include +#include // for pcl::make_shared, pcl::shared_ptr #include -#include #include namespace pcl @@ -57,16 +57,16 @@ namespace pcl public: using PointCloud = typename Comparator::PointCloud; using PointCloudConstPtr = typename Comparator::PointCloudConstPtr; - + using PointCloudN = pcl::PointCloud; using PointCloudNPtr = typename PointCloudN::Ptr; using PointCloudNConstPtr = typename PointCloudN::ConstPtr; - + using Ptr = shared_ptr >; using ConstPtr = shared_ptr >; using pcl::Comparator::input_; - + /** \brief Empty constructor for PlaneCoefficientComparator. */ PlaneCoefficientComparator () : normals_ () @@ -80,7 +80,7 @@ namespace pcl /** \brief Constructor for PlaneCoefficientComparator. * \param[in] plane_coeff_d a reference to a vector of d coefficients of plane equations. Must be the same size as the input cloud and input normals. a, b, and c coefficients are in the input normals. */ - PlaneCoefficientComparator (shared_ptr >& plane_coeff_d) + PlaneCoefficientComparator (shared_ptr >& plane_coeff_d) : normals_ () , plane_coeff_d_ (plane_coeff_d) , angular_threshold_ (pcl::deg2rad (2.0f)) @@ -89,19 +89,19 @@ namespace pcl , z_axis_ (Eigen::Vector3f (0.0f, 0.0f, 1.0f) ) { } - + /** \brief Destructor for PlaneCoefficientComparator. */ - + ~PlaneCoefficientComparator () { } - void + void setInputCloud (const PointCloudConstPtr& cloud) override { input_ = cloud; } - + /** \brief Provide a pointer to the input normals. * \param[in] normals the input normal cloud */ @@ -133,9 +133,9 @@ namespace pcl void setPlaneCoeffD (std::vector& plane_coeff_d) { - plane_coeff_d_ = boost::make_shared >(plane_coeff_d); + plane_coeff_d_ = pcl::make_shared >(plane_coeff_d); } - + /** \brief Get a pointer to the vector of the d-coefficient of the planes' hessian normal form. */ const std::vector& getPlaneCoeffD () const @@ -151,7 +151,7 @@ namespace pcl { angular_threshold_ = std::cos (angular_threshold); } - + /** \brief Get the angular threshold in radians for difference in normal direction between neighboring points, to be considered part of the same plane. */ inline float getAngularThreshold () const @@ -164,7 +164,7 @@ namespace pcl * \param[in] depth_dependent whether to scale the threshold based on range from the sensor (default: false) */ void - setDistanceThreshold (float distance_threshold, + setDistanceThreshold (float distance_threshold, bool depth_dependent = false) { distance_threshold_ = distance_threshold; @@ -177,7 +177,7 @@ namespace pcl { return (distance_threshold_); } - + /** \brief Compare points at two indices by their plane equations. True if the angle between the normals is less than the angular threshold, * and the difference between the d component of the normals is less than distance threshold, else false * \param idx1 The first index for the comparison @@ -190,14 +190,14 @@ namespace pcl if (depth_dependent_) { Eigen::Vector3f vec = input_->points[idx1].getVector3fMap (); - + float z = vec.dot (z_axis_); threshold *= z * z; } return ( (std::fabs ((*plane_coeff_d_)[idx1] - (*plane_coeff_d_)[idx2]) < threshold) && (normals_->points[idx1].getNormalVector3fMap ().dot (normals_->points[idx2].getNormalVector3fMap () ) > angular_threshold_ ) ); } - + protected: PointCloudNConstPtr normals_; shared_ptr > plane_coeff_d_; diff --git a/segmentation/include/pcl/segmentation/plane_refinement_comparator.h b/segmentation/include/pcl/segmentation/plane_refinement_comparator.h index 7c4bb0ec..735e152a 100644 --- a/segmentation/include/pcl/segmentation/plane_refinement_comparator.h +++ b/segmentation/include/pcl/segmentation/plane_refinement_comparator.h @@ -39,12 +39,12 @@ #pragma once -#include +#include // for pcl::make_shared #include namespace pcl { - /** \brief PlaneRefinementComparator is a Comparator that operates on plane coefficients, + /** \brief PlaneRefinementComparator is a Comparator that operates on plane coefficients, * for use in planar segmentation. * In conjunction with OrganizedConnectedComponentSegmentation, this allows planes to be segmented from organized data. * @@ -56,7 +56,7 @@ namespace pcl public: using PointCloud = typename Comparator::PointCloud; using PointCloudConstPtr = typename Comparator::PointCloudConstPtr; - + using PointCloudN = pcl::PointCloud; using PointCloudNPtr = typename PointCloudN::Ptr; using PointCloudNConstPtr = typename PointCloudN::ConstPtr; @@ -81,7 +81,7 @@ namespace pcl { } - /** \brief Empty constructor for PlaneCoefficientComparator. + /** \brief Empty constructor for PlaneCoefficientComparator. * \param[in] models * \param[in] refine_labels */ @@ -95,7 +95,6 @@ namespace pcl } /** \brief Destructor for PlaneCoefficientComparator. */ - ~PlaneRefinementComparator () { } @@ -115,7 +114,7 @@ namespace pcl void setModelCoefficients (std::vector& models) { - models_ = boost::make_shared >(models); + models_ = pcl::make_shared >(models); } /** \brief Set which labels should be refined. This is a vector of bools 0-max_label, true if the label should be refined. @@ -126,14 +125,14 @@ namespace pcl { refine_labels_ = refine_labels; } - + /** \brief Set which labels should be refined. This is a vector of bools 0-max_label, true if the label should be refined. * \param[in] refine_labels A vector of bools 0-max_label, true if the label should be refined. */ void setRefineLabels (std::vector& refine_labels) { - refine_labels_ = boost::make_shared >(refine_labels); + refine_labels_ = pcl::make_shared >(refine_labels); } /** \brief A mapping from label to index in the vector of models, allowing the model coefficients of a label to be accessed. @@ -144,14 +143,14 @@ namespace pcl { label_to_model_ = label_to_model; } - + /** \brief A mapping from label to index in the vector of models, allowing the model coefficients of a label to be accessed. * \param[in] label_to_model A vector of size max_label, with the index of each corresponding model in models */ inline void setLabelToModel (std::vector& label_to_model) { - label_to_model_ = boost::make_shared >(label_to_model); + label_to_model_ = pcl::make_shared >(label_to_model); } /** \brief Get the vector of model coefficients to which we will compare. */ @@ -182,26 +181,26 @@ namespace pcl if (!((*refine_labels_)[current_label] && !(*refine_labels_)[next_label])) return (false); - + const pcl::ModelCoefficients& model_coeff = (*models_)[(*label_to_model_)[current_label]]; - + PointT pt = input_->points[idx2]; - double ptp_dist = std::fabs (model_coeff.values[0] * pt.x + - model_coeff.values[1] * pt.y + + double ptp_dist = std::fabs (model_coeff.values[0] * pt.x + + model_coeff.values[1] * pt.y + model_coeff.values[2] * pt.z + model_coeff.values[3]); - + // depth dependent float threshold = distance_threshold_; if (depth_dependent_) { //Eigen::Vector4f origin = input_->sensor_origin_; Eigen::Vector3f vec = input_->points[idx1].getVector3fMap ();// - origin.head<3> (); - + float z = vec.dot (z_axis_); threshold *= z * z; } - + return (ptp_dist < threshold); } diff --git a/segmentation/include/pcl/segmentation/region_3d.h b/segmentation/include/pcl/segmentation/region_3d.h index 99a7439b..d68b093e 100644 --- a/segmentation/include/pcl/segmentation/region_3d.h +++ b/segmentation/include/pcl/segmentation/region_3d.h @@ -40,6 +40,7 @@ #include #include +#include #include namespace pcl diff --git a/segmentation/include/pcl/segmentation/region_growing.h b/segmentation/include/pcl/segmentation/region_growing.h index c61e2f31..445842c7 100644 --- a/segmentation/include/pcl/segmentation/region_growing.h +++ b/segmentation/include/pcl/segmentation/region_growing.h @@ -39,6 +39,7 @@ #pragma once +#include #include #include #include @@ -81,7 +82,7 @@ namespace pcl /** \brief This destructor destroys the cloud, normals and search method used for * finding KNN. In other words it frees memory. */ - + ~RegionGrowing (); /** \brief Get the minimum number of points that a cluster needs to contain in order to be considered valid. */ diff --git a/segmentation/include/pcl/segmentation/region_growing_rgb.h b/segmentation/include/pcl/segmentation/region_growing_rgb.h index 03f06656..f1799599 100644 --- a/segmentation/include/pcl/segmentation/region_growing_rgb.h +++ b/segmentation/include/pcl/segmentation/region_growing_rgb.h @@ -39,6 +39,7 @@ #pragma once +#include #include #include diff --git a/segmentation/include/pcl/segmentation/sac_segmentation.h b/segmentation/include/pcl/segmentation/sac_segmentation.h index 754f7af6..2add3f73 100644 --- a/segmentation/include/pcl/segmentation/sac_segmentation.h +++ b/segmentation/include/pcl/segmentation/sac_segmentation.h @@ -358,7 +358,7 @@ namespace pcl /** \brief Provide a pointer to the input dataset that contains the point normals of * the XYZ dataset. - * \param[in] normals the const boost shared pointer to a PointCloud message + * \param[in] normals the const shared pointer to a PointCloud message */ inline void setInputNormals (const PointCloudNConstPtr &normals) { normals_ = normals; } diff --git a/segmentation/include/pcl/segmentation/segment_differences.h b/segmentation/include/pcl/segmentation/segment_differences.h index ecd181b8..2428a61d 100755 --- a/segmentation/include/pcl/segmentation/segment_differences.h +++ b/segmentation/include/pcl/segmentation/segment_differences.h @@ -60,7 +60,7 @@ namespace pcl pcl::PointCloud &output); template - PCL_DEPRECATED("tgt parameter is not used; it is deprecated and will be removed in future releases") + PCL_DEPRECATED(1, 12, "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 5ce354c1..1c43f406 100644 --- a/segmentation/include/pcl/segmentation/supervoxel_clustering.h +++ b/segmentation/include/pcl/segmentation/supervoxel_clustering.h @@ -43,6 +43,7 @@ #include #include +#include #include #include #include @@ -69,7 +70,7 @@ namespace pcl Supervoxel () : voxels_ (new pcl::PointCloud ()), normals_ (new pcl::PointCloud ()) - { } + { } using Ptr = shared_ptr >; using ConstPtr = shared_ptr >; @@ -194,7 +195,7 @@ namespace pcl */ SupervoxelClustering (float voxel_resolution, float seed_resolution); - 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.") + PCL_DEPRECATED(1, 12, "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,10 +279,10 @@ 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 */ - 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") + PCL_DEPRECATED(1, 12, "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 - { + { return pcl::PointCloud::Ptr (new pcl::PointCloud); } diff --git a/segmentation/include/pcl/segmentation/unary_classifier.h b/segmentation/include/pcl/segmentation/unary_classifier.h index 2aa4bf43..25086923 100644 --- a/segmentation/include/pcl/segmentation/unary_classifier.h +++ b/segmentation/include/pcl/segmentation/unary_classifier.h @@ -39,6 +39,7 @@ #pragma once +#include #include #include #include diff --git a/simulation/include/pcl/simulation/camera.h b/simulation/include/pcl/simulation/camera.h index e9c854bd..c680219c 100644 --- a/simulation/include/pcl/simulation/camera.h +++ b/simulation/include/pcl/simulation/camera.h @@ -1,10 +1,10 @@ #pragma once +#include +#include + #include #include -#include - -#include namespace pcl { namespace simulation { @@ -138,7 +138,7 @@ public: // Return the pose of the camera: Eigen::Vector3d - getYPR() + getYPR() const { return Eigen::Vector3d(yaw_, pitch_, roll_); } diff --git a/simulation/include/pcl/simulation/glsl_shader.h b/simulation/include/pcl/simulation/glsl_shader.h index ea334c2f..655db3ce 100644 --- a/simulation/include/pcl/simulation/glsl_shader.h +++ b/simulation/include/pcl/simulation/glsl_shader.h @@ -7,11 +7,12 @@ #pragma once +#include #include #include + #include -#include namespace pcl { namespace simulation { @@ -39,7 +40,7 @@ public: /** Add a new shader object to the program. */ bool - addShaderText(const std::string& text, ShaderType shader_type); + addShaderText(const std::string& text, ShaderType shader_type) const; /** Add a new shader object to the program. */ bool @@ -47,7 +48,7 @@ public: /** Link the program. */ bool - link(); + link() const; /** Return true if the program is linked. */ bool @@ -55,7 +56,7 @@ public: /** Use the program. */ void - use(); + use() const; // Set uniforms void @@ -86,7 +87,7 @@ public: setUniform(const std::string& name, bool v); int - getUniformLocation(const std::string& name); + getUniformLocation(const std::string& name) const; void printActiveUniforms(); @@ -94,7 +95,7 @@ public: printActiveAttribs(); GLuint - programId() + programId() const { return program_id_; } diff --git a/simulation/include/pcl/simulation/model.h b/simulation/include/pcl/simulation/model.h index 5128d8d1..8b592f94 100644 --- a/simulation/include/pcl/simulation/model.h +++ b/simulation/include/pcl/simulation/model.h @@ -1,5 +1,13 @@ #pragma once +#include +#include +#include +#include +#include +#include +#include + #if defined(_WIN32) && !defined(APIENTRY) && !defined(__CYGWIN__) #define WIN32_LEAN_AND_MEAN 1 #include @@ -7,7 +15,6 @@ #include -#include #ifdef OPENGL_IS_A_FRAMEWORK #include #include @@ -16,13 +23,6 @@ #include #endif -#include -#include -#include -#include -#include -#include - namespace pcl { namespace simulation { @@ -161,7 +161,7 @@ public: /** Render the quad. */ void - render(); + render() const; private: GLuint quad_vbo_; @@ -176,7 +176,7 @@ public: ~TexturedQuad(); void - setTexture(const std::uint8_t* data); + setTexture(const std::uint8_t* data) const; void render(); diff --git a/simulation/include/pcl/simulation/range_likelihood.h b/simulation/include/pcl/simulation/range_likelihood.h index 6539924d..6b0d7e6e 100644 --- a/simulation/include/pcl/simulation/range_likelihood.h +++ b/simulation/include/pcl/simulation/range_likelihood.h @@ -1,8 +1,18 @@ #pragma once -#include - +#include +#include +#include +#include +#include +#include +#include #include +#include + +#include + +#include #ifdef OPENGL_IS_A_FRAMEWORK #include #include @@ -11,20 +21,9 @@ #include #endif -//#include -#include - -#include -//#include -#include -#include -#include -#include -#include -#include - namespace pcl { namespace simulation { + class PCL_EXPORTS RangeLikelihood { public: using Ptr = shared_ptr; @@ -252,7 +251,7 @@ private: applyCameraTransform(const Camera& camera); void - setupProjectionMatrix(); + setupProjectionMatrix() const; Scene::Ptr scene_; int rows_; diff --git a/simulation/include/pcl/simulation/scene.h b/simulation/include/pcl/simulation/scene.h index 287f8792..9fd7ad40 100644 --- a/simulation/include/pcl/simulation/scene.h +++ b/simulation/include/pcl/simulation/scene.h @@ -7,11 +7,9 @@ #pragma once -#include - +#include #include //#include - #include #include diff --git a/simulation/include/pcl/simulation/sum_reduce.h b/simulation/include/pcl/simulation/sum_reduce.h index 4424c925..098ecd05 100644 --- a/simulation/include/pcl/simulation/sum_reduce.h +++ b/simulation/include/pcl/simulation/sum_reduce.h @@ -7,18 +7,17 @@ #pragma once -#include - +#include +#include #include + +#include #ifdef OPENGL_IS_A_FRAMEWORK #include #else #include #endif -#include -#include - namespace pcl { namespace simulation { diff --git a/simulation/src/camera.cpp b/simulation/src/camera.cpp index f0244281..1e343c06 100644 --- a/simulation/src/camera.cpp +++ b/simulation/src/camera.cpp @@ -1,6 +1,7 @@ -#include #include +#include + using namespace Eigen; using namespace pcl::simulation; diff --git a/simulation/src/glsl_shader.cpp b/simulation/src/glsl_shader.cpp index 18cd8b04..3eb2c20b 100644 --- a/simulation/src/glsl_shader.cpp +++ b/simulation/src/glsl_shader.cpp @@ -5,9 +5,10 @@ * Author: hordurj */ +#include + #include #include -#include using namespace pcl::simulation::gllib; @@ -35,7 +36,7 @@ pcl::simulation::gllib::Program::Program() { program_id_ = glCreateProgram(); } pcl::simulation::gllib::Program::~Program() {} int -pcl::simulation::gllib::Program::getUniformLocation(const std::string& name) +pcl::simulation::gllib::Program::getUniformLocation(const std::string& name) const { return glGetUniformLocation(program_id_, name.c_str()); } @@ -103,7 +104,7 @@ pcl::simulation::gllib::Program::setUniform(const std::string& name, bool v) bool pcl::simulation::gllib::Program::addShaderText(const std::string& text, - ShaderType shader_type) + ShaderType shader_type) const { GLuint id; GLint compile_status; @@ -139,7 +140,7 @@ pcl::simulation::gllib::Program::addShaderFile(const std::string& filename, } bool -pcl::simulation::gllib::Program::link() +pcl::simulation::gllib::Program::link() const { glLinkProgram(program_id_); printProgramInfoLog(program_id_); @@ -148,7 +149,7 @@ pcl::simulation::gllib::Program::link() } void -pcl::simulation::gllib::Program::use() +pcl::simulation::gllib::Program::use() const { glUseProgram(program_id_); } diff --git a/simulation/src/model.cpp b/simulation/src/model.cpp index 165f0b35..a154d6ce 100644 --- a/simulation/src/model.cpp +++ b/simulation/src/model.cpp @@ -271,7 +271,7 @@ pcl::simulation::Quad::Quad() pcl::simulation::Quad::~Quad() { glDeleteBuffers(1, &quad_vbo_); } void -pcl::simulation::Quad::render() +pcl::simulation::Quad::render() const { glBindBuffer(GL_ARRAY_BUFFER, quad_vbo_); glEnableVertexAttribArray(0); @@ -313,7 +313,7 @@ pcl::simulation::TexturedQuad::TexturedQuad(int width, int height) pcl::simulation::TexturedQuad::~TexturedQuad() { glDeleteTextures(1, &texture_); } void -pcl::simulation::TexturedQuad::setTexture(const std::uint8_t* data) +pcl::simulation::TexturedQuad::setTexture(const std::uint8_t* data) const { glBindTexture(GL_TEXTURE_2D, texture_); glTexImage2D( diff --git a/simulation/src/range_likelihood.cpp b/simulation/src/range_likelihood.cpp index d722655d..01d42ec9 100644 --- a/simulation/src/range_likelihood.cpp +++ b/simulation/src/range_likelihood.cpp @@ -1,11 +1,10 @@ -#include -#include +#include +#include +#include #include #include - -#include #ifdef OPENGL_IS_A_FRAMEWORK #include #include @@ -14,8 +13,8 @@ #include #endif -#include -#include +#include +#include // For adding noise: static std::minstd_rand rng(std::random_device{}()); @@ -512,7 +511,7 @@ pcl::simulation::RangeLikelihood::sampleNormal(double sigma) } void -pcl::simulation::RangeLikelihood::setupProjectionMatrix() +pcl::simulation::RangeLikelihood::setupProjectionMatrix() const { glMatrixMode(GL_PROJECTION); glLoadIdentity(); diff --git a/simulation/tools/sim_terminal_demo.cpp b/simulation/tools/sim_terminal_demo.cpp index cd98bb8b..74ab0de3 100644 --- a/simulation/tools/sim_terminal_demo.cpp +++ b/simulation/tools/sim_terminal_demo.cpp @@ -10,8 +10,12 @@ * pcl_sim_terminal_demo 2 ../../../../kmcl/models/table_models/meta_model.ply */ +#include + #include -#include + +#include "simulation_io.hpp" + #include #include #ifdef _WIN32 @@ -19,8 +23,6 @@ #include #endif -#include "simulation_io.hpp" - using namespace Eigen; using namespace pcl; using namespace pcl::console; diff --git a/simulation/tools/sim_test_performance.cpp b/simulation/tools/sim_test_performance.cpp index 5e8ab6d6..c756c9b7 100644 --- a/simulation/tools/sim_test_performance.cpp +++ b/simulation/tools/sim_test_performance.cpp @@ -5,17 +5,27 @@ * */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + #include -#include -#include -#include -#ifdef _WIN32 -#define WIN32_LEAN_AND_MEAN -#include -#endif + #include -#include #ifdef OPENGL_IS_A_FRAMEWORK #include #include @@ -29,29 +39,12 @@ #include #endif -#include -#include - -#include -#include - -#include -#include -#include -#include - -// define the following in order to eliminate the deprecated headers warning -#define VTK_EXCLUDE_STRSTREAM_HEADERS -#include - -#include -#include -#include -#include - -#include -#include -#include +#include +#include +#ifdef _WIN32 +#define WIN32_LEAN_AND_MEAN +#include +#endif using namespace Eigen; using namespace pcl; diff --git a/simulation/tools/sim_test_simple.cpp b/simulation/tools/sim_test_simple.cpp index 47388cf5..6c7c4327 100644 --- a/simulation/tools/sim_test_simple.cpp +++ b/simulation/tools/sim_test_simple.cpp @@ -16,18 +16,28 @@ * */ +#include +#include +#include +#include +#include +#include +#include +#include // RangeImage +#include +#include +#include +#include +#include +#include // Pop-up viewer +#include +#include +#include + #include -#include -#include -#include -#include -#ifdef _WIN32 -#define WIN32_LEAN_AND_MEAN -#include -#endif + #include -#include #ifdef OPENGL_IS_A_FRAMEWORK #include #include @@ -41,34 +51,13 @@ #include #endif -#include -#include - -#include "pcl/common/common.h" -#include "pcl/common/transforms.h" - -#include -#include -#include -#include - -// define the following in order to eliminate the deprecated headers warning -#define VTK_EXCLUDE_STRSTREAM_HEADERS -#include - -#include "pcl/simulation/camera.h" -#include "pcl/simulation/model.h" -#include "pcl/simulation/range_likelihood.h" -#include "pcl/simulation/scene.h" - -#include -#include - -// RangeImage: -#include - -// Pop-up viewer -#include +#include +#include +#include +#ifdef _WIN32 +#define WIN32_LEAN_AND_MEAN +#include +#endif using namespace Eigen; using namespace pcl; diff --git a/simulation/tools/sim_viewer.cpp b/simulation/tools/sim_viewer.cpp index 02186372..a0e30f38 100644 --- a/simulation/tools/sim_viewer.cpp +++ b/simulation/tools/sim_viewer.cpp @@ -36,68 +36,52 @@ * $Id: pcd_viewer.cpp 5094 2012-03-15 01:03:51Z rusu $ * */ -#include -#include -#include -#include -#include -#ifdef _WIN32 -#define WIN32_LEAN_AND_MEAN -#include -#endif -#include - -#include -#ifdef OPENGL_IS_A_FRAMEWORK -#include -#else -#include -#endif - -#include -#include - -#include "pcl/common/common.h" -#include "pcl/common/transforms.h" - -#include -#include -#include -#include - -// define the following in order to eliminate the deprecated headers warning -#define VTK_EXCLUDE_STRSTREAM_HEADERS -#include - -#include "pcl/simulation/camera.h" -#include "pcl/simulation/model.h" -#include "pcl/simulation/range_likelihood.h" -#include "pcl/simulation/scene.h" - -#include -#include -#include - -// RangeImage: -#include - -// Pop-up viewer -#include - -#include #include +#include #include #include #include +#include #include +#include +#include // RangeImage +#include +#include +#include +#include +#include +#include // Pop-up viewer #include #include #include #include #include +#include +#include +#include + +#include +#include + #include +#include + +#ifdef OPENGL_IS_A_FRAMEWORK +#include +#else +#include +#endif + +#include +#include +#include +#ifdef _WIN32 +#define WIN32_LEAN_AND_MEAN +#include +#endif + using namespace Eigen; using namespace pcl; using namespace pcl::console; diff --git a/simulation/tools/simulation_io.cpp b/simulation/tools/simulation_io.cpp index 5bf95b3a..15c85740 100644 --- a/simulation/tools/simulation_io.cpp +++ b/simulation/tools/simulation_io.cpp @@ -1,4 +1,5 @@ #include "simulation_io.hpp" + #include pcl::simulation::SimExample::SimExample(int argc, char** argv, int height, int width) diff --git a/simulation/tools/simulation_io.hpp b/simulation/tools/simulation_io.hpp index 3ecc6bdd..c74e2211 100644 --- a/simulation/tools/simulation_io.hpp +++ b/simulation/tools/simulation_io.hpp @@ -1,10 +1,16 @@ #pragma once -#include +#include +#include +#include +#include +#include +#include +#include +#include #include -#include #ifdef OPENGL_IS_A_FRAMEWORK #include #include @@ -18,16 +24,6 @@ #include #endif -// define the following in order to eliminate the deprecated headers warning -#define VTK_EXCLUDE_STRSTREAM_HEADERS -#include -#include -#include - -#include -#include -#include - namespace pcl { namespace simulation { diff --git a/stereo/include/pcl/stereo/digital_elevation_map.h b/stereo/include/pcl/stereo/digital_elevation_map.h index bf3d3975..21f7824f 100644 --- a/stereo/include/pcl/stereo/digital_elevation_map.h +++ b/stereo/include/pcl/stereo/digital_elevation_map.h @@ -36,8 +36,8 @@ #pragma once -#include #include +#include namespace pcl { diff --git a/stereo/include/pcl/stereo/disparity_map_converter.h b/stereo/include/pcl/stereo/disparity_map_converter.h index a25e9eb3..0fb59953 100644 --- a/stereo/include/pcl/stereo/disparity_map_converter.h +++ b/stereo/include/pcl/stereo/disparity_map_converter.h @@ -36,12 +36,12 @@ #pragma once -#include -#include - #include #include +#include +#include + namespace pcl { /** \brief Compute point cloud from the disparity map. diff --git a/stereo/include/pcl/stereo/impl/disparity_map_converter.hpp b/stereo/include/pcl/stereo/impl/disparity_map_converter.hpp index b9e3af64..d0e72fd8 100644 --- a/stereo/include/pcl/stereo/impl/disparity_map_converter.hpp +++ b/stereo/include/pcl/stereo/impl/disparity_map_converter.hpp @@ -37,15 +37,14 @@ #ifndef PCL_DISPARITY_MAP_CONVERTER_IMPL_H_ #define PCL_DISPARITY_MAP_CONVERTER_IMPL_H_ +#include +#include #include +#include #include #include -#include -#include -#include - template pcl::DisparityMapConverter::DisparityMapConverter() : center_x_(0.0f) diff --git a/stereo/include/pcl/stereo/stereo_grabber.h b/stereo/include/pcl/stereo/stereo_grabber.h index 02263d98..dcad8bd6 100644 --- a/stereo/include/pcl/stereo/stereo_grabber.h +++ b/stereo/include/pcl/stereo/stereo_grabber.h @@ -38,10 +38,10 @@ #pragma once #include -#include #include -#include #include +#include +#include namespace pcl { @@ -72,21 +72,6 @@ public: float frames_per_second, bool repeat); - /** \brief Copy constructor. - * \param[in] src the Stereo Grabber base object to copy into this - */ - StereoGrabberBase(const StereoGrabberBase& src) : impl_() { *this = src; } - - /** \brief Copy operator. - * \param[in] src the Stereo Grabber base object to copy into this - */ - StereoGrabberBase& - operator=(const StereoGrabberBase& src) - { - impl_ = src.impl_; - return (*this); - } - /** \brief Virtual destructor. */ ~StereoGrabberBase() noexcept; diff --git a/stereo/include/pcl/stereo/stereo_matching.h b/stereo/include/pcl/stereo/stereo_matching.h index 20d85ae8..117d304f 100644 --- a/stereo/include/pcl/stereo/stereo_matching.h +++ b/stereo/include/pcl/stereo/stereo_matching.h @@ -37,11 +37,11 @@ #pragma once -#include - #include #include +#include + namespace pcl { template short int diff --git a/stereo/src/digital_elevation_map.cpp b/stereo/src/digital_elevation_map.cpp index 088ae62e..3efcb00b 100644 --- a/stereo/src/digital_elevation_map.cpp +++ b/stereo/src/digital_elevation_map.cpp @@ -34,10 +34,9 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include - #include #include +#include pcl::DigitalElevationMapBuilder::DigitalElevationMapBuilder() : resolution_column_(64), resolution_disparity_(32), min_points_in_cell_(1) diff --git a/stereo/src/stereo_grabber.cpp b/stereo/src/stereo_grabber.cpp index 1729a72e..45f5eed6 100644 --- a/stereo/src/stereo_grabber.cpp +++ b/stereo/src/stereo_grabber.cpp @@ -35,9 +35,9 @@ * */ +#include #include #include -#include /////////////////////////////////////////////////////////////////////////////////////////// //////////////////////// GrabberImplementation ////////////////////// diff --git a/surface/CMakeLists.txt b/surface/CMakeLists.txt index a70eedb6..e5e42311 100644 --- a/surface/CMakeLists.txt +++ b/surface/CMakeLists.txt @@ -82,6 +82,7 @@ set(POISSON_INCLUDES ) set(POISSON_SOURCES + src/3rdparty/poisson4/bspline_data.cpp src/3rdparty/poisson4/factor.cpp src/3rdparty/poisson4/geometry.cpp src/3rdparty/poisson4/marching_cubes_poisson.cpp diff --git a/surface/include/pcl/surface/3rdparty/opennurbs/opennurbs_array_defs.h b/surface/include/pcl/surface/3rdparty/opennurbs/opennurbs_array_defs.h index cfc372c2..441ad59d 100644 --- a/surface/include/pcl/surface/3rdparty/opennurbs/opennurbs_array_defs.h +++ b/surface/include/pcl/surface/3rdparty/opennurbs/opennurbs_array_defs.h @@ -466,15 +466,15 @@ void ON_SimpleArray::Append( int count, const T* p ) template void ON_SimpleArray::Insert( int i, const T& x ) { - if( i >= 0 && i <= m_count ) + if( i >= 0 && i <= m_count ) { - if ( m_count == m_capacity ) + if ( m_count == m_capacity ) { int newcapacity = NewCapacity(); Reserve( newcapacity ); } m_count++; - Move( i+1, i, m_count-1-i ); + Move( i+1, i, static_cast(m_count)-1-i ); m_a[i] = x; } } @@ -1372,7 +1372,7 @@ void ON_ClassArray::Insert( int i, const T& x ) DestroyElement( m_a[m_count] ); m_count++; if ( i < m_count-1 ) { - Move( i+1, i, m_count-1-i ); + Move( i+1, i, static_cast(m_count)-1-i ); // This call to memset is ok even when T has a vtable // because in-place construction is used later. memset( (void*)(&m_a[i]), 0, sizeof(T) ); diff --git a/surface/include/pcl/surface/3rdparty/poisson4/bspline_data.h b/surface/include/pcl/surface/3rdparty/poisson4/bspline_data.h index 80927a0e..9f2b87b8 100644 --- a/surface/include/pcl/surface/3rdparty/poisson4/bspline_data.h +++ b/surface/include/pcl/surface/3rdparty/poisson4/bspline_data.h @@ -32,6 +32,9 @@ DAMAGE. #include "ppolynomial.h" +#include +#include + namespace pcl { namespace poisson diff --git a/surface/include/pcl/surface/3rdparty/poisson4/bspline_data.hpp b/surface/include/pcl/surface/3rdparty/poisson4/bspline_data.hpp index 85c0d60e..49861aa8 100644 --- a/surface/include/pcl/surface/3rdparty/poisson4/bspline_data.hpp +++ b/surface/include/pcl/surface/3rdparty/poisson4/bspline_data.hpp @@ -27,6 +27,7 @@ DAMAGE. */ #include "poisson_exceptions.h" +#include "binary_node.h" namespace pcl { @@ -95,15 +96,15 @@ namespace pcl { if( functionCount ) { - if( vvDotTable ) delete[] vvDotTable; - if( dvDotTable ) delete[] dvDotTable; - if( ddDotTable ) delete[] ddDotTable; + delete[] vvDotTable; + delete[] dvDotTable; + delete[] ddDotTable; - if( valueTables ) delete[] valueTables; - if( dValueTables ) delete[] dValueTables; + delete[] valueTables; + delete[] dValueTables; - if( baseFunctions ) delete[] baseFunctions; - if( baseBSplines ) delete[] baseBSplines; + delete[] baseFunctions; + delete[] baseBSplines; } vvDotTable = dvDotTable = ddDotTable = NULL; valueTables = dValueTables=NULL; @@ -292,9 +293,11 @@ namespace pcl template void BSplineData::clearDotTables( int flags ) { - if( (flags & VV_DOT_FLAG) && vvDotTable ) delete[] vvDotTable , vvDotTable = NULL; - if( (flags & DV_DOT_FLAG) && dvDotTable ) delete[] dvDotTable , dvDotTable = NULL; - if( (flags & DD_DOT_FLAG) && ddDotTable ) delete[] ddDotTable , ddDotTable = NULL; + if (flags & VV_DOT_FLAG) { + delete[] vvDotTable ; vvDotTable = NULL; + delete[] dvDotTable ; dvDotTable = NULL; + delete[] ddDotTable ; ddDotTable = NULL; + } } template< int Degree , class Real > void BSplineData< Degree , Real >::setSampleSpan( int idx , int& start , int& end , double smooth ) const @@ -367,8 +370,8 @@ namespace pcl template void BSplineData::clearValueTables(void){ - if( valueTables){delete[] valueTables;} - if(dValueTables){delete[] dValueTables;} + delete[] valueTables; + delete[] dValueTables; valueTables=dValueTables=NULL; } @@ -447,60 +450,10 @@ namespace pcl POISSON_THROW_EXCEPTION (pcl::poisson::PoissonBadArgumentException, "B-spline up-sampling not supported for degree " << Degree); } template<> - void BSplineElements< 1 >::upSample( BSplineElements< 1 >& high ) const - { - high.resize( size()*2 ); - high.assign( high.size() , BSplineElementCoefficients<1>() ); - for( int i=0 ; i::upSample( BSplineElements< 1 >& high ) const; - high[2*i+0][0] += 1 * (*this)[i][1]; - high[2*i+0][1] += 2 * (*this)[i][1]; - high[2*i+1][0] += 0 * (*this)[i][1]; - high[2*i+1][1] += 1 * (*this)[i][1]; - } - high.denominator = denominator * 2; - } template<> - void BSplineElements< 2 >::upSample( BSplineElements< 2 >& high ) const - { - // /----\ - // / \ - // / \ = 1 /--\ +3 /--\ +3 /--\ +1 /--\ - // / \ / \ / \ / \ / \ - // |----------| |----------| |----------| |----------| |----------| - - high.resize( size()*2 ); - high.assign( high.size() , BSplineElementCoefficients<2>() ); - for( int i=0 ; i::upSample( BSplineElements< 2 >& high ) const; template< int Degree > void BSplineElements< Degree >::differentiate( BSplineElements< Degree-1 >& d ) const diff --git a/surface/include/pcl/surface/3rdparty/poisson4/function_data.hpp b/surface/include/pcl/surface/3rdparty/poisson4/function_data.hpp index 8fa4cdc3..9378b246 100644 --- a/surface/include/pcl/surface/3rdparty/poisson4/function_data.hpp +++ b/surface/include/pcl/surface/3rdparty/poisson4/function_data.hpp @@ -48,11 +48,11 @@ namespace pcl { if(res) { - if( dotTable) delete[] dotTable; - if( dDotTable) delete[] dDotTable; - if(d2DotTable) delete[] d2DotTable; - if( valueTables) delete[] valueTables; - if(dValueTables) delete[] dValueTables; + delete[] dotTable; + delete[] dDotTable; + delete[] d2DotTable; + delete[] valueTables; + delete[] dValueTables; } dotTable=dDotTable=d2DotTable=NULL; valueTables=dValueTables=NULL; @@ -224,18 +224,12 @@ namespace pcl template void FunctionData::clearDotTables( const int& flags ) { - if((flags & DOT_FLAG) && dotTable) + if (flags & DOT_FLAG) { delete[] dotTable; dotTable=NULL; - } - if((flags & D_DOT_FLAG) && dDotTable) - { delete[] dDotTable; dDotTable=NULL; - } - if((flags & D2_DOT_FLAG) && d2DotTable) - { delete[] d2DotTable; d2DotTable=NULL; } @@ -292,8 +286,8 @@ namespace pcl template void FunctionData::clearValueTables(void){ - if( valueTables){delete[] valueTables;} - if(dValueTables){delete[] dValueTables;} + delete[] valueTables; + delete[] dValueTables; valueTables=dValueTables=NULL; } diff --git a/surface/include/pcl/surface/3rdparty/poisson4/geometry.hpp b/surface/include/pcl/surface/3rdparty/poisson4/geometry.hpp index 9dfa2607..bc3b0a80 100644 --- a/surface/include/pcl/surface/3rdparty/poisson4/geometry.hpp +++ b/surface/include/pcl/surface/3rdparty/poisson4/geometry.hpp @@ -8,14 +8,14 @@ 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. +in the documentation and/or other materials provided with the distribution. Neither the name of the Johns Hopkins University nor the names of its contributors may be used to endorse or promote products derived from this software without specific -prior written permission. +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 +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 @@ -32,8 +32,6 @@ namespace pcl { namespace poisson { - - template Real Random(void){return Real(rand())/RAND_MAX;} @@ -78,6 +76,7 @@ namespace pcl p.coords[1]=-p1.coords[0]*p2.coords[2]+p1.coords[2]*p2.coords[0]; p.coords[2]= p1.coords[0]*p2.coords[1]-p1.coords[1]*p2.coords[0]; } + template void EdgeCollapse(const Real& edgeRatio,std::vector& triangles,std::vector< Point3D >& positions,std::vector< Point3D >* normals){ int i,j,*remapTable,*pointCount,idx[3]; @@ -173,6 +172,7 @@ namespace pcl delete[] pointCount; delete[] remapTable; } + template void TriangleCollapse(const Real& edgeRatio,std::vector& triangles,std::vector< Point3D >& positions,std::vector< Point3D >* normals){ int i,j,*remapTable,*pointCount,idx[3]; @@ -306,6 +306,7 @@ namespace pcl else {p3=edges[triangles[tIndex].eIndex[2]].pIndex[1];} return 1; } + template double Triangulation::area(int p1,int p2,int p3){ Point3D q1,q2,q; @@ -316,18 +317,21 @@ namespace pcl CrossProduct(q1,q2,q); return Length(q); } + template double Triangulation::area(int tIndex){ int p1,p2,p3; factor(tIndex,p1,p2,p3); return area(p1,p2,p3); } + template double Triangulation::area(void){ double a=0; for(int i=0;i int Triangulation::addTriangle(int p1,int p2,int p3){ int tIdx,eIdx,p[3]; @@ -366,6 +370,7 @@ namespace pcl } return tIdx; } + template int Triangulation::flipMinimize(int eIndex){ double oldArea,newArea; @@ -426,6 +431,5 @@ namespace pcl } return 1; } - } } diff --git a/surface/include/pcl/surface/3rdparty/poisson4/mat.hpp b/surface/include/pcl/surface/3rdparty/poisson4/mat.hpp index 1a936935..18a6677a 100644 --- a/surface/include/pcl/surface/3rdparty/poisson4/mat.hpp +++ b/surface/include/pcl/surface/3rdparty/poisson4/mat.hpp @@ -43,10 +43,8 @@ namespace pcl template MinimalAreaTriangulation::~MinimalAreaTriangulation(void) { - if(bestTriangulation) delete[] bestTriangulation; bestTriangulation=NULL; - if(midPoint) delete[] midPoint; midPoint=NULL; } @@ -104,9 +102,7 @@ namespace pcl } return; } - if(bestTriangulation) delete[] bestTriangulation; - if(midPoint) delete[] midPoint; bestTriangulation=NULL; midPoint=NULL; @@ -123,9 +119,7 @@ namespace pcl template Real MinimalAreaTriangulation::GetArea(const std::vector >& vertices) { - if(bestTriangulation) delete[] bestTriangulation; - if(midPoint) delete[] midPoint; bestTriangulation=NULL; midPoint=NULL; diff --git a/surface/include/pcl/surface/3rdparty/poisson4/multi_grid_octree_data.hpp b/surface/include/pcl/surface/3rdparty/poisson4/multi_grid_octree_data.hpp index ec13a00b..ebf0ae33 100644 --- a/surface/include/pcl/surface/3rdparty/poisson4/multi_grid_octree_data.hpp +++ b/surface/include/pcl/surface/3rdparty/poisson4/multi_grid_octree_data.hpp @@ -8,14 +8,14 @@ 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. +in the documentation and/or other materials provided with the distribution. Neither the name of the Johns Hopkins University nor the names of its contributors may be used to endorse or promote products derived from this software without specific -prior written permission. +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 +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 @@ -77,16 +77,16 @@ namespace pcl maxDepth=0; } SortedTreeNodes::~SortedTreeNodes(void){ - if( nodeCount ) delete[] nodeCount; - if( treeNodes ) delete[] treeNodes; + delete[] nodeCount; + delete[] treeNodes; nodeCount = NULL; treeNodes = NULL; } void SortedTreeNodes::set( TreeOctNode& root ) { - if( nodeCount ) delete[] nodeCount; - if( treeNodes ) delete[] treeNodes; + delete[] nodeCount; + delete[] treeNodes; maxDepth = root.maxDepth()+1; nodeCount = new int[ maxDepth+1 ]; treeNodes = new TreeOctNode*[ root.nodes() ]; @@ -1128,6 +1128,7 @@ namespace pcl } template< int Degree > int Octree< Degree >::GetMatrixRowSize( const OctNode< TreeNodeData , Real >::Neighbors5& neighbors5 ) const { return GetMatrixRowSize( neighbors5 , 0 , 5 , 0 , 5 , 0 , 5 ); } + template< int Degree > int Octree< Degree >::GetMatrixRowSize( const OctNode< TreeNodeData , Real >::Neighbors5& neighbors5 , int xStart , int xEnd , int yStart , int yEnd , int zStart , int zEnd ) const { @@ -1141,11 +1142,13 @@ namespace pcl count++; return count; } + template< int Degree > int Octree< Degree >::SetMatrixRow( const OctNode< TreeNodeData , Real >::Neighbors5& neighbors5 , MatrixEntry< float >* row , int offset , const double stencil[5][5][5] ) const { return SetMatrixRow( neighbors5 , row , offset , stencil , 0 , 5 , 0 , 5 , 0 , 5 ); } + template< int Degree > int Octree< Degree >::SetMatrixRow( const OctNode< TreeNodeData , Real >::Neighbors5& neighbors5 , MatrixEntry< float >* row , int offset , const double stencil[5][5][5] , int xStart , int xEnd , int yStart , int yEnd , int zStart , int zEnd ) const { @@ -1226,6 +1229,7 @@ namespace pcl } return count; } + template< int Degree > void Octree< Degree >::SetDivergenceStencil( int depth , Point3D< double > *stencil , bool scatter ) const { @@ -1284,6 +1288,7 @@ namespace pcl #endif // GRADIENT_DOMAIN_SOLUTION } } + template< int Degree > void Octree< Degree >::SetLaplacianStencil( int depth , double stencil[5][5][5] ) const { @@ -1304,6 +1309,7 @@ namespace pcl stencil[x][y][z] = GetLaplacian( symIndex ); } } + template< int Degree > void Octree< Degree >::SetLaplacianStencils( int depth , Stencil< double , 5 > stencils[2][2][2] ) const { @@ -1328,6 +1334,7 @@ namespace pcl } } } + template< int Degree > void Octree< Degree >::SetDivergenceStencils( int depth , Stencil< Point3D< double > , 5 > stencils[2][2][2] , bool scatter ) const { @@ -1380,6 +1387,7 @@ namespace pcl } } } + template< int Degree > void Octree< Degree >::SetEvaluationStencils( int depth , Stencil< double , 3 > stencil1[8] , Stencil< double , 3 > stencil2[8][8] ) const { @@ -1421,6 +1429,7 @@ namespace pcl } } } + template< int Degree > void Octree< Degree >::UpdateCoarserSupportBounds( const TreeOctNode* node , int& startX , int& endX , int& startY , int& endY , int& startZ , int& endZ ) { @@ -1497,6 +1506,7 @@ namespace pcl UpSampleData( void ) { start = 0 , v[0] = v[1] = 0.; } UpSampleData( int s , double v1 , double v2 ) { start = s , v[0] = v1 , v[1] = v2; } }; + template< int Degree > void Octree< Degree >::UpSampleCoarserSolution( int depth , const SortedTreeNodes& sNodes , Vector< Real >& Solution ) const { @@ -1672,6 +1682,7 @@ namespace pcl constraints[i] += cSum; } } + template< int Degree > template< class C > void Octree< Degree >::UpSample( int depth , const SortedTreeNodes& sNodes , C* coefficients ) const @@ -1727,6 +1738,7 @@ namespace pcl } } } + template< int Degree > void Octree< Degree >::SetCoarserPointValues( int depth , const SortedTreeNodes& sNodes , Real* metSolution ) { @@ -1748,6 +1760,7 @@ namespace pcl } } } + template< int Degree > Real Octree< Degree >::WeightedCoarserFunctionValue( const OctNode< TreeNodeData , Real >::NeighborKey3& neighborKey , const TreeOctNode* pointNode , Real* metSolution ) const { @@ -1781,6 +1794,7 @@ namespace pcl } return Real( pointValue * weight ); } + template int Octree::GetFixedDepthLaplacian( SparseSymmetricMatrix< Real >& matrix , int depth , const SortedTreeNodes& sNodes , Real* metSolution ) { @@ -1825,6 +1839,7 @@ namespace pcl } return 1; } + template int Octree::GetRestrictedFixedDepthLaplacian( SparseSymmetricMatrix< Real >& matrix,int depth,const int* entries,int entryCount, const TreeOctNode* rNode , Real radius , @@ -1962,6 +1977,7 @@ namespace pcl return iter; } + template int Octree::SolveFixedDepthMatrix( int depth , const SortedTreeNodes& sNodes , Real* metSolution , int startingDepth , bool showResidual , int minIters , double accuracy ) { @@ -2092,6 +2108,7 @@ namespace pcl delete[] asf.adjacencies; return tIter; } + template int Octree::HasNormals(TreeOctNode* node,Real epsilon) { @@ -2101,6 +2118,7 @@ namespace pcl return hasNormals; } + template void Octree::ClipTree( void ) { @@ -2114,6 +2132,7 @@ namespace pcl } MemoryUsage(); } + template void Octree::SetLaplacianConstraints( void ) { @@ -2312,15 +2331,19 @@ namespace pcl delete normals; normals = NULL; } + template void Octree::AdjacencyCountFunction::Function(const TreeOctNode* node1,const TreeOctNode* node2){adjacencyCount++;} + template void Octree::AdjacencySetFunction::Function(const TreeOctNode* node1,const TreeOctNode* node2){adjacencies[adjacencyCount++]=node1->nodeData.nodeIndex;} + template void Octree::RefineFunction::Function( TreeOctNode* node1 , const TreeOctNode* node2 ) { if( !node1->children && node1->depth()initChildren(); } + template< int Degree > void Octree< Degree >::FaceEdgesFunction::Function( const TreeOctNode* node1 , const TreeOctNode* node2 ) { @@ -2421,6 +2444,7 @@ namespace pcl _sNodes.set( tree ); MemoryUsage(); } + template void Octree::GetMCIsoTriangles( Real isoValue , int subdivideDepth , CoredMeshData* mesh , int fullDepthIso , int nonLinearFit , bool addBarycenter , bool polygonMesh ) { @@ -2592,6 +2616,7 @@ namespace pcl delete[] coarseRootData.cornerValuesSet , delete[] coarseRootData.cornerNormalsSet; delete rootData.boundaryValues; } + template Real Octree::getCenterValue( const OctNode< TreeNodeData , Real >::ConstNeighborKey3& neighborKey , const TreeOctNode* node){ int idx[3]; @@ -2634,6 +2659,7 @@ namespace pcl } return value; } + template< int Degree > Real Octree< Degree >::getCornerValue( const OctNode< TreeNodeData , Real >::ConstNeighborKey3& neighborKey3 , const TreeOctNode* node , int corner , const Real* metSolution ) { @@ -2694,6 +2720,7 @@ namespace pcl } return Real( value ); } + template< int Degree > Real Octree< Degree >::getCornerValue( const OctNode< TreeNodeData , Real >::ConstNeighborKey3& neighborKey3 , const TreeOctNode* node , int corner , const Real* metSolution , const double stencil1[3][3][3] , const double stencil2[3][3][3] ) { @@ -2733,6 +2760,7 @@ namespace pcl } return Real( value ); } + template< int Degree > Point3D< Real > Octree< Degree >::getCornerNormal( const OctNode< TreeNodeData , Real >::ConstNeighborKey5& neighborKey5 , const TreeOctNode* node , int corner , const Real* metSolution ) { @@ -2784,6 +2812,7 @@ namespace pcl } return normal; } + template< int Degree > Real Octree::GetIsoValue( void ) { @@ -2865,7 +2894,6 @@ namespace pcl } } - template int Octree::InteriorFaceRootCount(const TreeOctNode* node,const int &faceIndex,int maxDepth){ int c1,c2,e1,e2,dir,off,cnt=0; @@ -2945,6 +2973,7 @@ namespace pcl if(finest->children) return EdgeRootCount(&finest->children[c1],eIndex,maxDepth)+EdgeRootCount(&finest->children[c2],eIndex,maxDepth); else return MarchingCubes::HasEdgeRoots(finest->nodeData.mcIndex,eIndex); } + template int Octree::IsBoundaryFace(const TreeOctNode* node,int faceIndex,int subdivideDepth){ int dir,offset,d,o[3],idx; @@ -2957,12 +2986,14 @@ namespace pcl idx=(int(o[dir])<<1) + (offset<<1); return !(idx%(2<<(int(node->d)-subdivideDepth))); } + template int Octree::IsBoundaryEdge(const TreeOctNode* node,int edgeIndex,int subdivideDepth){ int dir,x,y; Cube::FactorEdgeIndex(edgeIndex,dir,x,y); return IsBoundaryEdge(node,dir,x,y,subdivideDepth); } + template int Octree::IsBoundaryEdge( const TreeOctNode* node , int dir , int x , int y , int subdivideDepth ) { @@ -2990,6 +3021,7 @@ namespace pcl mask = 1<<( int(node->d) - subdivideDepth ); return !(idx1%(mask)) || !(idx2%(mask)); } + template< int Degree > void Octree< Degree >::GetRootSpan( const RootInfo& ri , Point3D< float >& start , Point3D< float >& end ) { @@ -3021,6 +3053,7 @@ namespace pcl break; } } + ////////////////////////////////////////////////////////////////////////////////////// // The assumption made when calling this code is that the edge has at most one root // ////////////////////////////////////////////////////////////////////////////////////// @@ -3154,6 +3187,7 @@ namespace pcl position[o] = Real(center-width/2+width*averageRoot); return 1; } + template< int Degree > int Octree< Degree >::GetRootIndex( const TreeOctNode* node , int edgeIndex , int maxDepth , int sDepth,RootInfo& ri ) { @@ -3235,6 +3269,7 @@ namespace pcl return 1; } } + template int Octree::GetRootIndex( const TreeOctNode* node , int edgeIndex , int maxDepth , RootInfo& ri ) { @@ -3313,6 +3348,7 @@ namespace pcl return 1; } } + template int Octree::GetRootPair(const RootInfo& ri,int maxDepth,RootInfo& pair){ const TreeOctNode* node=ri.node; @@ -3328,8 +3364,8 @@ namespace pcl node=node->parent; } return 0; - } + template int Octree< Degree >::GetRootIndex( const RootInfo& ri , RootData& rootData , CoredPointIndex& index ) { @@ -3353,6 +3389,7 @@ namespace pcl } return 0; } + template< int Degree > int Octree< Degree >::SetMCRootPositions( TreeOctNode* node , int sDepth , Real isoValue , TreeOctNode::ConstNeighborKey5& neighborKey5 , RootData& rootData , std::vector< Point3D< float > >* interiorPositions , CoredMeshData* mesh , const Real* metSolution , int nonLinearFit ) @@ -3420,6 +3457,7 @@ namespace pcl } return count; } + template int Octree< Degree >::SetBoundaryMCRootPositions( int sDepth , Real isoValue , RootData& rootData , CoredMeshData* mesh , int nonLinearFit ) { @@ -3461,6 +3499,7 @@ namespace pcl } return count; } + template void Octree< Degree >::GetMCIsoEdges( TreeOctNode* node , int sDepth , std::vector< std::pair< RootInfo , RootInfo > >& edges ) { @@ -3558,6 +3597,7 @@ namespace pcl } } } + template #if MISHA_DEBUG int Octree< Degree >::GetMCIsoTriangles( TreeOctNode* node , CoredMeshData* mesh , RootData& rootData , std::vector< Point3D< float > >* interiorPositions , int offSet , int sDepth , bool polygonMesh , std::vector< Point3D< float > >* barycenters ) @@ -3636,6 +3676,7 @@ namespace pcl } return int(loops.size()); } + template #if MISHA_DEBUG int Octree::AddTriangles( CoredMeshData* mesh , std::vector& edges , std::vector >* interiorPositions , int offSet , bool polygonMesh , std::vector< Point3D< float > >* barycenters ) @@ -3758,6 +3799,7 @@ namespace pcl } return int(edges.size())-2; } + template< int Degree > Real* Octree< Degree >::GetSolutionGrid( int& res , float isoValue , int depth ) { @@ -3802,6 +3844,7 @@ namespace pcl return values; } + template< int Degree > Real* Octree< Degree >::GetWeightGrid( int& res , int depth ) { @@ -3847,20 +3890,24 @@ namespace pcl int idx[DIMENSION]; return CenterIndex(node,maxDepth,idx); } + long long VertexData::CenterIndex(const TreeOctNode* node,int maxDepth,int idx[DIMENSION]){ int d,o[3]; node->depthAndOffset(d,o); for(int i=0;i::CornerIndex(maxDepth+1,d+1,o[i]<<1,1);} return (long long)(idx[0]) | (long long)(idx[1])<<15 | (long long)(idx[2])<<30; } + long long VertexData::CenterIndex(int depth,const int offSet[DIMENSION],int maxDepth,int idx[DIMENSION]){ for(int i=0;i::CornerIndex(maxDepth+1,depth+1,offSet[i]<<1,1);} return (long long)(idx[0]) | (long long)(idx[1])<<15 | (long long)(idx[2])<<30; } + long long VertexData::CornerIndex(const TreeOctNode* node,int cIndex,int maxDepth){ int idx[DIMENSION]; return CornerIndex(node,cIndex,maxDepth,idx); } + long long VertexData::CornerIndex( const TreeOctNode* node , int cIndex , int maxDepth , int idx[DIMENSION] ) { int x[DIMENSION]; @@ -3870,6 +3917,7 @@ namespace pcl for( int i=0 ; i::CornerIndex( maxDepth+1 , d , o[i] , x[i] ); return CornerIndexKey( idx ); } + long long VertexData::CornerIndex( int depth , const int offSet[DIMENSION] , int cIndex , int maxDepth , int idx[DIMENSION] ) { int x[DIMENSION]; @@ -3877,14 +3925,17 @@ namespace pcl for( int i=0 ; i::CornerIndex( maxDepth+1 , depth , offSet[i] , x[i] ); return CornerIndexKey( idx ); } + long long VertexData::CornerIndexKey( const int idx[DIMENSION] ) { return (long long)(idx[0]) | (long long)(idx[1])<<15 | (long long)(idx[2])<<30; } + long long VertexData::FaceIndex(const TreeOctNode* node,int fIndex,int maxDepth){ int idx[DIMENSION]; return FaceIndex(node,fIndex,maxDepth,idx); } + long long VertexData::FaceIndex(const TreeOctNode* node,int fIndex,int maxDepth,int idx[DIMENSION]){ int dir,offset; Cube::FactorFaceIndex(fIndex,dir,offset); @@ -3894,10 +3945,12 @@ namespace pcl idx[dir]=BinaryNode::CornerIndex(maxDepth+1,d,o[dir],offset); return (long long)(idx[0]) | (long long)(idx[1])<<15 | (long long)(idx[2])<<30; } + long long VertexData::EdgeIndex(const TreeOctNode* node,int eIndex,int maxDepth){ int idx[DIMENSION]; return EdgeIndex(node,eIndex,maxDepth,idx); } + long long VertexData::EdgeIndex(const TreeOctNode* node,int eIndex,int maxDepth,int idx[DIMENSION]){ int o,i1,i2; int d,off[3]; @@ -3920,9 +3973,5 @@ namespace pcl }; return (long long)(idx[0]) | (long long)(idx[1])<<15 | (long long)(idx[2])<<30; } - - } } - - diff --git a/surface/include/pcl/surface/3rdparty/poisson4/octree_poisson.hpp b/surface/include/pcl/surface/3rdparty/poisson4/octree_poisson.hpp index 1ea060c0..9cae3118 100644 --- a/surface/include/pcl/surface/3rdparty/poisson4/octree_poisson.hpp +++ b/surface/include/pcl/surface/3rdparty/poisson4/octree_poisson.hpp @@ -8,14 +8,14 @@ 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. +in the documentation and/or other materials provided with the distribution. Neither the name of the Johns Hopkins University nor the names of its contributors may be used to endorse or promote products derived from this software without specific -prior written permission. +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 +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 @@ -40,7 +40,6 @@ namespace pcl { namespace poisson { - template const int OctNode::DepthShift=5; template const int OctNode::OffsetShift=19; template const int OctNode::DepthMask=(1< int OctNode::UseAllocator(void){return UseAlloc;} @@ -73,9 +73,10 @@ namespace pcl template OctNode::~OctNode(void){ - if(!UseAlloc){if(children){delete[] children;}} + if(!UseAlloc){delete[] children;} parent=children=NULL; } + template void OctNode::setFullDepth(int maxDepth){ if( maxDepth ) @@ -91,7 +92,7 @@ namespace pcl if(UseAlloc){children=internalAllocator.newElements(8);} else{ - if(children){delete[] children;} + delete[] children; children=NULL; children=new OctNode[Cube::CORNERS]; } @@ -116,6 +117,7 @@ namespace pcl } return 1; } + template inline void OctNode::Index(int depth,const int offset[3],short& d,short off[3]){ d=short(depth); @@ -131,6 +133,7 @@ namespace pcl offset[1]=(int(off[1])+1)&(~(1< inline int OctNode::depth(void) const {return int(d);} template @@ -140,6 +143,7 @@ namespace pcl offset[1]=(int((index>>OffsetShift2)&OffsetMask)+1)&(~(1<>OffsetShift3)&OffsetMask)+1)&(~(1< inline int OctNode::Depth(const long long& index){return int(index&DepthMask);} template @@ -152,6 +156,7 @@ namespace pcl width=Real(1.0/(1< bool OctNode< NodeData , Real >::isInside( Point3D< Real > p ) const { @@ -161,6 +166,7 @@ namespace pcl w /= 2; return (c[0]-w) inline void OctNode::CenterAndWidth(const long long& index,Point3D& center,Real& width){ int depth,offset[3]; @@ -184,6 +190,7 @@ namespace pcl return c+1; } } + template int OctNode::nodes(void) const{ if(!children){return 1;} @@ -193,6 +200,7 @@ namespace pcl return c+1; } } + template int OctNode::leaves(void) const{ if(!children){return 1;} @@ -202,6 +210,7 @@ namespace pcl return c; } } + template int OctNode::maxDepthLeaves(int maxDepth) const{ if(depth()>maxDepth){return 0;} @@ -212,6 +221,7 @@ namespace pcl return c; } } + template const OctNode* OctNode::root(void) const{ const OctNode* temp=this; @@ -219,7 +229,6 @@ namespace pcl return temp; } - template const OctNode* OctNode::nextBranch( const OctNode* current ) const { @@ -227,12 +236,14 @@ namespace pcl if(current-current->parent->children==Cube::CORNERS-1) return nextBranch( current->parent ); else return current+1; } + template OctNode* OctNode::nextBranch(OctNode* current){ if(!current->parent || current==this){return NULL;} if(current-current->parent->children==Cube::CORNERS-1){return nextBranch(current->parent);} else{return current+1;} } + template< class NodeData , class Real > const OctNode< NodeData , Real >* OctNode< NodeData , Real >::prevBranch( const OctNode* current ) const { @@ -240,6 +251,7 @@ namespace pcl if( current-current->parent->children==0 ) return prevBranch( current->parent ); else return current-1; } + template< class NodeData , class Real > OctNode< NodeData , Real >* OctNode< NodeData , Real >::prevBranch( OctNode* current ) { @@ -247,6 +259,7 @@ namespace pcl if( current-current->parent->children==0 ) return prevBranch( current->parent ); else return current-1; } + template const OctNode* OctNode::nextLeaf(const OctNode* current) const{ if(!current){ @@ -259,6 +272,7 @@ namespace pcl if(!temp){return NULL;} else{return temp->nextLeaf();} } + template OctNode* OctNode::nextLeaf(OctNode* current){ if(!current){ @@ -279,6 +293,7 @@ namespace pcl else if( current->children ) return ¤t->children[0]; else return nextBranch(current); } + template OctNode* OctNode::nextNode( OctNode* current ) { @@ -308,6 +323,7 @@ namespace pcl if(processCurrent){F->Function(this,node);} if(children){__processNodeNodes(node,F);} } + template template void OctNode::processNodeFaces(OctNode* node,NodeAdjacencyFunction* F,int fIndex,int processCurrent){ @@ -318,6 +334,7 @@ namespace pcl __processNodeFaces(node,F,c1,c2,c3,c4); } } + template template void OctNode::processNodeEdges(OctNode* node,NodeAdjacencyFunction* F,int eIndex,int processCurrent){ @@ -328,6 +345,7 @@ namespace pcl __processNodeEdges(node,F,c1,c2); } } + template template void OctNode::processNodeCorners(OctNode* node,NodeAdjacencyFunction* F,int cIndex,int processCurrent){ @@ -338,6 +356,7 @@ namespace pcl F->Function(temp,node); } } + template template void OctNode::__processNodeNodes(OctNode* node,NodeAdjacencyFunction* F){ @@ -358,6 +377,7 @@ namespace pcl if(children[6].children){children[6].__processNodeNodes(node,F);} if(children[7].children){children[7].__processNodeNodes(node,F);} } + template template void OctNode::__processNodeEdges(OctNode* node,NodeAdjacencyFunction* F,int cIndex1,int cIndex2){ @@ -366,6 +386,7 @@ namespace pcl if(children[cIndex1].children){children[cIndex1].__processNodeEdges(node,F,cIndex1,cIndex2);} if(children[cIndex2].children){children[cIndex2].__processNodeEdges(node,F,cIndex1,cIndex2);} } + template template void OctNode::__processNodeFaces(OctNode* node,NodeAdjacencyFunction* F,int cIndex1,int cIndex2,int cIndex3,int cIndex4){ @@ -378,6 +399,7 @@ namespace pcl if(children[cIndex3].children){children[cIndex3].__processNodeFaces(node,F,cIndex1,cIndex2,cIndex3,cIndex4);} if(children[cIndex4].children){children[cIndex4].__processNodeFaces(node,F,cIndex1,cIndex2,cIndex3,cIndex4);} } + template template void OctNode::ProcessNodeAdjacentNodes(int maxDepth,OctNode* node1,int width1,OctNode* node2,int width2,NodeAdjacencyFunction* F,int processCurrent){ @@ -389,6 +411,7 @@ namespace pcl ProcessNodeAdjacentNodes(c1[0]-c2[0],c1[1]-c2[1],c1[2]-c2[2],node1,(width1*w1)>>1,node2,(width2*w2)>>1,w2,F,processCurrent); } + template template void OctNode::ProcessNodeAdjacentNodes(int dx,int dy,int dz, @@ -400,6 +423,7 @@ namespace pcl if(!node2->children){return;} __ProcessNodeAdjacentNodes(-dx,-dy,-dz,node1,radius1,node2,radius2,width2/2,F); } + template template void OctNode::ProcessTerminatingNodeAdjacentNodes(int maxDepth,OctNode* node1,int width1,OctNode* node2,int width2,TerminatingNodeAdjacencyFunction* F,int processCurrent){ @@ -411,6 +435,7 @@ namespace pcl ProcessTerminatingNodeAdjacentNodes(c1[0]-c2[0],c1[1]-c2[1],c1[2]-c2[2],node1,(width1*w1)>>1,node2,(width2*w2)>>1,w2,F,processCurrent); } + template template void OctNode::ProcessTerminatingNodeAdjacentNodes(int dx,int dy,int dz, @@ -423,6 +448,7 @@ namespace pcl if(!node2->children){return;} __ProcessTerminatingNodeAdjacentNodes(-dx,-dy,-dz,node1,radius1,node2,radius2,width2/2,F); } + template template void OctNode::ProcessPointAdjacentNodes( int maxDepth , const int c1[3] , OctNode* node2 , int width2 , PointAdjacencyFunction* F , int processCurrent ) @@ -432,6 +458,7 @@ namespace pcl w2 = node2->width( maxDepth+1 ); ProcessPointAdjacentNodes( c1[0]-c2[0] , c1[1]-c2[1] , c1[2]-c2[2] , node2 , (width2*w2)>>1 , w2 , F , processCurrent ); } + template template void OctNode::ProcessPointAdjacentNodes(int dx,int dy,int dz, @@ -443,6 +470,7 @@ namespace pcl if( !node2->children ) return; __ProcessPointAdjacentNodes( -dx , -dy , -dz , node2 , radius2 , width2>>1 , F ); } + template template void OctNode::ProcessFixedDepthNodeAdjacentNodes(int maxDepth, @@ -458,6 +486,7 @@ namespace pcl ProcessFixedDepthNodeAdjacentNodes(c1[0]-c2[0],c1[1]-c2[1],c1[2]-c2[2],node1,(width1*w1)>>1,node2,(width2*w2)>>1,w2,depth,F,processCurrent); } + template template void OctNode::ProcessFixedDepthNodeAdjacentNodes(int dx,int dy,int dz, @@ -474,6 +503,7 @@ namespace pcl __ProcessFixedDepthNodeAdjacentNodes(-dx,-dy,-dz,node1,radius1,node2,radius2,width2/2,depth-1,F); } } + template template void OctNode::ProcessMaxDepthNodeAdjacentNodes(int maxDepth, @@ -488,6 +518,7 @@ namespace pcl w2=node2->width(maxDepth+1); ProcessMaxDepthNodeAdjacentNodes(c1[0]-c2[0],c1[1]-c2[1],c1[2]-c2[2],node1,(width1*w1)>>1,node2,(width2*w2)>>1,w2,depth,F,processCurrent); } + template template void OctNode::ProcessMaxDepthNodeAdjacentNodes(int dx,int dy,int dz, @@ -501,6 +532,7 @@ namespace pcl if(processCurrent){F->Function(node2,node1);} if(dchildren){__ProcessMaxDepthNodeAdjacentNodes(-dx,-dy,-dz,node1,radius1,node2,radius2,width2>>1,depth-1,F);} } + template template void OctNode::__ProcessNodeAdjacentNodes(int dx,int dy,int dz, @@ -528,6 +560,7 @@ namespace pcl if(o&128){F->Function(&node2->children[7],node1);if(node2->children[7].children){__ProcessNodeAdjacentNodes(dx2,dy2,dz2,node1,radius1,&node2->children[7],radius,cWidth,F);}} } } + template template void OctNode::__ProcessTerminatingNodeAdjacentNodes(int dx,int dy,int dz, @@ -555,6 +588,7 @@ namespace pcl if(o&128){if(F->Function(&node2->children[7],node1) && node2->children[7].children){__ProcessTerminatingNodeAdjacentNodes(dx2,dy2,dz2,node1,radius1,&node2->children[7],radius,cWidth,F);}} } } + template template void OctNode::__ProcessPointAdjacentNodes(int dx,int dy,int dz, @@ -582,6 +616,7 @@ namespace pcl if(o&128){F->Function(&node2->children[7]);if(node2->children[7].children){__ProcessPointAdjacentNodes(dx2,dy2,dz2,&node2->children[7],radius,cWidth,F);}} } } + template template void OctNode::__ProcessFixedDepthNodeAdjacentNodes(int dx,int dy,int dz, @@ -621,6 +656,7 @@ namespace pcl } } } + template template void OctNode::__ProcessMaxDepthNodeAdjacentNodes(int dx,int dy,int dz, @@ -660,6 +696,7 @@ namespace pcl } } } + template inline int OctNode::ChildOverlap(int dx,int dy,int dz,int d,int cRadius2) { @@ -703,6 +740,7 @@ namespace pcl } return temp; } + template const OctNode* OctNode::getNearestLeaf(const Point3D& p) const{ int nearest; @@ -752,6 +790,7 @@ namespace pcl if(idx1[0]==idx2[0] && idx1[1]==idx2[1]){return 1;} else {return 0;} } + template int OctNode::CornerIndex(const Point3D& center,const Point3D& p){ int cIndex=0; @@ -760,11 +799,12 @@ namespace pcl if(p.coords[2]>center.coords[2]){cIndex|=4;} return cIndex; } + template template OctNode& OctNode::operator = (const OctNode& node){ int i; - if(children){delete[] children;} + delete[] children; children=NULL; d=node.depth (); @@ -775,10 +815,12 @@ namespace pcl } return *this; } + template int OctNode::CompareForwardDepths(const void* v1,const void* v2){ return ((const OctNode*)v1)->depth-((const OctNode*)v2)->depth; } + template< class NodeData , class Real > int OctNode< NodeData , Real >::CompareByDepthAndXYZ( const void* v1 , const void* v2 ) { @@ -798,6 +840,7 @@ namespace pcl for( int i=0 ; i<31 ; i++ ) key |= ( ( _p[0] & (1ull< int OctNode::CompareByDepthAndZIndex( const void* v1 , const void* v2 ) { @@ -829,14 +872,17 @@ namespace pcl return int(n1->off[2])-int(n2->off[2]); return 0; } + template int OctNode::CompareBackwardDepths(const void* v1,const void* v2){ return ((const OctNode*)v2)->depth-((const OctNode*)v1)->depth; } + template int OctNode::CompareBackwardPointerDepths(const void* v1,const void* v2){ return (*(const OctNode**)v2)->depth()-(*(const OctNode**)v1)->depth(); } + template inline int OctNode::Overlap2(const int &depth1,const int offSet1[DIMENSION],const Real& multiplier1,const int &depth2,const int offSet2[DIMENSION],const Real& multiplier2){ int d=depth2-depth1; @@ -849,11 +895,13 @@ namespace pcl ){return 0;} return 1; } + template inline int OctNode::Overlap(int c1,int c2,int c3,int dWidth){ if(c1>=dWidth || c1<=-dWidth || c2>=dWidth || c2<=-dWidth || c3>=dWidth || c3<=-dWidth){return 0;} else{return 1;} } + template OctNode* OctNode::faceNeighbor(int faceIndex,int forceChildren){return __faceNeighbor(faceIndex>>1,faceIndex&1,forceChildren);} template @@ -874,6 +922,7 @@ namespace pcl return &temp->children[pIndex]; } } + template const OctNode* OctNode::__faceNeighbor(int dir,int off) const { if(!parent){return NULL;} @@ -898,6 +947,7 @@ namespace pcl }; return __edgeNeighbor(o,i,idx,forceChildren); } + template const OctNode* OctNode::edgeNeighbor(int edgeIndex) const { int idx[2],o,i[2]; @@ -909,6 +959,7 @@ namespace pcl }; return __edgeNeighbor(o,i,idx); } + template const OctNode* OctNode::__edgeNeighbor(int o,const int i[2],const int idx[2]) const{ if(!parent){return NULL;} @@ -938,6 +989,7 @@ namespace pcl } else{return NULL;} } + template OctNode* OctNode::__edgeNeighbor(int o,const int i[2],const int idx[2],int forceChildren){ if(!parent){return NULL;} @@ -1020,6 +1072,7 @@ namespace pcl } else{return NULL;} } + template OctNode* OctNode::cornerNeighbor(int cornerIndex,int forceChildren){ int pIndex,aIndex=0; @@ -1072,32 +1125,37 @@ namespace pcl } else{return NULL;} } + //////////////////////// // OctNodeNeighborKey // //////////////////////// template OctNode::Neighbors3::Neighbors3(void){clear();} + template void OctNode::Neighbors3::clear(void){ for(int i=0;i<3;i++){for(int j=0;j<3;j++){for(int k=0;k<3;k++){neighbors[i][j][k]=NULL;}}} } + template OctNode::NeighborKey3::NeighborKey3(void){ neighbors=NULL; } + template OctNode::NeighborKey3::~NeighborKey3(void) { - if( neighbors ) delete[] neighbors; + delete[] neighbors; neighbors = NULL; } template void OctNode::NeighborKey3::set( int d ) { - if( neighbors ) delete[] neighbors; + delete[] neighbors; neighbors = NULL; if( d<0 ) return; neighbors = new Neighbors3[d+1]; } + template< class NodeData , class Real > typename OctNode::Neighbors3& OctNode::NeighborKey3::setNeighbors( OctNode* root , Point3D< Real > p , int d ) { @@ -1174,6 +1232,7 @@ namespace pcl } return neighbors[d]; } + template< class NodeData , class Real > typename OctNode::Neighbors3& OctNode::NeighborKey3::getNeighbors( OctNode* root , Point3D< Real > p , int d ) { @@ -1307,6 +1366,7 @@ namespace pcl } return neighbors[d]; } + // Note the assumption is that if you enable an edge, you also enable adjacent faces. // And, if you enable a corner, you enable adjacent edges and faces. template< class NodeData , class Real > @@ -1467,25 +1527,29 @@ namespace pcl /////////////////////// template OctNode::ConstNeighbors3::ConstNeighbors3(void){clear();} + template void OctNode::ConstNeighbors3::clear(void){ for(int i=0;i<3;i++){for(int j=0;j<3;j++){for(int k=0;k<3;k++){neighbors[i][j][k]=NULL;}}} } + template OctNode::ConstNeighborKey3::ConstNeighborKey3(void){neighbors=NULL;} + template OctNode::ConstNeighborKey3::~ConstNeighborKey3(void){ - if(neighbors){delete[] neighbors;} + delete[] neighbors; neighbors=NULL; } template void OctNode::ConstNeighborKey3::set(int d){ - if(neighbors){delete[] neighbors;} + delete[] neighbors; neighbors=NULL; if(d<0){return;} neighbors=new ConstNeighbors3[d+1]; } + template typename OctNode::ConstNeighbors3& OctNode::ConstNeighborKey3::getNeighbors(const OctNode* node){ int d=node->depth(); @@ -1544,6 +1608,7 @@ namespace pcl } return neighbors[node->depth()]; } + template typename OctNode::ConstNeighbors3& OctNode::ConstNeighborKey3::getNeighbors( const OctNode* node , int minDepth ) { @@ -1606,59 +1671,69 @@ namespace pcl } template< class NodeData , class Real > OctNode< NodeData , Real >::Neighbors5::Neighbors5( void ){ clear(); } + template< class NodeData , class Real > OctNode< NodeData , Real >::ConstNeighbors5::ConstNeighbors5( void ){ clear(); } + template< class NodeData , class Real > void OctNode< NodeData , Real >::Neighbors5::clear( void ) { for( int i=0 ; i<5 ; i++ ) for( int j=0 ; j<5 ; j++ ) for( int k=0 ; k<5 ; k++ ) neighbors[i][j][k] = NULL; } + template< class NodeData , class Real > void OctNode< NodeData , Real >::ConstNeighbors5::clear( void ) { for( int i=0 ; i<5 ; i++ ) for( int j=0 ; j<5 ; j++ ) for( int k=0 ; k<5 ; k++ ) neighbors[i][j][k] = NULL; } + template< class NodeData , class Real > OctNode< NodeData , Real >::NeighborKey5::NeighborKey5( void ) { _depth = -1; neighbors = NULL; } + template< class NodeData , class Real > OctNode< NodeData , Real >::ConstNeighborKey5::ConstNeighborKey5( void ) { _depth = -1; neighbors = NULL; } + template< class NodeData , class Real > OctNode< NodeData , Real >::NeighborKey5::~NeighborKey5( void ) { - if( neighbors ) delete[] neighbors; + delete[] neighbors; neighbors = NULL; } + template< class NodeData , class Real > OctNode< NodeData , Real >::ConstNeighborKey5::~ConstNeighborKey5( void ) { - if( neighbors ) delete[] neighbors; + delete[] neighbors; neighbors = NULL; } + template< class NodeData , class Real > void OctNode< NodeData , Real >::NeighborKey5::set( int d ) { - if( neighbors ) delete[] neighbors; + delete[] neighbors; neighbors = NULL; if(d<0) return; _depth = d; neighbors=new Neighbors5[d+1]; } + template< class NodeData , class Real > void OctNode< NodeData , Real >::ConstNeighborKey5::set( int d ) { - if( neighbors ) delete[] neighbors; + delete[] neighbors; neighbors = NULL; if(d<0) return; _depth = d; neighbors=new ConstNeighbors5[d+1]; } + template< class NodeData , class Real > typename OctNode< NodeData , Real >::Neighbors5& OctNode< NodeData , Real >::NeighborKey5::getNeighbors( OctNode* node ) { @@ -1775,6 +1850,7 @@ namespace pcl } return neighbors[d]; } + template< class NodeData , class Real > typename OctNode< NodeData , Real >::Neighbors5& OctNode< NodeData , Real >::NeighborKey5::setNeighbors( OctNode* node , int xStart , int xEnd , int yStart , int yEnd , int zStart , int zEnd ) { @@ -1819,6 +1895,7 @@ namespace pcl } return neighbors[d]; } + template< class NodeData , class Real > typename OctNode< NodeData , Real >::ConstNeighbors5& OctNode< NodeData , Real >::ConstNeighborKey5::getNeighbors( const OctNode* node ) { @@ -1861,7 +1938,6 @@ namespace pcl return neighbors[d]; } - template int OctNode::write(const char* fileName) const{ FILE* fp=fopen(fileName,"wb"); @@ -1870,12 +1946,14 @@ namespace pcl fclose(fp); return ret; } + template int OctNode::write(FILE* fp) const{ fwrite(this,sizeof(OctNode),1,fp); if(children){for(int i=0;i int OctNode::read(const char* fileName){ FILE* fp=fopen(fileName,"rb"); @@ -1884,6 +1962,7 @@ namespace pcl fclose(fp); return ret; } + template int OctNode::read(FILE* fp){ fread(this,sizeof(OctNode),1,fp); @@ -1898,17 +1977,18 @@ namespace pcl } return 1; } + template int OctNode::width(int maxDepth) const { int d=depth(); return 1<<(maxDepth-d); } + template void OctNode::centerIndex(int maxDepth,int index[DIMENSION]) const { int d,o[3]; depthAndOffset(d,o); for(int i=0;i::CornerIndex(maxDepth,d+1,o[i]<<1,1);} } - } } diff --git a/surface/include/pcl/surface/3rdparty/poisson4/polynomial.hpp b/surface/include/pcl/surface/3rdparty/poisson4/polynomial.hpp index 45973dbc..1d7bffa7 100644 --- a/surface/include/pcl/surface/3rdparty/poisson4/polynomial.hpp +++ b/surface/include/pcl/surface/3rdparty/poisson4/polynomial.hpp @@ -8,14 +8,14 @@ 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. +in the documentation and/or other materials provided with the distribution. Neither the name of the Johns Hopkins University nor the names of its contributors may be used to endorse or promote products derived from this software without specific -prior written permission. +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 +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 @@ -26,10 +26,15 @@ ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF S DAMAGE. */ +#include "factor.h" + #include #include + #include -#include "factor.h" + +#include +#include //////////////// // Polynomial // @@ -74,9 +79,9 @@ namespace pcl for(int i=0;i<=Degree;i++){p.coefficients[i+1]=coefficients[i]/(i+1);} return p; } - template<> double Polynomial< 0 >::operator() ( double t ) const { return coefficients[0]; } - template<> double Polynomial< 1 >::operator() ( double t ) const { return coefficients[0]+coefficients[1]*t; } - template<> double Polynomial< 2 >::operator() ( double t ) const { return coefficients[0]+(coefficients[1]+coefficients[2]*t)*t; } + template<> inline double Polynomial< 0 >::operator() ( double t ) const { return coefficients[0]; } + template<> inline double Polynomial< 1 >::operator() ( double t ) const { return coefficients[0]+coefficients[1]*t; } + template<> inline double Polynomial< 2 >::operator() ( double t ) const { return coefficients[0]+(coefficients[1]+coefficients[2]*t)*t; } template double Polynomial::operator() ( double t ) const{ double v=coefficients[Degree]; @@ -296,14 +301,14 @@ namespace pcl } } } - template< > + template< > inline Polynomial< 0 > Polynomial< 0 >::BSplineComponent( int i ) { Polynomial p; p.coefficients[0] = 1.; return p; } - template< int Degree > + template< int Degree > inline Polynomial< Degree > Polynomial< Degree >::BSplineComponent( int i ) { Polynomial p; diff --git a/surface/include/pcl/surface/3rdparty/poisson4/ppolynomial.hpp b/surface/include/pcl/surface/3rdparty/poisson4/ppolynomial.hpp index 5f9de7d3..6d1d78a4 100644 --- a/surface/include/pcl/surface/3rdparty/poisson4/ppolynomial.hpp +++ b/surface/include/pcl/surface/3rdparty/poisson4/ppolynomial.hpp @@ -28,6 +28,9 @@ DAMAGE. #include "factor.h" +#include +#include + //////////////////////// // StartingPolynomial // //////////////////////// @@ -370,7 +373,7 @@ namespace pcl } printf("\n"); } - template< > + template< > inline PPolynomial< 0 > PPolynomial< 0 >::BSpline( double radius ) { PPolynomial q; diff --git a/surface/include/pcl/surface/bilateral_upsampling.h b/surface/include/pcl/surface/bilateral_upsampling.h index 0b3fd47d..193e64dd 100644 --- a/surface/include/pcl/surface/bilateral_upsampling.h +++ b/surface/include/pcl/surface/bilateral_upsampling.h @@ -39,6 +39,7 @@ #pragma once +#include #include #include diff --git a/surface/include/pcl/surface/boost.h b/surface/include/pcl/surface/boost.h index 208e3bd2..eb076d1f 100644 --- a/surface/include/pcl/surface/boost.h +++ b/surface/include/pcl/surface/boost.h @@ -40,8 +40,7 @@ #pragma once #if defined __GNUC__ -# pragma GCC system_header +# pragma GCC system_header #endif #include -#include diff --git a/surface/include/pcl/surface/convex_hull.h b/surface/include/pcl/surface/convex_hull.h index 4bad4b8b..2db2e92a 100644 --- a/surface/include/pcl/surface/convex_hull.h +++ b/surface/include/pcl/surface/convex_hull.h @@ -39,6 +39,7 @@ #pragma once +#include #include #include #ifdef HAVE_QHULL diff --git a/surface/include/pcl/surface/grid_projection.h b/surface/include/pcl/surface/grid_projection.h index 94fe4320..e1d5db84 100644 --- a/surface/include/pcl/surface/grid_projection.h +++ b/surface/include/pcl/surface/grid_projection.h @@ -37,6 +37,7 @@ #pragma once +#include #include #include #include diff --git a/surface/include/pcl/surface/impl/mls.hpp b/surface/include/pcl/surface/impl/mls.hpp index 028d2702..f98a9609 100644 --- a/surface/include/pcl/surface/impl/mls.hpp +++ b/surface/include/pcl/surface/impl/mls.hpp @@ -40,7 +40,7 @@ #ifndef PCL_SURFACE_IMPL_MLS_H_ #define PCL_SURFACE_IMPL_MLS_H_ -#include +#include #include #include #include @@ -292,9 +292,11 @@ pcl::MovingLeastSquares::performProcessing (PointCloudOut & #endif // For all points -#ifdef _OPENMP -#pragma omp parallel for schedule (dynamic,1000) num_threads (threads) -#endif +#pragma omp parallel for \ + default(none) \ + shared(corresponding_input_indices, projected_points, projected_points_normals) \ + schedule(dynamic,1000) \ + num_threads(threads) for (int cp = 0; cp < static_cast (indices_->size ()); ++cp) { // Allocate enough space to hold the results of nearest neighbor searches diff --git a/surface/include/pcl/surface/impl/processing.hpp b/surface/include/pcl/surface/impl/processing.hpp index 1cdaa821..4c262cdc 100644 --- a/surface/include/pcl/surface/impl/processing.hpp +++ b/surface/include/pcl/surface/impl/processing.hpp @@ -37,10 +37,14 @@ * */ +#pragma once + + +namespace pcl +{ -////////////////////////////////////////////////////////////////////////////////////////////// template void -pcl::CloudSurfaceProcessing::process (pcl::PointCloud &output) +CloudSurfaceProcessing::process (pcl::PointCloud &output) { // Copy the header output.header = input_->header; @@ -57,3 +61,6 @@ pcl::CloudSurfaceProcessing::process (pcl::PointCloud -////////////////////////////////////////////////////////////////////////////////////////////// + +namespace pcl +{ + template void -pcl::SurfaceReconstruction::reconstruct (pcl::PolygonMesh &output) +SurfaceReconstruction::reconstruct (pcl::PolygonMesh &output) { // Copy the header output.header = input_->header; - if (!initCompute ()) + if (!initCompute ()) { output.cloud.width = output.cloud.height = 0; output.cloud.data.clear (); @@ -81,15 +85,15 @@ pcl::SurfaceReconstruction::reconstruct (pcl::PolygonMesh &output) deinitCompute (); } -////////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::SurfaceReconstruction::reconstruct (pcl::PointCloud &points, - std::vector &polygons) +SurfaceReconstruction::reconstruct (pcl::PointCloud &points, + std::vector &polygons) { // Copy the header points.header = input_->header; - if (!initCompute ()) + if (!initCompute ()) { points.width = points.height = 0; points.clear (); @@ -121,14 +125,14 @@ pcl::SurfaceReconstruction::reconstruct (pcl::PointCloud &po deinitCompute (); } -////////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::MeshConstruction::reconstruct (pcl::PolygonMesh &output) +MeshConstruction::reconstruct (pcl::PolygonMesh &output) { // Copy the header output.header = input_->header; - if (!initCompute ()) + if (!initCompute ()) { output.cloud.width = output.cloud.height = 1; output.cloud.data.clear (); @@ -161,11 +165,11 @@ pcl::MeshConstruction::reconstruct (pcl::PolygonMesh &output) deinitCompute (); } -////////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::MeshConstruction::reconstruct (std::vector &polygons) +MeshConstruction::reconstruct (std::vector &polygons) { - if (!initCompute ()) + if (!initCompute ()) { polygons.clear (); return; @@ -195,6 +199,7 @@ pcl::MeshConstruction::reconstruct (std::vector &polygo deinitCompute (); } +} // namespace pcl #endif // PCL_SURFACE_RECONSTRUCTION_IMPL_H_ diff --git a/surface/include/pcl/surface/impl/texture_mapping.hpp b/surface/include/pcl/surface/impl/texture_mapping.hpp index a6d60716..48ad61ef 100644 --- a/surface/include/pcl/surface/impl/texture_mapping.hpp +++ b/surface/include/pcl/surface/impl/texture_mapping.hpp @@ -40,6 +40,7 @@ #include #include +#include /////////////////////////////////////////////////////////////////////////////////////////////// template std::vector > @@ -579,6 +580,9 @@ pcl::TextureMapping::sortFacesByCamera (pcl::TextureMesh &tex_mesh, pc removeOccludedPoints (transformed_cloud, filtered_cloud, octree_voxel_size, visible, occluded); visible_pts = *filtered_cloud; + // pushing occluded idxs into a set for faster lookup + std::unordered_set occluded_set(occluded.cbegin(), occluded.cend()); + // find visible faces => add them to polygon N for camera N // add polygon group for current camera in clean std::vector visibleFaces_currentCam; @@ -586,32 +590,18 @@ pcl::TextureMapping::sortFacesByCamera (pcl::TextureMesh &tex_mesh, pc for (std::size_t faces = 0; faces < tex_mesh.tex_polygons[0].size (); ++faces) { // check if all the face's points are visible - bool faceIsVisible = true; - std::vector::iterator it; - // iterate over face's vertex - for (std::size_t current_pt_indice = 0; faceIsVisible && current_pt_indice < tex_mesh.tex_polygons[0][faces].vertices.size (); ++current_pt_indice) + const auto faceIsVisible = std::all_of(tex_mesh.tex_polygons[0][faces].vertices.cbegin(), + tex_mesh.tex_polygons[0][faces].vertices.cend(), + [&](const auto& vertex) { - // TODO this is far too long! Better create an helper function that raycasts here. - it = find (occluded.begin (), occluded.end (), tex_mesh.tex_polygons[0][faces].vertices[current_pt_indice]); - - if (it == occluded.end ()) - { - // point is not occluded - // does it land on the camera's image plane? - PointInT pt = transformed_cloud->points[tex_mesh.tex_polygons[0][faces].vertices[current_pt_indice]]; - Eigen::Vector2f dummy_UV; - if (!getPointUVCoordinates (pt, camera, dummy_UV)) - { - // point is not visible by the camera - faceIsVisible = false; + if (occluded_set.find(vertex) != occluded_set.cend()) { + return false; // point is occluded } - } - else - { - faceIsVisible = false; - } - } + // is the point visible to the camera? + Eigen::Vector2f dummy_UV; + return this->getPointUVCoordinates ((*transformed_cloud)[vertex], camera, dummy_UV); + }); if (faceIsVisible) { @@ -1099,4 +1089,3 @@ pcl::TextureMapping::isFaceProjected (const Camera &camera, const Poin template class PCL_EXPORTS pcl::TextureMapping; #endif /* TEXTURE_MAPPING_HPP_ */ - diff --git a/surface/include/pcl/surface/marching_cubes.h b/surface/include/pcl/surface/marching_cubes.h index 5a499be5..514c2545 100644 --- a/surface/include/pcl/surface/marching_cubes.h +++ b/surface/include/pcl/surface/marching_cubes.h @@ -35,6 +35,7 @@ #pragma once +#include #include #include #include diff --git a/surface/include/pcl/surface/marching_cubes_hoppe.h b/surface/include/pcl/surface/marching_cubes_hoppe.h index ebdfb71d..3912b177 100644 --- a/surface/include/pcl/surface/marching_cubes_hoppe.h +++ b/surface/include/pcl/surface/marching_cubes_hoppe.h @@ -35,6 +35,7 @@ #pragma once +#include #include #include #include @@ -91,7 +92,7 @@ namespace pcl /** \brief Method that sets the distance for ignoring voxels which are far from point cloud. * If the distance is negative, then the distance functions would be calculated in all voxels; * otherwise, only voxels with distance lower than dist_ignore would be involved in marching cube. - * \param[in] threshold of distance. Default value is -1.0. Set to negative if all voxels are + * \param[in] dist_ignore threshold of distance. Default value is -1.0. Set to negative if all voxels are * to be involved. */ inline void diff --git a/surface/include/pcl/surface/marching_cubes_rbf.h b/surface/include/pcl/surface/marching_cubes_rbf.h index 22fa923a..f8b69193 100644 --- a/surface/include/pcl/surface/marching_cubes_rbf.h +++ b/surface/include/pcl/surface/marching_cubes_rbf.h @@ -35,6 +35,7 @@ #pragma once +#include #include #include #include diff --git a/surface/include/pcl/surface/mls.h b/surface/include/pcl/surface/mls.h index 232a3975..deca6064 100644 --- a/surface/include/pcl/surface/mls.h +++ b/surface/include/pcl/surface/mls.h @@ -44,6 +44,7 @@ #include // PCL includes +#include #include #include #include @@ -356,7 +357,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 */ - PCL_DEPRECATED("use setPolynomialOrder() instead") + PCL_DEPRECATED(1, 12, "use setPolynomialOrder() instead") inline void setPolynomialFit (bool polynomial_fit) { @@ -374,7 +375,7 @@ namespace pcl } /** \brief Get the polynomial_fit value (true if the surface and normal are approximated using a polynomial). */ - PCL_DEPRECATED("use getPolynomialOrder() instead") + PCL_DEPRECATED(1, 12, "use getPolynomialOrder() instead") inline bool getPolynomialFit () const { return (order_ > 1); } @@ -485,7 +486,7 @@ namespace pcl getDilationIterations () const { return (dilation_iteration_num_); } /** \brief Set whether the mls results should be stored for each point in the input cloud - * \param[in] True if the mls results should be stored, otherwise false. + * \param[in] cache_mls_results True if the mls results should be stored, otherwise false. * \note The cache_mls_results_ is forced to true when using upsampling method VOXEL_GRID_DILATION or DISTINCT_CLOUD. * \note If memory consumption is a concern set to false when not using upsampling method VOXEL_GRID_DILATION or DISTINCT_CLOUD. */ @@ -746,7 +747,7 @@ namespace pcl }; template - using MovingLeastSquaresOMP PCL_DEPRECATED("use MovingLeastSquares instead, it supports OpenMP now") = MovingLeastSquares; + using MovingLeastSquaresOMP PCL_DEPRECATED(1, 12, "use MovingLeastSquares instead, it supports OpenMP now") = MovingLeastSquares; } #ifdef PCL_NO_PRECOMPILE diff --git a/surface/include/pcl/surface/on_nurbs/fitting_cylinder_pdm.h b/surface/include/pcl/surface/on_nurbs/fitting_cylinder_pdm.h index 6752ae8f..999c6b3b 100644 --- a/surface/include/pcl/surface/on_nurbs/fitting_cylinder_pdm.h +++ b/surface/include/pcl/surface/on_nurbs/fitting_cylinder_pdm.h @@ -197,12 +197,12 @@ namespace pcl return grc2gl (E + i, F + j); } // local row/col index to global lexicographic index int - gl2gr (int A) + gl2gr (int A) const { return (A / m_nurbs.CVCount (1)); } // global lexicographic in global row index int - gl2gc (int A) + gl2gc (int A) const { return (A % m_nurbs.CVCount (1)); } // global lexicographic in global col index diff --git a/surface/include/pcl/surface/on_nurbs/fitting_sphere_pdm.h b/surface/include/pcl/surface/on_nurbs/fitting_sphere_pdm.h index 41c77bb6..83882168 100644 --- a/surface/include/pcl/surface/on_nurbs/fitting_sphere_pdm.h +++ b/surface/include/pcl/surface/on_nurbs/fitting_sphere_pdm.h @@ -175,12 +175,12 @@ namespace pcl return grc2gl (E + i, F + j); } // local row/col index to global lexicographic index int - gl2gr (int A) + gl2gr (int A) const { return (A / m_nurbs.CVCount (1)); } // global lexicographic in global row index int - gl2gc (int A) + gl2gc (int A) const { return (A % m_nurbs.CVCount (1)); } // global lexicographic in global col index diff --git a/surface/include/pcl/surface/on_nurbs/fitting_surface_im.h b/surface/include/pcl/surface/on_nurbs/fitting_surface_im.h index d074c996..3fefdcf3 100644 --- a/surface/include/pcl/surface/on_nurbs/fitting_surface_im.h +++ b/surface/include/pcl/surface/on_nurbs/fitting_surface_im.h @@ -41,6 +41,7 @@ #include #include +#include #include #include #include diff --git a/surface/include/pcl/surface/on_nurbs/fitting_surface_pdm.h b/surface/include/pcl/surface/on_nurbs/fitting_surface_pdm.h index d0794e1c..8d6654aa 100644 --- a/surface/include/pcl/surface/on_nurbs/fitting_surface_pdm.h +++ b/surface/include/pcl/surface/on_nurbs/fitting_surface_pdm.h @@ -291,7 +291,7 @@ namespace pcl // index routines int - grc2gl (int I, int J) + grc2gl (int I, int J) const { return m_nurbs.CVCount (1) * I + J; } // global row/col index to global lexicographic index @@ -301,12 +301,12 @@ namespace pcl return grc2gl (E + i, F + j); } // local row/col index to global lexicographic index int - gl2gr (int A) + gl2gr (int A) const { return (static_cast (A / m_nurbs.CVCount (1))); } // global lexicographic in global row index int - gl2gc (int A) + gl2gc (int A) const { return (static_cast (A % m_nurbs.CVCount (1))); } // global lexicographic in global col index diff --git a/surface/include/pcl/surface/on_nurbs/nurbs_data.h b/surface/include/pcl/surface/on_nurbs/nurbs_data.h index 9b5a0786..7639c0ac 100644 --- a/surface/include/pcl/surface/on_nurbs/nurbs_data.h +++ b/surface/include/pcl/surface/on_nurbs/nurbs_data.h @@ -44,6 +44,7 @@ #undef Success #include +#include #include namespace pcl diff --git a/surface/include/pcl/surface/on_nurbs/nurbs_solve.h b/surface/include/pcl/surface/on_nurbs/nurbs_solve.h index 89a672ae..de6ae41d 100644 --- a/surface/include/pcl/surface/on_nurbs/nurbs_solve.h +++ b/surface/include/pcl/surface/on_nurbs/nurbs_solve.h @@ -40,6 +40,7 @@ #undef Success #include +#include #include #include diff --git a/surface/include/pcl/surface/on_nurbs/sequential_fitter.h b/surface/include/pcl/surface/on_nurbs/sequential_fitter.h index 4796ecfc..79131069 100644 --- a/surface/include/pcl/surface/on_nurbs/sequential_fitter.h +++ b/surface/include/pcl/surface/on_nurbs/sequential_fitter.h @@ -31,7 +31,7 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * - * + * * */ @@ -50,6 +50,7 @@ #include #include +#include #include #include #include @@ -105,18 +106,18 @@ namespace pcl void compute_quadfit (); void - compute_refinement (FittingSurface* fitting); + compute_refinement (FittingSurface* fitting) const; void - compute_boundary (FittingSurface* fitting); + compute_boundary (FittingSurface* fitting) const; void - compute_interior (FittingSurface* fitting); + compute_interior (FittingSurface* fitting) const; Eigen::Vector2d project (const Eigen::Vector3d &pt); bool - is_back_facing (const Eigen::Vector3d &v0, - const Eigen::Vector3d &v1, + is_back_facing (const Eigen::Vector3d &v0, + const Eigen::Vector3d &v1, const Eigen::Vector3d &v2, const Eigen::Vector3d &v3); @@ -149,7 +150,7 @@ namespace pcl * \param[in] intrinsic The camera projection matrix. * \param[in] intrinsic The world matrix.*/ void - setProjectionMatrix (const Eigen::Matrix4d &intrinsic, + setProjectionMatrix (const Eigen::Matrix4d &intrinsic, const Eigen::Matrix4d &extrinsic); /** \brief Compute point cloud and fit (multiple) models */ @@ -175,27 +176,27 @@ namespace pcl /** \brief Get error of each interior point (L2-norm of point to closest point on surface) and square-error */ void - getInteriorError (std::vector &error); + getInteriorError (std::vector &error) const; /** \brief Get error of each boundary point (L2-norm of point to closest point on surface) and square-error */ void - getBoundaryError (std::vector &error); + getBoundaryError (std::vector &error) const; /** \brief Get parameter of each interior point */ void - getInteriorParams (std::vector > ¶ms); + getInteriorParams (std::vector > ¶ms) const; /** \brief Get parameter of each boundary point */ void - getBoundaryParams (std::vector > ¶ms); + getBoundaryParams (std::vector > ¶ms) const; /** \brief get the normals to the interior points given by setInterior() */ void - getInteriorNormals (std::vector > &normal); + getInteriorNormals (std::vector > &normal) const; /** \brief get the normals to the boundary points given by setBoundary() */ void - getBoundaryNormals (std::vector > &normals); + getBoundaryNormals (std::vector > &normals) const; /** \brief Get the closest point on a NURBS from a point pt in parameter space * \param[in] nurbs The NURBS surface @@ -204,9 +205,9 @@ namespace pcl * \param[in] maxSteps Maximum iteration steps * \param[in] accuracy Accuracy below which the iterations stop */ static void - getClosestPointOnNurbs (ON_NurbsSurface nurbs, - const Eigen::Vector3d &pt, - Eigen::Vector2d& params, + getClosestPointOnNurbs (ON_NurbsSurface nurbs, + const Eigen::Vector3d &pt, + Eigen::Vector2d& params, int maxSteps = 100, double accuracy = 1e-4); diff --git a/surface/include/pcl/surface/organized_fast_mesh.h b/surface/include/pcl/surface/organized_fast_mesh.h index b9fe9f0b..60312cc5 100644 --- a/surface/include/pcl/surface/organized_fast_mesh.h +++ b/surface/include/pcl/surface/organized_fast_mesh.h @@ -41,8 +41,10 @@ #pragma once #include +#include // for pcl::isFinite #include + namespace pcl { diff --git a/surface/include/pcl/surface/poisson.h b/surface/include/pcl/surface/poisson.h index 2aefc1be..0c6ceeae 100644 --- a/surface/include/pcl/surface/poisson.h +++ b/surface/include/pcl/surface/poisson.h @@ -37,6 +37,7 @@ #pragma once +#include #include #include diff --git a/surface/include/pcl/surface/simplification_remove_unused_vertices.h b/surface/include/pcl/surface/simplification_remove_unused_vertices.h index 06161044..02f9b5e2 100644 --- a/surface/include/pcl/surface/simplification_remove_unused_vertices.h +++ b/surface/include/pcl/surface/simplification_remove_unused_vertices.h @@ -39,6 +39,7 @@ #include #include +#include #include namespace pcl diff --git a/surface/include/pcl/surface/texture_mapping.h b/surface/include/pcl/surface/texture_mapping.h index 59ea2cfd..85250080 100644 --- a/surface/include/pcl/surface/texture_mapping.h +++ b/surface/include/pcl/surface/texture_mapping.h @@ -39,6 +39,7 @@ #pragma once +#include #include #include #include diff --git a/surface/include/pcl/surface/vtk_smoothing/vtk_mesh_quadric_decimation.h b/surface/include/pcl/surface/vtk_smoothing/vtk_mesh_quadric_decimation.h index 7bb22a28..3f9884b2 100644 --- a/surface/include/pcl/surface/vtk_smoothing/vtk_mesh_quadric_decimation.h +++ b/surface/include/pcl/surface/vtk_smoothing/vtk_mesh_quadric_decimation.h @@ -65,7 +65,7 @@ namespace pcl /** \brief Get the target reduction factor */ inline float - getTargetReductionFactor () + getTargetReductionFactor () const { return target_reduction_factor_; } diff --git a/surface/include/pcl/surface/vtk_smoothing/vtk_mesh_smoothing_laplacian.h b/surface/include/pcl/surface/vtk_smoothing/vtk_mesh_smoothing_laplacian.h index 67755ca6..e2edb771 100644 --- a/surface/include/pcl/surface/vtk_smoothing/vtk_mesh_smoothing_laplacian.h +++ b/surface/include/pcl/surface/vtk_smoothing/vtk_mesh_smoothing_laplacian.h @@ -73,7 +73,7 @@ namespace pcl /** \brief Get the number of iterations. */ inline int - getNumIter () + getNumIter () const { return num_iter_; }; @@ -89,7 +89,7 @@ namespace pcl /** \brief Get the convergence criterion. */ inline float - getConvergence () + getConvergence () const { return convergence_; }; @@ -108,7 +108,7 @@ namespace pcl /** \brief Get the relaxation factor of the Laplacian smoothing */ inline float - getRelaxationFactor () + getRelaxationFactor () const { return relaxation_factor_; }; @@ -124,7 +124,7 @@ namespace pcl /** \brief Get the status of the feature edge smoothing */ inline bool - getFeatureEdgeSmoothing () + getFeatureEdgeSmoothing () const { return feature_edge_smoothing_; }; @@ -140,7 +140,7 @@ namespace pcl /** \brief Get the angle threshold for considering an edge to be sharp */ inline float - getFeatureAngle () + getFeatureAngle () const { return feature_angle_; }; @@ -156,7 +156,7 @@ namespace pcl /** \brief Get the edge angle to control smoothing along edges */ inline float - getEdgeAngle () + getEdgeAngle () const { return edge_angle_; }; @@ -172,7 +172,7 @@ namespace pcl /** \brief Get the status of the boundary smoothing */ inline bool - getBoundarySmoothing () + getBoundarySmoothing () const { return boundary_smoothing_; } diff --git a/surface/include/pcl/surface/vtk_smoothing/vtk_mesh_smoothing_windowed_sinc.h b/surface/include/pcl/surface/vtk_smoothing/vtk_mesh_smoothing_windowed_sinc.h index 7780eaad..220e17ba 100644 --- a/surface/include/pcl/surface/vtk_smoothing/vtk_mesh_smoothing_windowed_sinc.h +++ b/surface/include/pcl/surface/vtk_smoothing/vtk_mesh_smoothing_windowed_sinc.h @@ -73,7 +73,7 @@ namespace pcl /** \brief Get the number of iterations. */ inline int - getNumIter () + getNumIter () const { return num_iter_; }; @@ -89,7 +89,7 @@ namespace pcl /** \brief Get the pass band value. */ inline float - getPassBand () + getPassBand () const { return pass_band_; }; @@ -108,7 +108,7 @@ namespace pcl /** \brief Get whether the coordinate normalization is active or not */ inline bool - getNormalizeCoordinates () + getNormalizeCoordinates () const { return normalize_coordinates_; } @@ -124,7 +124,7 @@ namespace pcl /** \brief Get the status of the feature edge smoothing */ inline bool - getFeatureEdgeSmoothing () + getFeatureEdgeSmoothing () const { return feature_edge_smoothing_; }; @@ -140,7 +140,7 @@ namespace pcl /** \brief Get the angle threshold for considering an edge to be sharp */ inline float - getFeatureAngle () + getFeatureAngle () const { return feature_angle_; }; @@ -156,7 +156,7 @@ namespace pcl /** \brief Get the edge angle to control smoothing along edges */ inline float - getEdgeAngle () + getEdgeAngle () const { return edge_angle_; }; @@ -173,7 +173,7 @@ namespace pcl /** \brief Get the status of the boundary smoothing */ inline bool - getBoundarySmoothing () + getBoundarySmoothing () const { return boundary_smoothing_; } diff --git a/surface/src/3rdparty/opennurbs/opennurbs_annotation2.cpp b/surface/src/3rdparty/opennurbs/opennurbs_annotation2.cpp index 1e3d2b08..e9fac7bc 100644 --- a/surface/src/3rdparty/opennurbs/opennurbs_annotation2.cpp +++ b/surface/src/3rdparty/opennurbs/opennurbs_annotation2.cpp @@ -5822,7 +5822,7 @@ void ON_Annotation2Text::SetText(const wchar_t* s) // SDKBREAK Oct 30, 07 - LW // This function should not be used any longer -PCL_DEPRECATED("Use the version that takes a model transform argument") +PCL_DEPRECATED(1, 12, "Use the version that takes a model transform argument") bool ON_Annotation2::GetTextXform( ON_RECT /*gdi_text_rect*/, const ON_Font& /*font*/, @@ -5862,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 -PCL_DEPRECATED("Use the version that takes a dimstyle pointer") +PCL_DEPRECATED(1, 12, "Use the version that takes a dimstyle pointer") bool ON_Annotation2::GetTextXform( ON_RECT gdi_text_rect, const ON_Font& font, @@ -5991,7 +5991,7 @@ static bool GetLeaderEndAndDirection( const ON_Annotation2* pAnn, // SDKBREAK Oct 30, 07 - LW // This function should not be used any longer -PCL_DEPRECATED("Use the version that takes a model transform argument") +PCL_DEPRECATED(1, 12, "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/3rdparty/opennurbs/opennurbs_evaluate_nurbs.cpp b/surface/src/3rdparty/opennurbs/opennurbs_evaluate_nurbs.cpp index 7090b932..9d5374b8 100644 --- a/surface/src/3rdparty/opennurbs/opennurbs_evaluate_nurbs.cpp +++ b/surface/src/3rdparty/opennurbs/opennurbs_evaluate_nurbs.cpp @@ -509,10 +509,10 @@ RELATED FUNCTIONS: { unsigned char stack_buffer[4*64*sizeof(double)]; double delta_t; - register double alpha0; - register double alpha1; - register double *cv0, *cv1; - register int i, j, k; + double alpha0; + double alpha1; + double *cv0, *cv1; + int i, j, k; double* CV, *tmp; void* free_me = 0; const int degree = order-1; @@ -733,11 +733,11 @@ RELATED FUNCTIONS: TL_EvNurbBasis TL_EvNurbBasisDer *****************************************************************************/ - register double a0, a1, x, y; + double a0, a1, x, y; const double *k0; double *t_k, *k_t, *N0; const int d = order-1; - register int j, r; + int j, r; t_k = (double*)alloca( d<<4 ); k_t = t_k + d; diff --git a/surface/src/3rdparty/poisson4/bspline_data.cpp b/surface/src/3rdparty/poisson4/bspline_data.cpp new file mode 100644 index 00000000..7c19a76c --- /dev/null +++ b/surface/src/3rdparty/poisson4/bspline_data.cpp @@ -0,0 +1,99 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2020-, OpenPerception + * + * 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 + +namespace pcl { +namespace poisson { +template <> +void +BSplineElements<1>::upSample(BSplineElements<1>& high) const +{ + high.resize(size() * 2); + high.assign(high.size(), BSplineElementCoefficients<1>()); + for (int i = 0; i < int(size()); i++) { + high[2 * i + 0][0] += 1 * (*this)[i][0]; + high[2 * i + 0][1] += 0 * (*this)[i][0]; + high[2 * i + 1][0] += 2 * (*this)[i][0]; + high[2 * i + 1][1] += 1 * (*this)[i][0]; + + high[2 * i + 0][0] += 1 * (*this)[i][1]; + high[2 * i + 0][1] += 2 * (*this)[i][1]; + high[2 * i + 1][0] += 0 * (*this)[i][1]; + high[2 * i + 1][1] += 1 * (*this)[i][1]; + } + high.denominator = denominator * 2; +} + +template <> +void +BSplineElements<2>::upSample(BSplineElements<2>& high) const +{ + /* /----\ + * / \ + * / \ = 1 /--\ +3 /--\ +3 /--\ +1 /--\ + * / \ / \ / \ / \ / \ + * |----------| |----------| |----------| |----------| |----------| + */ + + high.resize(size() * 2); + high.assign(high.size(), BSplineElementCoefficients<2>()); + for (int i = 0; i < int(size()); i++) { + high[2 * i + 0][0] += 1 * (*this)[i][0]; + high[2 * i + 0][1] += 0 * (*this)[i][0]; + high[2 * i + 0][2] += 0 * (*this)[i][0]; + high[2 * i + 1][0] += 3 * (*this)[i][0]; + high[2 * i + 1][1] += 1 * (*this)[i][0]; + high[2 * i + 1][2] += 0 * (*this)[i][0]; + + high[2 * i + 0][0] += 3 * (*this)[i][1]; + high[2 * i + 0][1] += 3 * (*this)[i][1]; + high[2 * i + 0][2] += 1 * (*this)[i][1]; + high[2 * i + 1][0] += 1 * (*this)[i][1]; + high[2 * i + 1][1] += 3 * (*this)[i][1]; + high[2 * i + 1][2] += 3 * (*this)[i][1]; + + high[2 * i + 0][0] += 0 * (*this)[i][2]; + high[2 * i + 0][1] += 1 * (*this)[i][2]; + high[2 * i + 0][2] += 3 * (*this)[i][2]; + high[2 * i + 1][0] += 0 * (*this)[i][2]; + high[2 * i + 1][1] += 0 * (*this)[i][2]; + high[2 * i + 1][2] += 1 * (*this)[i][2]; + } + high.denominator = denominator * 4; +} +} // namespace poisson +} // namespace pcl diff --git a/surface/src/on_nurbs/sequential_fitter.cpp b/surface/src/on_nurbs/sequential_fitter.cpp index ea8ffd71..fcd68467 100644 --- a/surface/src/on_nurbs/sequential_fitter.cpp +++ b/surface/src/on_nurbs/sequential_fitter.cpp @@ -122,7 +122,7 @@ SequentialFitter::compute_quadfit () } void -SequentialFitter::compute_refinement (FittingSurface* fitting) +SequentialFitter::compute_refinement (FittingSurface* fitting) const { // Refinement FittingSurface::Parameter paramFP (0.0, 1.0, 0.0, m_params.forceBoundary, m_params.stiffnessBoundary, 0.0); @@ -137,7 +137,7 @@ SequentialFitter::compute_refinement (FittingSurface* fitting) } void -SequentialFitter::compute_boundary (FittingSurface* fitting) +SequentialFitter::compute_boundary (FittingSurface* fitting) const { FittingSurface::Parameter paramFP (0.0, 1.0, 0.0, m_params.forceBoundary, m_params.stiffnessBoundary, 0.0); @@ -149,7 +149,7 @@ SequentialFitter::compute_boundary (FittingSurface* fitting) } } void -SequentialFitter::compute_interior (FittingSurface* fitting) +SequentialFitter::compute_interior (FittingSurface* fitting) const { // iterate interior points //std::vector wInt(m_data.interior.PointCount(), m_params.forceInterior); @@ -420,37 +420,37 @@ SequentialFitter::compute_interior (const ON_NurbsSurface &nurbs) } void -SequentialFitter::getInteriorError (std::vector &error) +SequentialFitter::getInteriorError (std::vector &error) const { error = m_data.interior_error; } void -SequentialFitter::getBoundaryError (std::vector &error) +SequentialFitter::getBoundaryError (std::vector &error) const { error = m_data.boundary_error; } void -SequentialFitter::getInteriorParams (std::vector > ¶ms) +SequentialFitter::getInteriorParams (std::vector > ¶ms) const { params = m_data.interior_param; } void -SequentialFitter::getBoundaryParams (std::vector > ¶ms) +SequentialFitter::getBoundaryParams (std::vector > ¶ms) const { params = m_data.boundary_param; } void -SequentialFitter::getInteriorNormals (std::vector > &normals) +SequentialFitter::getInteriorNormals (std::vector > &normals) const { normals = m_data.interior_normals; } void -SequentialFitter::getBoundaryNormals (std::vector > &normals) +SequentialFitter::getBoundaryNormals (std::vector > &normals) const { normals = m_data.boundary_normals; } diff --git a/test/2d/CMakeLists.txt b/test/2d/CMakeLists.txt index 5791ec27..c1d8c2ac 100644 --- a/test/2d/CMakeLists.txt +++ b/test/2d/CMakeLists.txt @@ -26,3 +26,8 @@ PCL_ADD_TEST(test_2d test_2d FILES test_2d.cpp "${PCL_SOURCE_DIR}/test/2d/opening_binary.pcd" "${PCL_SOURCE_DIR}/test/2d/closing_binary.pcd" "${PCL_SOURCE_DIR}/test/2d/canny.pcd") + +PCL_ADD_TEST(test_2d_keypoint_instantiation_with_precompile test_2d_keypoint_instantiation_with_precompile FILES keypoint_instantiation.cpp LINK_WITH pcl_gtest pcl_common) + +PCL_ADD_TEST(test_2d_keypoint_instantiation_without_precompile test_2d_keypoint_instantiation_without_precompile FILES keypoint_instantiation.cpp LINK_WITH pcl_gtest pcl_common) +target_compile_definitions(test_2d_keypoint_instantiation_without_precompile PUBLIC PCL_NO_PRECOMPILE) diff --git a/test/2d/keypoint_instantiation.cpp b/test/2d/keypoint_instantiation.cpp new file mode 100644 index 00000000..6b8a9154 --- /dev/null +++ b/test/2d/keypoint_instantiation.cpp @@ -0,0 +1,56 @@ +/** + * 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 Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include // for pcl::Keypoint + +#include // for pcl::PointXYZ +#include // for SUCCEED + +/** This isn't useful except for testing the instantiation of this class. See #3898 */ +TEST (Keypoint, instantiatesWithAndWithoutPrecompiledHeaders) +{ + pcl::Keypoint keypoint = pcl::Keypoint(); + SUCCEED(); +} + +/** --[ */ +int +main (int argc, char** argv) +{ + testing::InitGoogleTest (&argc, argv); + return (RUN_ALL_TESTS ()); +} +/* ]-- */ diff --git a/test/2d/test_2d.cpp b/test/2d/test_2d.cpp index 2301318a..b669b589 100644 --- a/test/2d/test_2d.cpp +++ b/test/2d/test_2d.cpp @@ -67,8 +67,8 @@ using namespace pcl; TEST (Convolution, borderOptions) { - kernel *k = new kernel (); - Convolution *conv = new Convolution (); + kernel k; + Convolution conv; /*dummy clouds*/ pcl::PointCloud::Ptr input_cloud (new pcl::PointCloud); @@ -80,14 +80,14 @@ TEST (Convolution, borderOptions) int height = input_cloud->height; int width = input_cloud->width; - k->setKernelType(kernel::DERIVATIVE_CENTRAL_X); - k->fetchKernel (*kernel_cloud); + k.setKernelType(kernel::DERIVATIVE_CENTRAL_X); + k.fetchKernel (*kernel_cloud); - conv->setKernel(*kernel_cloud); - conv->setInputCloud(input_cloud); + conv.setKernel(*kernel_cloud); + conv.setInputCloud(input_cloud); - conv->setBoundaryOptions(Convolution::BOUNDARY_OPTION_MIRROR); - conv->filter (*output_cloud); + conv.setBoundaryOptions(Convolution::BOUNDARY_OPTION_MIRROR); + conv.filter (*output_cloud); for (int i = 1; i < height - 1; i++) for (int j = 1; j < width - 1; j++) EXPECT_NEAR ((*output_cloud)(j,i).intensity, ((*input_cloud)(j+1,i).intensity-(*input_cloud)(j-1,i).intensity), 1); @@ -98,8 +98,8 @@ TEST (Convolution, borderOptions) EXPECT_NEAR ((*output_cloud)(width-1,i).intensity, ((*input_cloud)(width-1,i).intensity-(*input_cloud)(width-2,i).intensity), 1); } - conv->setBoundaryOptions(Convolution::BOUNDARY_OPTION_CLAMP); - conv->filter (*output_cloud); + conv.setBoundaryOptions(Convolution::BOUNDARY_OPTION_CLAMP); + conv.filter (*output_cloud); for (int i = 1; i < height - 1; i++) for (int j = 1; j < width - 1; j++) EXPECT_NEAR ((*output_cloud)(j,i).intensity, ((*input_cloud)(j+1,i).intensity-(*input_cloud)(j-1,i).intensity), 1); @@ -110,8 +110,8 @@ TEST (Convolution, borderOptions) // EXPECT_NEAR ((*output_cloud)(width-1,i).intensity, ((*input_cloud)(width-1,i).intensity-(*input_cloud)(width-2,i).intensity), 1); } - conv->setBoundaryOptions(Convolution::BOUNDARY_OPTION_ZERO_PADDING); - conv->filter (*output_cloud); + conv.setBoundaryOptions(Convolution::BOUNDARY_OPTION_ZERO_PADDING); + conv.filter (*output_cloud); for (int i = 1; i < height - 1; i++) for (int j = 1; j < width - 1; j++) EXPECT_NEAR ((*output_cloud)(j,i).intensity, ((*input_cloud)(j+1,i).intensity-(*input_cloud)(j-1,i).intensity), 1); @@ -125,8 +125,8 @@ TEST (Convolution, borderOptions) TEST (Convolution, gaussianSmooth) { - kernel *k = new kernel (); - Convolution *conv = new Convolution (); + kernel k; + Convolution conv; /*dummy clouds*/ pcl::PointCloud::Ptr input_cloud (new pcl::PointCloud); @@ -140,20 +140,19 @@ TEST (Convolution, gaussianSmooth) int height = input_cloud->height; int width = input_cloud->width; - k->setKernelType(kernel::GAUSSIAN); - k->setKernelSize(3); - k->setKernelSigma(1.0f); - k->fetchKernel (*kernel_cloud); + k.setKernelType(kernel::GAUSSIAN); + k.setKernelSize(3); + k.setKernelSigma(1.0f); + k.fetchKernel (*kernel_cloud); - conv->setKernel(*kernel_cloud); - conv->setInputCloud(input_cloud); + conv.setKernel(*kernel_cloud); + conv.setInputCloud(input_cloud); - conv->setBoundaryOptions(Convolution::BOUNDARY_OPTION_MIRROR); - conv->filter (*output_cloud); + conv.setBoundaryOptions(Convolution::BOUNDARY_OPTION_MIRROR); + conv.filter (*output_cloud); for (int i = 1; i < height - 1; i++) for (int j = 1; j < width - 1; j++) EXPECT_NEAR ((*output_cloud)(j,i).intensity, (*gt_output_cloud)(j,i).intensity, 1); - } TEST(Edge, sobel) @@ -272,11 +271,11 @@ TEST(Morphology, erosion) pcl::io::loadPCDFile(lena, *input_cloud); pcl::io::loadPCDFile(erosion_ref, *gt_output_cloud); - Morphology *morph = new Morphology(); - morph->setInputCloud(input_cloud); - morph->structuringElementRectangle(*structuring_element_cloud, 3, 3); - morph->setStructuringElement(structuring_element_cloud); - morph->erosionGray(*output_cloud); + Morphology morph; + morph.setInputCloud(input_cloud); + morph.structuringElementRectangle(*structuring_element_cloud, 3, 3); + morph.setStructuringElement(structuring_element_cloud); + morph.erosionGray(*output_cloud); int height = input_cloud->height; int width = input_cloud->width; @@ -287,12 +286,11 @@ TEST(Morphology, erosion) pcl::io::loadPCDFile(erosion_binary_ref, *gt_output_cloud); threshold(input_cloud, 100); - morph->setInputCloud(input_cloud); - morph->erosionBinary(*output_cloud); + morph.setInputCloud(input_cloud); + morph.erosionBinary(*output_cloud); for (int i = 1; i < height - 1; i++) for (int j = 1; j < width - 1; j++) EXPECT_NEAR ((*output_cloud)(j,i).intensity, (*gt_output_cloud)(j,i).intensity/255.0, 1); - } TEST(Morphology, dilation) @@ -306,11 +304,11 @@ TEST(Morphology, dilation) pcl::io::loadPCDFile(lena, *input_cloud); pcl::io::loadPCDFile(dilation_ref, *gt_output_cloud); - Morphology *morph = new Morphology(); - morph->setInputCloud(input_cloud); - morph->structuringElementRectangle(*structuring_element_cloud, 3, 3); - morph->setStructuringElement(structuring_element_cloud); - morph->dilationGray(*output_cloud); + Morphology morph; + morph.setInputCloud(input_cloud); + morph.structuringElementRectangle(*structuring_element_cloud, 3, 3); + morph.setStructuringElement(structuring_element_cloud); + morph.dilationGray(*output_cloud); int height = input_cloud->height; int width = input_cloud->width; @@ -321,8 +319,8 @@ TEST(Morphology, dilation) pcl::io::loadPCDFile(dilation_binary_ref, *gt_output_cloud); threshold(input_cloud, 100); - morph->setInputCloud(input_cloud); - morph->dilationBinary(*output_cloud); + morph.setInputCloud(input_cloud); + morph.dilationBinary(*output_cloud); for (int i = 1; i < height - 1; i++) for (int j = 1; j < width - 1; j++) EXPECT_NEAR ((*output_cloud)(j,i).intensity, (*gt_output_cloud)(j,i).intensity/255.0, 1); @@ -339,11 +337,11 @@ TEST(Morphology, opening) pcl::io::loadPCDFile(lena, *input_cloud); pcl::io::loadPCDFile(opening_ref, *gt_output_cloud); - Morphology *morph = new Morphology(); - morph->setInputCloud(input_cloud); - morph->structuringElementRectangle(*structuring_element_cloud, 3, 3); - morph->setStructuringElement(structuring_element_cloud); - morph->openingGray(*output_cloud); + Morphology morph; + morph.setInputCloud(input_cloud); + morph.structuringElementRectangle(*structuring_element_cloud, 3, 3); + morph.setStructuringElement(structuring_element_cloud); + morph.openingGray(*output_cloud); int height = input_cloud->height; int width = input_cloud->width; @@ -354,8 +352,8 @@ TEST(Morphology, opening) pcl::io::loadPCDFile(opening_binary_ref, *gt_output_cloud); threshold(input_cloud, 100); - morph->setInputCloud(input_cloud); - morph->openingBinary(*output_cloud); + morph.setInputCloud(input_cloud); + morph.openingBinary(*output_cloud); for (int i = 1; i < height - 1; i++) for (int j = 1; j < width - 1; j++) EXPECT_NEAR ((*output_cloud)(j,i).intensity, (*gt_output_cloud)(j,i).intensity/255.0, 1); @@ -372,11 +370,11 @@ TEST(Morphology, closing) pcl::io::loadPCDFile(lena, *input_cloud); pcl::io::loadPCDFile(closing_ref, *gt_output_cloud); - Morphology *morph = new Morphology(); - morph->setInputCloud(input_cloud); - morph->structuringElementRectangle(*structuring_element_cloud, 3, 3); - morph->setStructuringElement(structuring_element_cloud); - morph->closingGray(*output_cloud); + Morphology morph; + morph.setInputCloud(input_cloud); + morph.structuringElementRectangle(*structuring_element_cloud, 3, 3); + morph.setStructuringElement(structuring_element_cloud); + morph.closingGray(*output_cloud); int height = input_cloud->height; int width = input_cloud->width; @@ -387,8 +385,8 @@ TEST(Morphology, closing) pcl::io::loadPCDFile(closing_binary_ref, *gt_output_cloud); threshold(input_cloud, 100); - morph->setInputCloud(input_cloud); - morph->closingBinary(*output_cloud); + morph.setInputCloud(input_cloud); + morph.closingBinary(*output_cloud); for (int i = 1; i < height - 1; i++) for (int j = 1; j < width - 1; j++) EXPECT_NEAR ((*output_cloud)(j,i).intensity, (*gt_output_cloud)(j,i).intensity/255.0, 1); diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index ad5492cf..3da1ebea 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -3,6 +3,7 @@ set(SUBSYS_DESC "Point cloud library global unit tests") set(DEFAULT OFF) set(build TRUE) +set(REASON "Disabled by default") PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ${DEFAULT} "${REASON}") PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS} OPT_DEPS ${OPT_DEPS}) @@ -22,7 +23,7 @@ if(MSVC) 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() +endif() set_target_properties(tests PROPERTIES FOLDER "Tests") diff --git a/test/common/test_intensity.cpp b/test/common/test_intensity.cpp index 74026aec..9ad268f7 100644 --- a/test/common/test_intensity.cpp +++ b/test/common/test_intensity.cpp @@ -40,7 +40,6 @@ #include #include #include -#include #include using namespace pcl; diff --git a/test/common/test_io.cpp b/test/common/test_io.cpp index e42c78f8..86ce0d95 100644 --- a/test/common/test_io.cpp +++ b/test/common/test_io.cpp @@ -130,6 +130,11 @@ TEST (PCL, copyPointCloud) } /////////////////////////////////////////////////////////////////////////////////////////// +// Ignore unknown pragma warning on MSVC (4996) +#ifdef _MSC_VER +#pragma warning(push) +#pragma warning(disable: 4068) +#endif #pragma GCC diagnostic ignored "-Wdeprecated-declarations" #pragma GCC diagnostic push TEST (PCL, concatenatePointCloud) @@ -317,6 +322,9 @@ TEST (PCL, concatenatePointCloud) } } #pragma GCC diagnostic pop +#ifdef _MSC_VER +#pragma warning(pop) +#endif /////////////////////////////////////////////////////////////////////////////////////////// TEST (PCL, concatenatePointCloud2) diff --git a/test/common/test_operators.cpp b/test/common/test_operators.cpp index fdd43f79..6208e505 100644 --- a/test/common/test_operators.cpp +++ b/test/common/test_operators.cpp @@ -38,7 +38,6 @@ #include #include #include -#include using namespace pcl; using namespace pcl::test; diff --git a/test/common/test_transforms.cpp b/test/common/test_transforms.cpp index b58e8fb9..94bf64ed 100644 --- a/test/common/test_transforms.cpp +++ b/test/common/test_transforms.cpp @@ -39,6 +39,7 @@ #include +#include #include #include #include diff --git a/test/common/test_type_traits.cpp b/test/common/test_type_traits.cpp index a2bace35..637af732 100644 --- a/test/common/test_type_traits.cpp +++ b/test/common/test_type_traits.cpp @@ -36,8 +36,10 @@ */ -#include +#include // for pcl::has_custom_allocator, PCL_MAKE_ALIGNED_OPERATOR_NEW +#include #include +#include // for pcl::isXYFinite, pcl::isXYZFinite, pcl::isNormalFinite #include @@ -46,7 +48,15 @@ TEST (TypeTraits, HasCustomAllocatorTrait) struct Foo { public: + // Manually ignore warnings here because of an issue in Eigen which + // results in a local typedef being unused inside the new and delete + // operators added by Eigen for C++14 or lower standards + /** \todo Remove for C++17 (or future standards) + */ + #pragma clang diagnostic push + #pragma clang diagnostic ignored "-Wunused-local-typedef" PCL_MAKE_ALIGNED_OPERATOR_NEW + #pragma clang diagnostic pop }; struct Bar diff --git a/test/features/test_normal_estimation.cpp b/test/features/test_normal_estimation.cpp index ba1ea939..4f4cef3f 100644 --- a/test/features/test_normal_estimation.cpp +++ b/test/features/test_normal_estimation.cpp @@ -308,7 +308,7 @@ TEST (PCL, IntegralImageNormalEstimationIndexingIssue) pcl::IndicesPtr indicesptr (new pcl::Indices ()); indicesptr->resize(cloudptr->size() / 2); - for (int i = 0; i < cloudptr->size() / 2; ++i) + for (std::size_t i = 0; i < cloudptr->size() / 2; ++i) { (*indicesptr)[i] = i + cloudptr->size() / 2; } @@ -328,7 +328,7 @@ TEST (PCL, IntegralImageNormalEstimationIndexingIssue) std::vector normalsVec; normalsVec.resize(normals->size()); - for( int i = 0; i < normals->size(); ++i ) + for(std::size_t i = 0; i < normals->size(); ++i ) { normalsVec[i].x = normals->points[i].normal_x; normalsVec[i].y = normals->points[i].normal_y; diff --git a/test/filters/test_clipper.cpp b/test/filters/test_clipper.cpp index 98485104..dd518002 100644 --- a/test/filters/test_clipper.cpp +++ b/test/filters/test_clipper.cpp @@ -39,6 +39,8 @@ */ #include +#include // for pcl::make_shared +#include // for pcl::Indices #include #include #include @@ -71,7 +73,7 @@ TEST (BoxClipper3D, Filters) input->push_back (PointXYZ (-0.9f, -0.9f, -0.9f)); ExtractIndices extract_indices; - std::vector indices; + pcl::Indices indices; BoxClipper3D boxClipper3D (Affine3f::Identity ()); boxClipper3D.clipPointCloud3D (*input, indices); @@ -79,7 +81,7 @@ TEST (BoxClipper3D, Filters) PointCloud cloud_out; extract_indices.setInputCloud (input); - extract_indices.setIndices (boost::make_shared > (indices)); + extract_indices.setIndices (pcl::make_shared (indices)); extract_indices.filter (cloud_out); EXPECT_EQ (int (indices.size ()), 9); diff --git a/test/filters/test_filters.cpp b/test/filters/test_filters.cpp index d2bb3a62..fe429bba 100644 --- a/test/filters/test_filters.cpp +++ b/test/filters/test_filters.cpp @@ -266,7 +266,7 @@ TEST (ExtractIndices, Filters) ps.setFilterLimits (0.99, 2.01); for (int i = 0; i < 2; i++) { - ps.setFilterLimitsNegative ((bool)i); + ps.setNegative ((bool)i); ps.filter (scf); std::cerr << scf.points.size () << std::endl; for (std::size_t j = 0; j < scf.points.size (); ++j) @@ -306,7 +306,7 @@ TEST (PassThrough, Filters) EXPECT_NEAR (output.points[41].y, 0.039749, 1e-5); EXPECT_NEAR (output.points[41].z, 0.052133, 1e-5); - pt.setFilterLimitsNegative (true); + pt.setNegative (true); pt.filter (output); EXPECT_EQ (int (output.points.size ()), 355); @@ -350,7 +350,7 @@ TEST (PassThrough, Filters) EXPECT_NEAR (output.points[41].y, 0.039749, 1e-5); EXPECT_NEAR (output.points[41].z, 0.052133, 1e-5); - pt_.setFilterLimitsNegative (true); + pt_.setNegative (true); pt_.filter (output); EXPECT_EQ (int (output.points.size ()), 355); @@ -380,7 +380,7 @@ TEST (PassThrough, Filters) EXPECT_NEAR (output.points[output.points.size () - 1].x, cloud->points[cloud->points.size () - 1].x, 1e-5); pt.setFilterFieldName ("z"); - pt.setFilterLimitsNegative (false); + pt.setNegative (false); pt.setKeepOrganized (true); pt.filter (output); @@ -396,7 +396,7 @@ TEST (PassThrough, Filters) EXPECT_TRUE (std::isnan (output.points[41].y)); EXPECT_TRUE (std::isnan (output.points[41].z)); - pt.setFilterLimitsNegative (true); + pt.setNegative (true); pt.filter (output); EXPECT_EQ (output.points.size (), cloud->points.size ()); diff --git a/test/filters/test_sampling.cpp b/test/filters/test_sampling.cpp index 04dacaad..76cb2cfb 100644 --- a/test/filters/test_sampling.cpp +++ b/test/filters/test_sampling.cpp @@ -105,31 +105,44 @@ TEST (CovarianceSampling, Filters) ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// TEST (NormalSpaceSampling, Filters) { + // pcl::Normal is not precompiled by default so we use PointNormal + auto cloud = pcl::make_shared> (); + // generate 16 points (8 unique) with unit norm + cloud->reserve (16); + // ensure that the normals have unit norm + const auto value = std::sqrt(1/3.f); + for (int unique = 0; unique < 8; ++unique) { + const auto i = ((unique % 2) < 1) ? -1 : 1; // points alternate sign + const auto j = ((unique % 4) < 2) ? -1 : 1; // 2 points negative, 2 positive + const auto k = ((unique % 8) < 4) ? -1 : 1; // first 4 points negative, rest positive + for (int duplicate = 0; duplicate < 2; ++duplicate) { + cloud->emplace_back (0.f, 0.f, 0.f, i * value, j * value, k * value); + } + } + NormalSpaceSampling normal_space_sampling; - normal_space_sampling.setInputCloud (cloud_walls_normals); - normal_space_sampling.setNormals (cloud_walls_normals); - normal_space_sampling.setBins (4, 4, 4); + normal_space_sampling.setInputCloud (cloud); + normal_space_sampling.setNormals (cloud); + normal_space_sampling.setBins (2, 2, 2); normal_space_sampling.setSeed (0); - normal_space_sampling.setSample (static_cast (cloud_walls_normals->size ()) / 4); + normal_space_sampling.setSample (8); - IndicesPtr walls_indices (new std::vector ()); + IndicesPtr walls_indices = pcl::make_shared (); normal_space_sampling.filter (*walls_indices); - CovarianceSampling covariance_sampling; - covariance_sampling.setInputCloud (cloud_walls_normals); - covariance_sampling.setNormals (cloud_walls_normals); - covariance_sampling.setIndices (walls_indices); - covariance_sampling.setNumberOfSamples (0); - double cond_num_walls_sampled = covariance_sampling.computeConditionNumber (); - + // The orientation space of the normals is divided into 2x2x2 buckets + // points are samples arbitrarily from each bucket in succession until the + // requested number of samples is met. This means we expect to see only one index + // for every two elements in the original array e.g. 0, 3, 4, 6, etc... + // if 0 is sampled, index 1 can no longer be there and so forth + std::array, 8> buckets; + for (const auto index : *walls_indices) + buckets[index/2].insert (index); - EXPECT_NEAR (33.04893, cond_num_walls_sampled, 1e-1); - EXPECT_EQ (1412, (*walls_indices)[0]); - EXPECT_EQ (1943, (*walls_indices)[walls_indices->size () / 4]); - EXPECT_EQ (2771, (*walls_indices)[walls_indices->size () / 2]); - EXPECT_EQ (3215, (*walls_indices)[walls_indices->size () * 3 / 4]); - EXPECT_EQ (2503, (*walls_indices)[walls_indices->size () - 1]); + EXPECT_EQ (8u, walls_indices->size ()); + for (const auto& bucket : buckets) + EXPECT_EQ (1u, bucket.size ()); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/test/geometry/test_mesh_circulators.cpp b/test/geometry/test_mesh_circulators.cpp index 5c4ba540..5a615b38 100644 --- a/test/geometry/test_mesh_circulators.cpp +++ b/test/geometry/test_mesh_circulators.cpp @@ -41,6 +41,7 @@ #include #include +#include #include #include "test_mesh_common_functions.h" diff --git a/test/geometry/test_mesh_conversion.cpp b/test/geometry/test_mesh_conversion.cpp index 1e95033a..93ae1d5d 100644 --- a/test/geometry/test_mesh_conversion.cpp +++ b/test/geometry/test_mesh_conversion.cpp @@ -44,6 +44,7 @@ #include #include #include +#include #include #include #include diff --git a/test/io/test_io.cpp b/test/io/test_io.cpp index dca0a776..d1cb5c65 100644 --- a/test/io/test_io.cpp +++ b/test/io/test_io.cpp @@ -39,7 +39,7 @@ #include #include -#include +#include #include #include #include diff --git a/test/io/test_octree_compression.cpp b/test/io/test_octree_compression.cpp index fcbc8782..f2c90aab 100644 --- a/test/io/test_octree_compression.cpp +++ b/test/io/test_octree_compression.cpp @@ -63,8 +63,8 @@ TEST (PCL, OctreeDeCompressionRandomPointXYZRGBA) for (int compression_profile = pcl::io::LOW_RES_ONLINE_COMPRESSION_WITHOUT_COLOR; compression_profile != pcl::io::COMPRESSION_PROFILE_COUNT; ++compression_profile) { // instantiate point cloud compression encoder/decoder - pcl::io::OctreePointCloudCompression* pointcloud_encoder = new pcl::io::OctreePointCloudCompression((pcl::io::compression_Profiles_e) compression_profile, false); - pcl::io::OctreePointCloudCompression* pointcloud_decoder = new pcl::io::OctreePointCloudCompression(); + pcl::io::OctreePointCloudCompression pointcloud_encoder((pcl::io::compression_Profiles_e) compression_profile, false); + pcl::io::OctreePointCloudCompression pointcloud_decoder; pcl::PointCloud::Ptr cloud_out(new pcl::PointCloud()); // iterate over runs for (int test_idx = 0; test_idx < NUMBER_OF_TEST_RUNS; test_idx++, total_runs++) @@ -98,8 +98,8 @@ TEST (PCL, OctreeDeCompressionRandomPointXYZRGBA) // std::cout << "Run: " << total_runs << " compression profile:" << compression_profile << " point_count: " << point_count; std::stringstream compressed_data; - pointcloud_encoder->encodePointCloud(cloud, compressed_data); - pointcloud_decoder->decodePointCloud(compressed_data, cloud_out); + pointcloud_encoder.encodePointCloud(cloud, compressed_data); + pointcloud_decoder.decodePointCloud(compressed_data, cloud_out); EXPECT_GT((int)cloud_out->width, 0) << "decoded PointCloud width <= 0"; EXPECT_GT((int)cloud_out->height, 0) << " decoded PointCloud height <= 0 "; } @@ -120,8 +120,8 @@ TEST (PCL, OctreeDeCompressionRandomPointXYZ) compression_profile != pcl::io::COMPRESSION_PROFILE_COUNT; ++compression_profile) { // instantiate point cloud compression encoder/decoder - pcl::io::OctreePointCloudCompression* pointcloud_encoder = new pcl::io::OctreePointCloudCompression((pcl::io::compression_Profiles_e) compression_profile, false); - pcl::io::OctreePointCloudCompression* pointcloud_decoder = new pcl::io::OctreePointCloudCompression(); + pcl::io::OctreePointCloudCompression pointcloud_encoder((pcl::io::compression_Profiles_e) compression_profile, false); + pcl::io::OctreePointCloudCompression pointcloud_decoder; pcl::PointCloud::Ptr cloud_out(new pcl::PointCloud()); // loop over runs for (int test_idx = 0; test_idx < NUMBER_OF_TEST_RUNS; test_idx++, total_runs++) @@ -143,8 +143,8 @@ TEST (PCL, OctreeDeCompressionRandomPointXYZ) std::stringstream compressed_data; try { // decodePointCloud() throws exceptions on errors - pointcloud_encoder->encodePointCloud(cloud, compressed_data); - pointcloud_decoder->decodePointCloud(compressed_data, cloud_out); + pointcloud_encoder.encodePointCloud(cloud, compressed_data); + pointcloud_decoder.decodePointCloud(compressed_data, cloud_out); EXPECT_GT((int)cloud_out->width, 0) << "decoded PointCloud width <= 0"; EXPECT_GT((int)cloud_out->height, 0) << " decoded PointCloud height <= 0 "; } diff --git a/test/io/test_ply_mesh_io.cpp b/test/io/test_ply_mesh_io.cpp index 360976d6..a6dd6f1c 100644 --- a/test/io/test_ply_mesh_io.cpp +++ b/test/io/test_ply_mesh_io.cpp @@ -40,7 +40,7 @@ #include #include -#include +#include #include #include #include diff --git a/test/kdtree/test_kdtree.cpp b/test/kdtree/test_kdtree.cpp index c2f09d55..0de67ac1 100644 --- a/test/kdtree/test_kdtree.cpp +++ b/test/kdtree/test_kdtree.cpp @@ -36,18 +36,22 @@ * */ -#include -#include // For debug -#include -#include +#include + #include #include #include +#include // for pcl::isFinite +#include #include -#include +#include + #include #include +#include // For debug +#include + using namespace std; using namespace pcl; diff --git a/test/registration/test_registration.cpp b/test/registration/test_registration.cpp index a8cd9eb2..f05cf495 100644 --- a/test/registration/test_registration.cpp +++ b/test/registration/test_registration.cpp @@ -154,6 +154,48 @@ TEST (PCL, findFeatureCorrespondences) */ } +///////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// This test if the ICP algorithm can successfully find the transformation of a cloud that has been +// moved 0.7 in x direction. +// It indirectly test the KDTree doesn't get an empty input cloud, see #3624 +// It is more or less a copy of https://github.com/PointCloudLibrary/pcl/blob/cc7fe363c6463a0abc617b1e17e94ab4bd4169ef/doc/tutorials/content/sources/iterative_closest_point/iterative_closest_point.cpp +TEST(PCL, ICP_translated) +{ + pcl::PointCloud::Ptr cloud_in(new pcl::PointCloud(5,1)); + pcl::PointCloud::Ptr cloud_out(new pcl::PointCloud); + + // Fill in the CloudIn data + for (auto& point : *cloud_in) + { + point.x = 1024 * rand() / (RAND_MAX + 1.0f); + point.y = 1024 * rand() / (RAND_MAX + 1.0f); + point.z = 1024 * rand() / (RAND_MAX + 1.0f); + } + + *cloud_out = *cloud_in; + + for (auto& point : *cloud_out) + point.x += 0.7f; + + pcl::IterativeClosestPoint icp; + icp.setInputSource(cloud_in); + icp.setInputTarget(cloud_out); + + pcl::PointCloud Final; + icp.align(Final); + + // Check that we have sucessfully converged + ASSERT_EQ(icp.hasConverged(), true); + + // Test that the fitness score is below acceptable threshold + EXPECT_LT(icp.getFitnessScore(), 1e-6); + + // Ensure that the translation found is within acceptable threshold. + EXPECT_NEAR(icp.getFinalTransformation()(0, 3), 0.7, 2e-3); + EXPECT_NEAR(icp.getFinalTransformation()(1, 3), 0.0, 2e-3); + EXPECT_NEAR(icp.getFinalTransformation()(2, 3), 0.0, 2e-3); +} + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// TEST (PCL, IterativeClosestPoint) { @@ -169,26 +211,26 @@ TEST (PCL, IterativeClosestPoint) reg.align (cloud_reg); EXPECT_EQ (int (cloud_reg.points.size ()), int (cloud_source.points.size ())); - //Eigen::Matrix4f transformation = reg.getFinalTransformation (); -// EXPECT_NEAR (transformation (0, 0), 0.8806, 1e-3); -// EXPECT_NEAR (transformation (0, 1), 0.036481287330389023, 1e-2); -// EXPECT_NEAR (transformation (0, 2), -0.4724, 1e-3); -// EXPECT_NEAR (transformation (0, 3), 0.03453, 1e-3); -// -// EXPECT_NEAR (transformation (1, 0), -0.02354, 1e-3); -// EXPECT_NEAR (transformation (1, 1), 0.9992, 1e-3); -// EXPECT_NEAR (transformation (1, 2), 0.03326, 1e-3); -// EXPECT_NEAR (transformation (1, 3), -0.001519, 1e-3); -// -// EXPECT_NEAR (transformation (2, 0), 0.4732, 1e-3); -// EXPECT_NEAR (transformation (2, 1), -0.01817, 1e-3); -// EXPECT_NEAR (transformation (2, 2), 0.8808, 1e-3); -// EXPECT_NEAR (transformation (2, 3), 0.04116, 1e-3); -// -// EXPECT_EQ (transformation (3, 0), 0); -// EXPECT_EQ (transformation (3, 1), 0); -// EXPECT_EQ (transformation (3, 2), 0); -// EXPECT_EQ (transformation (3, 3), 1); + Eigen::Matrix4f transformation = reg.getFinalTransformation (); + EXPECT_NEAR (transformation (0, 0), 0.8806, 1e-3); + EXPECT_NEAR (transformation (0, 1), 0.036481287330389023, 1e-2); + EXPECT_NEAR (transformation (0, 2), -0.4724, 1e-3); + EXPECT_NEAR (transformation (0, 3), 0.03453, 1e-3); + + EXPECT_NEAR (transformation (1, 0), -0.02354, 1e-3); + EXPECT_NEAR (transformation (1, 1), 0.9992, 1e-3); + EXPECT_NEAR (transformation (1, 2), 0.03326, 1e-3); + EXPECT_NEAR (transformation (1, 3), -0.001519, 1e-3); + + EXPECT_NEAR (transformation (2, 0), 0.4732, 1e-3); + EXPECT_NEAR (transformation (2, 1), -0.01817, 1e-3); + EXPECT_NEAR (transformation (2, 2), 0.8808, 1e-3); + EXPECT_NEAR (transformation (2, 3), 0.04116, 1e-3); + + EXPECT_EQ (transformation (3, 0), 0); + EXPECT_EQ (transformation (3, 1), 0); + EXPECT_EQ (transformation (3, 2), 0); + EXPECT_EQ (transformation (3, 3), 1); } TEST (PCL, IterativeClosestPointWithNormals) diff --git a/test/search/test_auto_search.cpp b/test/search/test_auto_search.cpp index 66087a74..97ead781 100644 --- a/test/search/test_auto_search.cpp +++ b/test/search/test_auto_search.cpp @@ -106,7 +106,7 @@ TEST (PCL, Octree_Pointcloud_Nearest_K_Neighbour_Search) std::priority_queue > > pointCandidates; // create octree - pcl::search::Search* octree = new pcl::search::AutotunedSearch(pcl::search::OCTREE); + pcl::search::Search octree(pcl::search::OCTREE); std::vector k_indices; std::vector k_sqr_distances; @@ -166,8 +166,8 @@ TEST (PCL, Octree_Pointcloud_Nearest_K_Neighbour_Search) } // octree nearest neighbor search - octree->setInputCloud (cloudIn); - octree->nearestKSearch (searchPoint, (int)K, k_indices, k_sqr_distances); + octree.setInputCloud (cloudIn); + octree.nearestKSearch (searchPoint, (int)K, k_indices, k_sqr_distances); ASSERT_EQ ( k_indices.size() , k_indices_bruteforce.size() ); @@ -260,8 +260,8 @@ TEST (PCL, Octree_Pointcloud_Approx_Nearest_Neighbour_Search) TEST (PCL, KdTreeWrapper_nearestKSearch) { - pcl::search::Search* kdtree = new pcl::search::AutotunedSearch(pcl::search::KDTREE); - kdtree->setInputCloud (cloud.makeShared ()); + pcl::search::Search kdtree(pcl::search::KDTREE); + kdtree.setInputCloud (cloud.makeShared ()); PointXYZ test_point (0.01f, 0.01f, 0.01f); unsigned int no_of_neighbors = 20; multimap sorted_brute_force_result; @@ -283,7 +283,7 @@ TEST (PCL, KdTreeWrapper_nearestKSearch) std::vector k_distances; k_distances.resize (no_of_neighbors); - kdtree->nearestKSearch (test_point, no_of_neighbors, k_indices, k_distances); + kdtree.nearestKSearch (test_point, no_of_neighbors, k_indices, k_distances); EXPECT_EQ (k_indices.size (), no_of_neighbors); @@ -301,20 +301,19 @@ TEST (PCL, KdTreeWrapper_nearestKSearch) ScopeTime scopeTime ("FLANN nearestKSearch"); { - pcl::search::Search* kdtree = new pcl::search::AutotunedSearch(pcl::search::KDTREE); - //kdtree->initSearchDS (); - kdtree->setInputCloud (cloud_big.makeShared ()); + pcl::search::AutotunedSearch kdtree (pcl::search::KDTREE); + //kdtree.initSearchDS (); + kdtree.setInputCloud (cloud_big.makeShared ()); for (std::size_t i = 0; i < cloud_big.points.size (); ++i) - kdtree->nearestKSearch (cloud_big.points[i], no_of_neighbors, k_indices, k_distances); + kdtree.nearestKSearch (cloud_big.points[i], no_of_neighbors, k_indices, k_distances); } - } /* Function to auto evaluate the best search structure for the given dataset */ TEST (PCL, AutoTunedSearch_Evaluate) { - pcl::search::Search* search = new pcl::search::AutotunedSearch(pcl::search::AUTO_TUNED); + pcl::search::AutotunedSearch search (pcl::search::AUTO_TUNED); pcl::PCDReader pcd; pcl::PointCloud::Ptr cloudIn (new pcl::PointCloud); @@ -325,8 +324,8 @@ TEST (PCL, AutoTunedSearch_Evaluate) return; } - search->evaluateSearchMethods (cloudIn, pcl::search::NEAREST_K_SEARCH); - search->evaluateSearchMethods (cloudIn, pcl::search::NEAREST_RADIUS_SEARCH); + search.evaluateSearchMethods (cloudIn, pcl::search::NEAREST_K_SEARCH); + search.evaluateSearchMethods (cloudIn, pcl::search::NEAREST_RADIUS_SEARCH); } @@ -338,8 +337,8 @@ main(int argc, char** argv) init (); // Testing using explicit instantiation of inherited class - pcl::search::Search* kdtree = new pcl::search::AutotunedSearch(pcl::search::KDTREE); - kdtree->setInputCloud (cloud.makeShared ()); + pcl::search::AutotunedSearch kdtree(pcl::search::KDTREE); + kdtree.setInputCloud (cloud.makeShared ()); return (RUN_ALL_TESTS ()); }; diff --git a/test/search/test_flann_search.cpp b/test/search/test_flann_search.cpp index df666e67..c2833097 100644 --- a/test/search/test_flann_search.cpp +++ b/test/search/test_flann_search.cpp @@ -76,8 +76,8 @@ init () /* Test for FlannSearch nearestKSearch */ TEST (PCL, FlannSearch_nearestKSearch) { - pcl::search::Search* FlannSearch = new pcl::search::FlannSearch (new search::FlannSearch::KdTreeIndexCreator); - FlannSearch->setInputCloud (cloud.makeShared ()); + pcl::search::FlannSearch FlannSearch (new search::FlannSearch::KdTreeIndexCreator); + FlannSearch.setInputCloud (cloud.makeShared ()); PointXYZ test_point (0.01f, 0.01f, 0.01f); unsigned int no_of_neighbors = 20; multimap sorted_brute_force_result; @@ -100,7 +100,7 @@ TEST (PCL, FlannSearch_nearestKSearch) std::vector k_distances; k_distances.resize (no_of_neighbors); - FlannSearch->nearestKSearch (test_point, no_of_neighbors, k_indices, k_distances); + FlannSearch.nearestKSearch (test_point, no_of_neighbors, k_indices, k_distances); //if (k_indices.size () != no_of_neighbors) std::cerr << "Found "<* FlannSearch = new pcl::search::FlannSearch( new search::FlannSearch::KdTreeIndexCreator); - //FlannSearch->initSearchDS (); - FlannSearch->setInputCloud (cloud_big.makeShared ()); + pcl::search::FlannSearch FlannSearch( new search::FlannSearch::KdTreeIndexCreator); + //FlannSearch.initSearchDS (); + FlannSearch.setInputCloud (cloud_big.makeShared ()); for (const auto &point : cloud_big.points) - FlannSearch->nearestKSearch (point, no_of_neighbors, k_indices, k_distances); + FlannSearch.nearestKSearch (point, no_of_neighbors, k_indices, k_distances); } } @@ -133,9 +133,9 @@ TEST (PCL, FlannSearch_differentPointT) unsigned int no_of_neighbors = 20; - pcl::search::Search* FlannSearch = new pcl::search::FlannSearch (new search::FlannSearch::KdTreeIndexCreator); - //FlannSearch->initSearchDS (); - FlannSearch->setInputCloud (cloud_big.makeShared ()); + pcl::search::FlannSearch FlannSearch (new search::FlannSearch::KdTreeIndexCreator); + //FlannSearch.initSearchDS (); + FlannSearch.setInputCloud (cloud_big.makeShared ()); PointCloud cloud_rgb; @@ -145,7 +145,7 @@ TEST (PCL, FlannSearch_differentPointT) std::vector< std::vector< float > > dists; std::vector< std::vector< int > > indices; - FlannSearch->nearestKSearchT (cloud_rgb, std::vector (),no_of_neighbors,indices,dists); + FlannSearch.nearestKSearchT (cloud_rgb, std::vector (),no_of_neighbors,indices,dists); std::vector k_indices; k_indices.resize (no_of_neighbors); @@ -159,8 +159,8 @@ TEST (PCL, FlannSearch_differentPointT) for (std::size_t i = 0; i < cloud_rgb.points.size (); ++i) { - //FlannSearch->nearestKSearchT (cloud_rgb.points[i], no_of_neighbors, k_indices_t, k_distances_t); - FlannSearch->nearestKSearch (cloud_big.points[i], no_of_neighbors, k_indices, k_distances); + //FlannSearch.nearestKSearchT (cloud_rgb.points[i], no_of_neighbors, k_indices_t, k_distances_t); + FlannSearch.nearestKSearch (cloud_big.points[i], no_of_neighbors, k_indices, k_distances); EXPECT_EQ (k_indices.size (), indices[i].size ()); EXPECT_EQ (k_distances.size (), dists[i].size ()); for (std::size_t j = 0; j< no_of_neighbors; j++) @@ -180,13 +180,13 @@ TEST (PCL, FlannSearch_multipointKnnSearch) unsigned int no_of_neighbors = 20; - pcl::search::Search* FlannSearch = new pcl::search::FlannSearch (new search::FlannSearch::KdTreeIndexCreator); - //FlannSearch->initSearchDS (); - FlannSearch->setInputCloud (cloud_big.makeShared ()); + pcl::search::FlannSearch FlannSearch (new search::FlannSearch::KdTreeIndexCreator); + //FlannSearch.initSearchDS (); + FlannSearch.setInputCloud (cloud_big.makeShared ()); std::vector< std::vector< float > > dists; std::vector< std::vector< int > > indices; - FlannSearch->nearestKSearch (cloud_big, std::vector(),no_of_neighbors,indices,dists); + FlannSearch.nearestKSearch (cloud_big, std::vector(),no_of_neighbors,indices,dists); std::vector k_indices; k_indices.resize (no_of_neighbors); @@ -195,7 +195,7 @@ TEST (PCL, FlannSearch_multipointKnnSearch) for (std::size_t i = 0; i < cloud_big.points.size (); ++i) { - FlannSearch->nearestKSearch (cloud_big.points[i], no_of_neighbors, k_indices, k_distances); + FlannSearch.nearestKSearch (cloud_big.points[i], no_of_neighbors, k_indices, k_distances); EXPECT_EQ (k_indices.size (), indices[i].size ()); EXPECT_EQ (k_distances.size (), dists[i].size ()); for (std::size_t j = 0; j< no_of_neighbors; j++ ) @@ -213,9 +213,9 @@ TEST (PCL, FlannSearch_knnByIndex) unsigned int no_of_neighbors = 3; - pcl::search::Search* flann_search = new pcl::search::FlannSearch (new search::FlannSearch::KdTreeIndexCreator); + pcl::search::FlannSearch flann_search (new search::FlannSearch::KdTreeIndexCreator); //FlannSearch->initSearchDS (); - flann_search->setInputCloud (cloud_big.makeShared ()); + flann_search.setInputCloud (cloud_big.makeShared ()); std::vector< std::vector< float > > dists; std::vector< std::vector< int > > indices; @@ -224,7 +224,7 @@ TEST (PCL, FlannSearch_knnByIndex) { query_indices.push_back (int (i)); } - flann_search->nearestKSearch (cloud_big, query_indices,no_of_neighbors,indices,dists); + flann_search.nearestKSearch (cloud_big, query_indices,no_of_neighbors,indices,dists); std::vector k_indices; k_indices.resize (no_of_neighbors); @@ -233,14 +233,14 @@ TEST (PCL, FlannSearch_knnByIndex) for (std::size_t i = 0; i < query_indices.size (); ++i) { - flann_search->nearestKSearch (cloud_big[2*i], no_of_neighbors, k_indices, k_distances); + flann_search.nearestKSearch (cloud_big[2*i], no_of_neighbors, k_indices, k_distances); EXPECT_EQ (k_indices.size (), indices[i].size ()); EXPECT_EQ (k_distances.size (), dists[i].size ()); for (std::size_t j = 0; j< no_of_neighbors; j++) { EXPECT_TRUE (k_indices[j] == indices[i][j] || k_distances[j] == dists[i][j]); } - flann_search->nearestKSearch (cloud_big,query_indices[i], no_of_neighbors, k_indices, k_distances); + flann_search.nearestKSearch (cloud_big,query_indices[i], no_of_neighbors, k_indices, k_distances); EXPECT_EQ (k_indices.size (), indices[i].size ()); EXPECT_EQ (k_distances.size (), dists[i].size ()); for (std::size_t j = 0; j< no_of_neighbors; j++) @@ -372,8 +372,8 @@ main (int argc, char** argv) init (); // Testing using explicit instantiation of inherited class - pcl::search::Search* FlannSearch = new pcl::search::FlannSearch ( new search::FlannSearch::KdTreeIndexCreator); - FlannSearch->setInputCloud (cloud.makeShared ()); + pcl::search::FlannSearch FlannSearch ( new search::FlannSearch::KdTreeIndexCreator); + FlannSearch.setInputCloud (cloud.makeShared ()); return (RUN_ALL_TESTS ()); } diff --git a/test/search/test_kdtree.cpp b/test/search/test_kdtree.cpp index 028bbbf5..6c73feb1 100644 --- a/test/search/test_kdtree.cpp +++ b/test/search/test_kdtree.cpp @@ -71,8 +71,8 @@ init () /* Test for KdTree nearestKSearch */TEST (PCL, KdTree_nearestKSearch) { - pcl::search::Search* kdtree = new pcl::search::KdTree (); - kdtree->setInputCloud (cloud.makeShared ()); + pcl::search::KdTree kdtree; + kdtree.setInputCloud (cloud.makeShared ()); PointXYZ test_point (0.01f, 0.01f, 0.01f); unsigned int no_of_neighbors = 20; multimap sorted_brute_force_result; @@ -95,7 +95,7 @@ init () std::vector k_distances; k_distances.resize (no_of_neighbors); - kdtree->nearestKSearch (test_point, no_of_neighbors, k_indices, k_distances); + kdtree.nearestKSearch (test_point, no_of_neighbors, k_indices, k_distances); //if (k_indices.size () != no_of_neighbors) std::cerr << "Found "<* kdtree = new pcl::search::KdTree(); - //kdtree->initSearchDS (); - kdtree->setInputCloud (cloud_big.makeShared ()); + pcl::search::KdTree kdtree; + //kdtree.initSearchDS (); + kdtree.setInputCloud (cloud_big.makeShared ()); for (const auto &point : cloud_big.points) - kdtree->nearestKSearch (point, no_of_neighbors, k_indices, k_distances); + kdtree.nearestKSearch (point, no_of_neighbors, k_indices, k_distances); } } @@ -128,9 +128,9 @@ TEST (PCL, KdTree_differentPointT) { unsigned int no_of_neighbors = 20; - pcl::search::Search* kdtree = new pcl::search::KdTree (); - //kdtree->initSearchDS (); - kdtree->setInputCloud (cloud_big.makeShared ()); + pcl::search::KdTree kdtree; + //kdtree.initSearchDS (); + kdtree.setInputCloud (cloud_big.makeShared ()); PointCloud cloud_rgb; @@ -138,7 +138,7 @@ TEST (PCL, KdTree_differentPointT) std::vector< std::vector< float > > dists; std::vector< std::vector< int > > indices; - kdtree->nearestKSearchT (cloud_rgb, std::vector (),no_of_neighbors,indices,dists); + kdtree.nearestKSearchT (cloud_rgb, std::vector (),no_of_neighbors,indices,dists); std::vector k_indices; k_indices.resize (no_of_neighbors); @@ -152,8 +152,8 @@ TEST (PCL, KdTree_differentPointT) for (std::size_t i = 0; i < cloud_rgb.points.size (); ++i) { - kdtree->nearestKSearchT (cloud_rgb.points[i], no_of_neighbors, k_indices_t, k_distances_t); - kdtree->nearestKSearch (cloud_big.points[i], no_of_neighbors, k_indices, k_distances); + kdtree.nearestKSearchT (cloud_rgb.points[i], no_of_neighbors, k_indices_t, k_distances_t); + kdtree.nearestKSearch (cloud_big.points[i], no_of_neighbors, k_indices, k_distances); EXPECT_EQ (k_indices.size (), indices[i].size ()); EXPECT_EQ (k_distances.size (), dists[i].size ()); for (std::size_t j=0; j< no_of_neighbors; j++) @@ -170,13 +170,13 @@ TEST (PCL, KdTree_multipointKnnSearch) { unsigned int no_of_neighbors = 20; - pcl::search::Search* kdtree = new pcl::search::KdTree (); - //kdtree->initSearchDS (); - kdtree->setInputCloud (cloud_big.makeShared ()); + pcl::search::KdTree kdtree; + //kdtree.initSearchDS (); + kdtree.setInputCloud (cloud_big.makeShared ()); std::vector< std::vector< float > > dists; std::vector< std::vector< int > > indices; - kdtree->nearestKSearch (cloud_big, std::vector (),no_of_neighbors,indices,dists); + kdtree.nearestKSearch (cloud_big, std::vector (),no_of_neighbors,indices,dists); std::vector k_indices; k_indices.resize (no_of_neighbors); @@ -185,7 +185,7 @@ TEST (PCL, KdTree_multipointKnnSearch) for (std::size_t i = 0; i < cloud_big.points.size (); ++i) { - kdtree->nearestKSearch (cloud_big.points[i], no_of_neighbors, k_indices, k_distances); + kdtree.nearestKSearch (cloud_big.points[i], no_of_neighbors, k_indices, k_distances); EXPECT_EQ (k_indices.size (), indices[i].size ()); EXPECT_EQ (k_distances.size (), dists[i].size ()); for (std::size_t j=0; j< no_of_neighbors; j++) @@ -202,8 +202,8 @@ main (int argc, char** argv) init (); // Testing using explicit instantiation of inherited class - pcl::search::Search* kdtree = new pcl::search::KdTree (); - kdtree->setInputCloud (cloud.makeShared ()); + pcl::search::KdTree kdtree; + kdtree.setInputCloud (cloud.makeShared ()); return (RUN_ALL_TESTS ()); } diff --git a/test/search/test_octree.cpp b/test/search/test_octree.cpp index b2a7f2ef..3d1cf026 100644 --- a/test/search/test_octree.cpp +++ b/test/search/test_octree.cpp @@ -83,7 +83,7 @@ TEST (PCL, Octree_Pointcloud_Nearest_K_Neighbour_Search) std::priority_queue::VectorType> pointCandidates; // create octree - pcl::search::Search* octree = new pcl::search::Octree (0.1); + pcl::search::Octree octree(0.1); std::vector k_indices; std::vector k_sqr_distances; @@ -146,8 +146,8 @@ TEST (PCL, Octree_Pointcloud_Nearest_K_Neighbour_Search) pointCandidates.pop (); } // octree nearest neighbor search - octree->setInputCloud (cloudIn); - octree->nearestKSearch (searchPoint, static_cast (K), k_indices, k_sqr_distances); + octree.setInputCloud (cloudIn); + octree.nearestKSearch (searchPoint, static_cast (K), k_indices, k_sqr_distances); ASSERT_EQ ( k_indices.size() , k_indices_bruteforce.size() ); diff --git a/test/search/test_organized_index.cpp b/test/search/test_organized_index.cpp index 262dd592..24afaaed 100644 --- a/test/search/test_organized_index.cpp +++ b/test/search/test_organized_index.cpp @@ -81,7 +81,7 @@ TEST (PCL, Organized_Neighbor_Search_Pointcloud_Nearest_K_Neighbour_Search) srand (time (NULL)); // create organized search - pcl::search::Search* organizedNeighborSearch = new pcl::search::OrganizedNeighbor(); + pcl::search::OrganizedNeighbor organizedNeighborSearch; std::vector k_indices; std::vector k_sqr_distances; @@ -124,8 +124,8 @@ TEST (PCL, Organized_Neighbor_Search_Pointcloud_Nearest_K_Neighbour_Search) k_sqr_distances.clear(); // organized nearest neighbor search - organizedNeighborSearch->setInputCloud (cloudIn); -// organizedNeighborSearch->nearestKSearch (searchPoint, (int)K, k_indices, k_sqr_distances); + organizedNeighborSearch.setInputCloud (cloudIn); +// organizedNeighborSearch.nearestKSearch (searchPoint, (int)K, k_indices, k_sqr_distances); k_indices_bruteforce.clear(); k_sqr_distances_bruteforce.clear(); @@ -182,7 +182,7 @@ TEST (PCL, Organized_Neighbor_Search_Pointcloud_Nearest_K_Neighbour_Search_Kinec srand (time (NULL)); // create organized search - pcl::search::Search* organizedNeighborSearch = new pcl::search::OrganizedNeighbor(); + pcl::search::OrganizedNeighbor organizedNeighborSearch; std::vector k_indices; std::vector k_sqr_distances; @@ -222,8 +222,8 @@ TEST (PCL, Organized_Neighbor_Search_Pointcloud_Nearest_K_Neighbour_Search_Kinec k_sqr_distances.clear(); // organized nearest neighbor search - organizedNeighborSearch->setInputCloud (cloudIn); - organizedNeighborSearch->nearestKSearch (searchIdx, (int)K, k_indices, k_sqr_distances); + organizedNeighborSearch.setInputCloud (cloudIn); + organizedNeighborSearch.nearestKSearch (searchIdx, (int)K, k_indices, k_sqr_distances); k_indices_bruteforce.clear(); k_sqr_distances_bruteforce.clear(); @@ -265,7 +265,7 @@ TEST (PCL, Organized_Neighbor_Search_Pointcloud_Neighbours_Within_Radius_Search) srand (time (NULL)); - pcl::search::Search* organizedNeighborSearch = new pcl::search::OrganizedNeighbor(); + pcl::search::OrganizedNeighbor organizedNeighborSearch; std::vector k_indices; std::vector k_sqr_distances; @@ -327,9 +327,9 @@ TEST (PCL, Organized_Neighbor_Search_Pointcloud_Neighbours_Within_Radius_Search) std::vector cloudNWRSearch; std::vector cloudNWRRadius; - organizedNeighborSearch->setInputCloud (cloudIn); + organizedNeighborSearch.setInputCloud (cloudIn); - organizedNeighborSearch->radiusSearch (randomIdx, searchRadius, cloudNWRSearch, cloudNWRRadius, INT_MAX); //,INT_MAX); + organizedNeighborSearch.radiusSearch (randomIdx, searchRadius, cloudNWRSearch, cloudNWRRadius, INT_MAX); //,INT_MAX); // check if result from organized radius search can be also found in bruteforce search @@ -368,7 +368,7 @@ TEST (PCL, Organized_Neighbor_Search_Pointcloud_Neighbours_Within_Radius_Search) ASSERT_EQ (cloudNWRRadius.size() , cloudSearchBruteforce.size ()); // check if result limitation works - organizedNeighborSearch->radiusSearch (searchPoint, searchRadius, cloudNWRSearch, cloudNWRRadius, 5); + organizedNeighborSearch.radiusSearch (searchPoint, searchRadius, cloudNWRSearch, cloudNWRRadius, 5); ASSERT_EQ (cloudNWRRadius.size () <= 5, true); } @@ -380,7 +380,7 @@ TEST (PCL, Organized_Neighbor_Search_Pointcloud_Neighbours_Within_Radius_Search_ constexpr unsigned int test_runs = 10; srand (time (NULL)); - pcl::search::Search* organizedNeighborSearch = new pcl::search::OrganizedNeighbor(); + pcl::search::OrganizedNeighbor organizedNeighborSearch(); std::vector k_indices; std::vector k_sqr_distances; @@ -446,15 +446,15 @@ TEST (PCL, Organized_Neighbor_Search_Pointcloud_Neighbours_Within_Radius_Search_ std::vector cloudNWRRadius; double check_time = getTime(); - organizedNeighborSearch->setInputCloud (cloudIn); - organizedNeighborSearch->radiusSearch (randomIdx, searchRadius, cloudNWRSearch, cloudNWRRadius, INT_MAX); //,INT_MAX); + organizedNeighborSearch.setInputCloud (cloudIn); + organizedNeighborSearch.radiusSearch (randomIdx, searchRadius, cloudNWRSearch, cloudNWRRadius, INT_MAX); //,INT_MAX); double check_time2 = getTime(); radiusSearchLPTime += check_time2 - check_time; - organizedNeighborSearch->setInputCloud (cloudIn); - organizedNeighborSearch->radiusSearch (randomIdx, searchRadius, cloudNWRSearch, cloudNWRRadius, INT_MAX); //,INT_MAX); + organizedNeighborSearch.setInputCloud (cloudIn); + organizedNeighborSearch.radiusSearch (randomIdx, searchRadius, cloudNWRSearch, cloudNWRRadius, INT_MAX); //,INT_MAX); radiusSearchTime += getTime() - check_time2; } @@ -475,7 +475,7 @@ TEST (PCL, Organized_Neighbor_Search_Pointcloud_Neighbours_Within_Radius_Search_ constexpr unsigned int test_runs = 10; srand (time (NULL)); - pcl::search::Search* organizedNeighborSearch = new pcl::search::OrganizedNeighbor(); + pcl::search::OrganizedNeighbor organizedNeighborSearch(); // typical focal length from kinect unsigned int randomIdx; @@ -512,8 +512,8 @@ TEST (PCL, Organized_Neighbor_Search_Pointcloud_Neighbours_Within_Radius_Search_ cloudNWRRadius2.clear(); double check_time = getTime(); - organizedNeighborSearch->setInputCloud (cloudIn); - organizedNeighborSearch->radiusSearch (randomIdx, searchRadius, cloudNWRSearch2, cloudNWRRadius2, INT_MAX); //,INT_MAX); + organizedNeighborSearch.setInputCloud (cloudIn); + organizedNeighborSearch.radiusSearch (randomIdx, searchRadius, cloudNWRSearch2, cloudNWRRadius2, INT_MAX); //,INT_MAX); double check_time2 = getTime(); sum_time+= check_time2 - check_time; @@ -525,8 +525,8 @@ TEST (PCL, Organized_Neighbor_Search_Pointcloud_Neighbours_Within_Radius_Search_ cloudNWRSearch.clear(); cloudNWRRadius.clear(); double check_time = getTime(); - organizedNeighborSearch->setInputCloud (cloudIn); - organizedNeighborSearch->approxRadiusSearch (cloudIn, randomIdx, searchRadius, cloudNWRSearch, cloudNWRRadius, INT_MAX); //,INT_MAX); + organizedNeighborSearch.setInputCloud (cloudIn); + organizedNeighborSearch.approxRadiusSearch (cloudIn, randomIdx, searchRadius, cloudNWRSearch, cloudNWRRadius, INT_MAX); //,INT_MAX); double check_time2 = getTime(); sum_time2+= check_time2 - check_time; @@ -587,7 +587,7 @@ TEST (PCL, Organized_Neighbor_Search_Pointcloud_Neighbours_Within_Radius_Search_ ASSERT_EQ (cloudNWRRadius.size() , cloudSearchBruteforce.size ()); // check if result limitation works - organizedNeighborSearch->radiusSearch (searchPoint, searchRadius, cloudNWRSearch, cloudNWRRadius, 5); + organizedNeighborSearch.radiusSearch (searchPoint, searchRadius, cloudNWRSearch, cloudNWRRadius, 5); ASSERT_EQ (cloudNWRRadius.size () <= 5, true); } diff --git a/test/search/test_search.cpp b/test/search/test_search.cpp index 348fe834..ef6af3b1 100644 --- a/test/search/test_search.cpp +++ b/test/search/test_search.cpp @@ -44,8 +44,10 @@ #include #include #include +#include // for pcl::isFinite #include + using namespace pcl; using namespace std; @@ -299,7 +301,9 @@ testKNNSearch (typename PointCloud::ConstPtr point_cloud, std::vectorsize ()); ++pIdx) { if (!isFinite (point_cloud->points [pIdx])) @@ -310,7 +314,9 @@ testKNNSearch (typename PointCloud::ConstPtr point_cloud, std::vectorsetInputCloud (point_cloud, input_indices_); @@ -320,7 +326,9 @@ testKNNSearch (typename PointCloud::ConstPtr point_cloud, std::vectornearestKSearch (point_cloud->points[query_index], knn, indices [sIdx], distances [sIdx]); @@ -330,7 +338,9 @@ testKNNSearch (typename PointCloud::ConstPtr point_cloud, std::vectorgetName (), @@ -369,7 +379,9 @@ testRadiusSearch (typename PointCloud::ConstPtr point_cloud, std::vector } // remove also Nans - #pragma omp parallel for + #pragma omp parallel for \ + default(none) \ + shared(nan_mask, point_cloud) for (int pIdx = 0; pIdx < int (point_cloud->size ()); ++pIdx) { if (!isFinite (point_cloud->points [pIdx])) @@ -380,7 +392,9 @@ testRadiusSearch (typename PointCloud::ConstPtr point_cloud, std::vector if (!input_indices.empty ()) input_indices_.reset (new pcl::Indices (input_indices)); - #pragma omp parallel for + #pragma omp parallel for \ + default(none) \ + shared(input_indices_, point_cloud, search_methods) for (int sIdx = 0; sIdx < int (search_methods.size ()); ++sIdx) search_methods [sIdx]->setInputCloud (point_cloud, input_indices_); @@ -391,7 +405,9 @@ testRadiusSearch (typename PointCloud::ConstPtr point_cloud, std::vector // find nn for each point in the cloud for (const int &query_index : query_indices) { - #pragma omp parallel for + #pragma omp parallel for \ + default(none) \ + shared(distances, indices, indices_mask, input_indices, nan_mask, passed, point_cloud, radius, query_index, search_methods) for (int sIdx = 0; sIdx < static_cast (search_methods.size ()); ++sIdx) { search_methods [sIdx]->radiusSearch (point_cloud->points[query_index], radius, indices [sIdx], distances [sIdx], 0); @@ -401,7 +417,9 @@ testRadiusSearch (typename PointCloud::ConstPtr point_cloud, std::vector } // compare results to each other - #pragma omp parallel for + #pragma omp parallel for \ + default(none) \ + shared(distances, indices, passed, search_methods) for (int sIdx = 1; sIdx < static_cast (search_methods.size ()); ++sIdx) { passed [sIdx] = passed [sIdx] && compareResults (indices [0], distances [0], search_methods [0]->getName (), diff --git a/test/test_recognition_ransac_based_ORROctree.cpp b/test/test_recognition_ransac_based_ORROctree.cpp index 73a9b5f7..3c83692e 100644 --- a/test/test_recognition_ransac_based_ORROctree.cpp +++ b/test/test_recognition_ransac_based_ORROctree.cpp @@ -94,9 +94,9 @@ TEST (ORROctreeTest, OctreeSphereIntersection) float frac_of_points_for_registration = 0.3f; std::string object_name = "test_object"; - ModelLibrary::Model* new_model = new ModelLibrary::Model (*model_cloud, *model_cloud_normals, voxel_size, object_name, frac_of_points_for_registration); + ModelLibrary::Model new_model (*model_cloud, *model_cloud_normals, voxel_size, object_name, frac_of_points_for_registration); - const ORROctree& octree = new_model->getOctree (); + const ORROctree& octree = new_model.getOctree (); const std::vector &full_leaves = octree.getFullLeaves (); list inter_leaves; @@ -113,7 +113,6 @@ TEST (ORROctreeTest, OctreeSphereIntersection) EXPECT_NE(*leaf1, *leaf2); } } - } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/test/visualization/test_visualization.cpp b/test/visualization/test_visualization.cpp index a6c4c5c6..b34c30d5 100644 --- a/test/visualization/test_visualization.cpp +++ b/test/visualization/test_visualization.cpp @@ -39,6 +39,7 @@ #include +#include #include #include #include @@ -60,6 +61,28 @@ PointCloud::Ptr cloud_with_normals1 (new PointCloud); search::KdTree::Ptr tree3; search::KdTree::Ptr tree4; +// Test that updatepointcloud works when removing points. Ie. modifying vtk data structure to reflect modified pointcloud +// See #4001 and #3452 for previously undetected error. +//////////////////////////////////////////////////////////////////////////////// +TEST(PCL, PCLVisualizer_updatePointCloud) +{ + pcl::common::CloudGenerator > generator; + + pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); + int result = generator.fill(3, 1, *cloud); + + // Setup a basic viewport window + pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer")); + pcl::visualization::PointCloudColorHandlerCustom color_green(cloud, 0, 225, 100); + viewer->addPointCloud(cloud, color_green, "sample cloud"); + + // remove points one by one) + while (!cloud->empty()) { + cloud->erase(cloud->end() - 1); + viewer->updatePointCloud(cloud, "sample cloud"); + viewer->spinOnce(100); + } +} ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// TEST (PCL, PCLVisualizer_camera) { diff --git a/tools/CMakeLists.txt b/tools/CMakeLists.txt index 097c6e13..1911f7b9 100644 --- a/tools/CMakeLists.txt +++ b/tools/CMakeLists.txt @@ -183,9 +183,6 @@ else() PCL_ADD_EXECUTABLE(pcl_pcd2png COMPONENT ${SUBSYS_NAME} SOURCES pcd2png.cpp) target_link_libraries(pcl_pcd2png pcl_common pcl_io) - PCL_ADD_EXECUTABLE(pcl_organized_pcd_to_png COMPONENT ${SUBSYS_NAME} SOURCES organized_pcd_to_png.cpp) - target_link_libraries (pcl_organized_pcd_to_png pcl_common pcl_io) - PCL_ADD_EXECUTABLE(pcl_tiff2pcd COMPONENT ${SUBSYS_NAME} SOURCES tiff2pcd.cpp) target_link_libraries(pcl_tiff2pcd pcl_common pcl_io) diff --git a/tools/boost.h b/tools/boost.h index 46bb030e..ed20728a 100644 --- a/tools/boost.h +++ b/tools/boost.h @@ -46,7 +46,6 @@ #ifndef Q_MOC_RUN // Marking all Boost headers as system headers to remove warnings -#include #include #include #include diff --git a/tools/cluster_extraction.cpp b/tools/cluster_extraction.cpp index 90ecb022..2136cd43 100644 --- a/tools/cluster_extraction.cpp +++ b/tools/cluster_extraction.cpp @@ -3,7 +3,7 @@ * * Point Cloud Library (PCL) - www.pointclouds.org * Copyright (c) 2011, Willow Garage, Inc. - * + * * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -36,6 +36,7 @@ */ #include +#include // for pcl::make_shared #include #include #include @@ -43,8 +44,8 @@ #include #include #include + #include -#include "boost.h" using namespace pcl; using namespace pcl::io; @@ -62,11 +63,11 @@ printHelp (int, char **argv) { print_error ("Syntax is: %s input.pcd output.pcd \n", argv[0]); print_info (" where options are:\n"); - print_info (" -min X = use a minimum of X points peer cluster (default: "); + print_info (" -min X = use a minimum of X points peer cluster (default: "); print_value ("%d", default_min); print_info (")\n"); - print_info (" -max X = use a maximum of X points peer cluster (default: "); + print_info (" -max X = use a maximum of X points peer cluster (default: "); print_value ("%d", default_max); print_info (")\n"); - print_info (" -tolerance X = the spacial distance between clusters (default: "); + print_info (" -tolerance X = the spacial distance between clusters (default: "); print_value ("%lf", default_tolerance); print_info (")\n"); } @@ -96,7 +97,7 @@ compute (const pcl::PCLPointCloud2::ConstPtr &input, std::vector extract; extract.setInputCloud (input); - extract.setIndices (boost::make_shared (*it)); + extract.setIndices (pcl::make_shared (*it)); pcl::PCLPointCloud2::Ptr out (new pcl::PCLPointCloud2); extract.filter (*out); output.push_back (out); @@ -143,7 +144,7 @@ saveCloud (const std::string &filename, const std::vectorwidth * output[i]->height); print_info (" points]\n"); } - + } /* ---[ */ @@ -178,7 +179,7 @@ main (int argc, char** argv) // Load the first file pcl::PCLPointCloud2::Ptr cloud (new pcl::PCLPointCloud2); - if (!loadCloud (argv[p_file_indices[0]], *cloud)) + if (!loadCloud (argv[p_file_indices[0]], *cloud)) return (-1); // Perform the feature estimation diff --git a/tools/depth_sense_viewer.cpp b/tools/depth_sense_viewer.cpp index 699babe3..d5565587 100644 --- a/tools/depth_sense_viewer.cpp +++ b/tools/depth_sense_viewer.cpp @@ -35,6 +35,7 @@ * */ +#include #include #include #include @@ -43,7 +44,6 @@ #include #include -#include #include #include diff --git a/tools/fast_bilateral_filter.cpp b/tools/fast_bilateral_filter.cpp index 315a7f95..cbec0683 100644 --- a/tools/fast_bilateral_filter.cpp +++ b/tools/fast_bilateral_filter.cpp @@ -112,9 +112,9 @@ saveCloud (const string &filename, const pcl::PCLPointCloud2 &output, int batchProcess (const std::vector &pcd_files, string &output_dir, float sigma_s, float sigma_r) { -#if _OPENMP -#pragma omp parallel for -#endif +#pragma omp parallel for \ + default(none) \ + shared(output_dir, pcd_files, sigma_r, sigma_s) for (int i = 0; i < int (pcd_files.size ()); ++i) { // Load the first file diff --git a/tools/normal_estimation.cpp b/tools/normal_estimation.cpp index b5f44a8d..adea87ea 100644 --- a/tools/normal_estimation.cpp +++ b/tools/normal_estimation.cpp @@ -131,9 +131,9 @@ saveCloud (const string &filename, const pcl::PCLPointCloud2 &output, int batchProcess (const std::vector &pcd_files, string &output_dir, int k, double radius) { -#if _OPENMP -#pragma omp parallel for -#endif +#pragma omp parallel for \ + default(none) \ + shared(k, output_dir, pcd_files, radius) for (int i = 0; i < int (pcd_files.size ()); ++i) { // Load the first file diff --git a/tools/oni_viewer_simple.cpp b/tools/oni_viewer_simple.cpp index 7cde623a..54f35df7 100644 --- a/tools/oni_viewer_simple.cpp +++ b/tools/oni_viewer_simple.cpp @@ -185,24 +185,21 @@ main(int argc, char ** argv) pcl::TimeTrigger trigger; - pcl::ONIGrabber* grabber = nullptr; - if (frame_rate == 0) - grabber = new pcl::ONIGrabber(arg, true, true); - else + pcl::ONIGrabber grabber (arg, true, frame_rate == 0); + if (frame_rate != 0) { - grabber = new pcl::ONIGrabber(arg, true, false); trigger.setInterval (1.0 / static_cast (frame_rate)); - trigger.registerCallback ([=] { grabber->start (); }); + trigger.registerCallback ([&] { grabber.start (); }); trigger.start(); } - if (grabber->providesCallback () && !pcl::console::find_switch (argc, argv, "-xyz")) + if (grabber.providesCallback () && !pcl::console::find_switch (argc, argv, "-xyz")) { - SimpleONIViewer v(*grabber); + SimpleONIViewer v(grabber); v.run(); } else { - SimpleONIViewer v(*grabber); + SimpleONIViewer v(grabber); v.run(); } diff --git a/tools/openni_viewer_simple.cpp b/tools/openni_viewer_simple.cpp index 9c630660..1df1fb65 100644 --- a/tools/openni_viewer_simple.cpp +++ b/tools/openni_viewer_simple.cpp @@ -224,8 +224,7 @@ class SimpleOpenNIViewer cloud_connection.disconnect(); image_connection.disconnect(); - if (rgb_data) - delete[] rgb_data; + delete[] rgb_data; } pcl::visualization::CloudViewer cloud_viewer_; diff --git a/tools/organized_pcd_to_png.cpp b/tools/organized_pcd_to_png.cpp deleted file mode 100644 index 0a2be3ae..00000000 --- a/tools/organized_pcd_to_png.cpp +++ /dev/null @@ -1,131 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Point Cloud Library (PCL) - www.pointclouds.org - * Copyright (c) 2013, Open Perception, Inc. - * - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder(s) nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - */ - -#include -#include -#include -#include -#include - -using namespace pcl; -using namespace pcl::io; -using namespace pcl::console; - -void -printHelp (int, char **argv) -{ - print_error ("Syntax is: %s input.pcd output.png\n", argv[0]); -} - -bool -loadCloud (const std::string &filename, pcl::PCLPointCloud2 &cloud) -{ - TicToc tt; - print_highlight ("Loading "); print_value ("%s ", filename.c_str ()); - - tt.tic (); - if (loadPCDFile (filename, cloud) < 0) - return (false); - print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", cloud.width * cloud.height); print_info (" points]\n"); - print_info ("Available dimensions: "); print_value ("%s\n", pcl::getFieldsList (cloud).c_str ()); - - return (true); -} - -void -saveImage (const std::string &filename, const PointCloud &image) -{ - TicToc tt; - tt.tic (); - - print_highlight ("Saving "); print_value ("%s ", filename.c_str ()); - io::savePNGFile (filename, image, "rgb"); - - print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", image.width * image.height); print_info (" points]\n"); -} - -/* ---[ */ -int -main (int argc, char** argv) -{ - print_error ("This tool is deprecated, please use \"pcl_pcd2png\" instead!\n"); - print_info ("Convert the RGB information of an organized PCD file to a PNG image. For more information, use: %s -h\n", argv[0]); - - if (argc < 3) - { - printHelp (argc, argv); - return (-1); - } - - // Parse the command line arguments for .pcd files - std::vector pcd_file_indices = parse_file_extension_argument (argc, argv, ".pcd"); - if (pcd_file_indices.size () != 1) - { - print_error ("Need one input PCD file and one output PNG file.\n"); - return (-1); - } - - std::vector png_file_indices = parse_file_extension_argument (argc, argv, ".png"); - if (png_file_indices.size () != 1) - { - print_error ("Need one input PCD file and one output PNG file.\n"); - return (-1); - } - - // Load the first file - pcl::PCLPointCloud2 cloud; - if (!loadCloud (argv[pcd_file_indices[0]], cloud)) - return (-1); - - - PointCloud image; - fromPCLPointCloud2 (cloud, image); - - - // Check if the cloud is organized - if (!image.isOrganized ()) - { - PCL_ERROR ("Input cloud is not organized.\n"); - return (-1); - } - - // Save cloud - saveImage (argv[png_file_indices[0]], image); - - return (0); -} - diff --git a/tools/real_sense_viewer.cpp b/tools/real_sense_viewer.cpp index 8dd71afc..92450abb 100644 --- a/tools/real_sense_viewer.cpp +++ b/tools/real_sense_viewer.cpp @@ -35,6 +35,7 @@ * */ +#include #include #include #include @@ -44,7 +45,6 @@ #include #include -#include #include #include diff --git a/tools/virtual_scanner.cpp b/tools/virtual_scanner.cpp index 8baf0a7a..7c93d2de 100644 --- a/tools/virtual_scanner.cpp +++ b/tools/virtual_scanner.cpp @@ -39,21 +39,25 @@ * \author Radu Bogdan Rusu * * @b virtual_scanner takes in a .ply or a .vtk file of an object model, and virtually scans it - * in a raytracing fashion, saving the end results as PCD (Point Cloud Data) files. In addition, - * it noisifies the PCD models, and downsamples them. + * in a raytracing fashion, saving the end results as PCD (Point Cloud Data) files. In addition, + * it noisifies the PCD models, and downsamples them. * The viewpoint can be set to 1 or multiple views on a sphere. */ #include #include + #include #include #include #include +#include // for pcl::make_shared #include #include #include -#include "boost.h" + +#include // for boost::is_any_of, boost::split, boost::token_compress_on, boost::trim +#include // for boost::filesystem::create_directories, boost::filesystem::exists, boost::filesystem::extension, boost::filesystem::path using namespace pcl; @@ -138,7 +142,7 @@ main (int argc, char** argv) PCL_ERROR ("Error: no .PLY or .VTK files given!\n"); return (-1); } - + std::stringstream filename_stream; if (!p_file_indices_ply.empty ()) filename_stream << argv[p_file_indices_ply.at (0)]; @@ -146,9 +150,9 @@ main (int argc, char** argv) filename_stream << argv[p_file_indices_vtk.at (0)]; filename = filename_stream.str (); - + data = loadDataSet (filename.c_str ()); - + PCL_INFO ("Loaded model with %d vertices/points.\n", data->GetNumberOfPoints ()); // Default scan parameters @@ -207,7 +211,7 @@ main (int argc, char** argv) double temp_beam[3], beam[3], p[3]; double p_coords[3], x[3], t; int subId; - + // Create a Icosahedron at center in origin and radius of 1 vtkSmartPointer icosa = vtkSmartPointer::New (); icosa->SetSolidTypeToIcosahedron (); @@ -300,7 +304,7 @@ main (int argc, char** argv) up[0] /= up_len; up[1] /= up_len; up[2] /= up_len; - + // Output resulting vectors std::cerr << "Viewray Right Up:" << std::endl; std::cerr << viewray[0] << " " << viewray[1] << " " << viewray[2] << " " << std::endl; @@ -329,7 +333,7 @@ main (int argc, char** argv) for (double hor = hor_start; hor <= hor_end; hor += sp.hor_res) { pid ++; - + // Create a beam vector with (lat,long) angles (vert, hor) with the viewray tr2->Identity (); tr2->RotateWXYZ (hor, up); @@ -347,11 +351,11 @@ main (int argc, char** argv) pcl::PointWithViewpoint pt; if (object_coordinates) { - pt.x = static_cast (x[0]); - pt.y = static_cast (x[1]); + pt.x = static_cast (x[0]); + pt.y = static_cast (x[1]); pt.z = static_cast (x[2]); - pt.vp_x = static_cast (eye[0]); - pt.vp_y = static_cast (eye[1]); + pt.vp_x = static_cast (eye[0]); + pt.vp_y = static_cast (eye[1]); pt.vp_z = static_cast (eye[2]); } else @@ -393,7 +397,7 @@ main (int argc, char** argv) // Downsample and remove silly point duplicates pcl::PointCloud cloud_downsampled; - grid.setInputCloud (boost::make_shared > (cloud)); + grid.setInputCloud (pcl::make_shared > (cloud)); //grid.filter (cloud_downsampled); // Saves the point cloud data to disk diff --git a/tracking/include/pcl/tracking/boost.h b/tracking/include/pcl/tracking/boost.h index ef28f5ee..64a8d172 100644 --- a/tracking/include/pcl/tracking/boost.h +++ b/tracking/include/pcl/tracking/boost.h @@ -41,6 +41,6 @@ #include -PCL_PRAGMA_WARNING("This header is deprecated and will be removed in an upcoming release.") +PCL_DEPRECATED_HEADER(1, 12, "") #include diff --git a/tracking/include/pcl/tracking/distance_coherence.h b/tracking/include/pcl/tracking/distance_coherence.h index de5a1aae..f2bc6f27 100644 --- a/tracking/include/pcl/tracking/distance_coherence.h +++ b/tracking/include/pcl/tracking/distance_coherence.h @@ -1,9 +1,9 @@ #pragma once -#include - +#include #include + namespace pcl { namespace tracking diff --git a/tracking/include/pcl/tracking/impl/kld_adaptive_particle_filter_omp.hpp b/tracking/include/pcl/tracking/impl/kld_adaptive_particle_filter_omp.hpp index e1fe8630..f1c68720 100644 --- a/tracking/include/pcl/tracking/impl/kld_adaptive_particle_filter_omp.hpp +++ b/tracking/include/pcl/tracking/impl/kld_adaptive_particle_filter_omp.hpp @@ -23,9 +23,9 @@ pcl::tracking::KLDAdaptiveParticleFilterOMPTracker::weight () { if (!use_normal_) { -#ifdef _OPENMP -#pragma omp parallel for num_threads(threads_) -#endif +#pragma omp parallel for \ + default(none) \ + num_threads(threads_) for (int i = 0; i < particle_num_; i++) this->computeTransformedPointCloudWithoutNormal (particles_->points[i], *transed_reference_vector_[i]); @@ -40,9 +40,9 @@ pcl::tracking::KLDAdaptiveParticleFilterOMPTracker::weight () change_counter_ = change_detector_interval_; coherence_->setTargetCloud (coherence_input); coherence_->initCompute (); -#ifdef _OPENMP -#pragma omp parallel for num_threads(threads_) -#endif +#pragma omp parallel for \ + default(none) \ + num_threads(threads_) for (int i = 0; i < particle_num_; i++) { IndicesPtr indices; @@ -57,9 +57,9 @@ pcl::tracking::KLDAdaptiveParticleFilterOMPTracker::weight () --change_counter_; coherence_->setTargetCloud (coherence_input); coherence_->initCompute (); -#ifdef _OPENMP -#pragma omp parallel for num_threads(threads_) -#endif +#pragma omp parallel for \ + default(none) \ + num_threads(threads_) for (int i = 0; i < particle_num_; i++) { IndicesPtr indices; @@ -74,9 +74,10 @@ pcl::tracking::KLDAdaptiveParticleFilterOMPTracker::weight () { indices_list[i] = IndicesPtr (new std::vector); } -#ifdef _OPENMP -#pragma omp parallel for num_threads(threads_) -#endif +#pragma omp parallel for \ + default(none) \ + shared(indices_list) \ + num_threads(threads_) for (int i = 0; i < particle_num_; i++) { this->computeTransformedPointCloudWithNormal (particles_->points[i], *indices_list[i], *transed_reference_vector_[i]); @@ -87,9 +88,10 @@ pcl::tracking::KLDAdaptiveParticleFilterOMPTracker::weight () coherence_->setTargetCloud (coherence_input); coherence_->initCompute (); -#ifdef _OPENMP -#pragma omp parallel for num_threads(threads_) -#endif +#pragma omp parallel for \ + default(none) \ + shared(indices_list) \ + num_threads(threads_) for (int i = 0; i < particle_num_; i++) { coherence_->compute (transed_reference_vector_[i], indices_list[i], particles_->points[i].weight); diff --git a/tracking/include/pcl/tracking/impl/particle_filter_omp.hpp b/tracking/include/pcl/tracking/impl/particle_filter_omp.hpp index bfc100bc..6932574f 100644 --- a/tracking/include/pcl/tracking/impl/particle_filter_omp.hpp +++ b/tracking/include/pcl/tracking/impl/particle_filter_omp.hpp @@ -23,9 +23,9 @@ pcl::tracking::ParticleFilterOMPTracker::weight () { if (!use_normal_) { -#ifdef _OPENMP -#pragma omp parallel for num_threads(threads_) -#endif +#pragma omp parallel for \ + default(none) \ + num_threads(threads_) for (int i = 0; i < particle_num_; i++) this->computeTransformedPointCloudWithoutNormal (particles_->points[i], *transed_reference_vector_[i]); @@ -40,9 +40,9 @@ pcl::tracking::ParticleFilterOMPTracker::weight () change_counter_ = change_detector_interval_; coherence_->setTargetCloud (coherence_input); coherence_->initCompute (); -#ifdef _OPENMP -#pragma omp parallel for num_threads(threads_) -#endif +#pragma omp parallel for \ + default(none) \ + num_threads(threads_) for (int i = 0; i < particle_num_; i++) { IndicesPtr indices; // dummy @@ -57,9 +57,9 @@ pcl::tracking::ParticleFilterOMPTracker::weight () --change_counter_; coherence_->setTargetCloud (coherence_input); coherence_->initCompute (); -#ifdef _OPENMP -#pragma omp parallel for num_threads(threads_) -#endif +#pragma omp parallel for \ + default(none) \ + num_threads(threads_) for (int i = 0; i < particle_num_; i++) { IndicesPtr indices; // dummy @@ -74,9 +74,10 @@ pcl::tracking::ParticleFilterOMPTracker::weight () { indices_list[i] = IndicesPtr (new std::vector); } -#ifdef _OPENMP -#pragma omp parallel for num_threads(threads_) -#endif +#pragma omp parallel for \ + default(none) \ + shared(indices_list) \ + num_threads(threads_) for (int i = 0; i < particle_num_; i++) { this->computeTransformedPointCloudWithNormal (particles_->points[i], *indices_list[i], *transed_reference_vector_[i]); @@ -87,9 +88,10 @@ pcl::tracking::ParticleFilterOMPTracker::weight () coherence_->setTargetCloud (coherence_input); coherence_->initCompute (); -#ifdef _OPENMP -#pragma omp parallel for num_threads(threads_) -#endif +#pragma omp parallel for \ + default(none) \ + shared(indices_list) \ + num_threads(threads_) for (int i = 0; i < particle_num_; i++) { coherence_->compute (transed_reference_vector_[i], indices_list[i], particles_->points[i].weight); diff --git a/tracking/include/pcl/tracking/impl/pyramidal_klt.hpp b/tracking/include/pcl/tracking/impl/pyramidal_klt.hpp index 16887e4b..14a951ba 100644 --- a/tracking/include/pcl/tracking/impl/pyramidal_klt.hpp +++ b/tracking/include/pcl/tracking/impl/pyramidal_klt.hpp @@ -43,17 +43,23 @@ #include #include -/////////////////////////////////////////////////////////////////////////////////////////////// + +namespace pcl +{ + +namespace tracking +{ + template inline void -pcl::tracking::PyramidalKLTTracker::setTrackingWindowSize (int width, int height) +PyramidalKLTTracker::setTrackingWindowSize (int width, int height) { track_width_ = width; track_height_ = height; } -/////////////////////////////////////////////////////////////////////////////////////////////// + template inline void -pcl::tracking::PyramidalKLTTracker::setPointsToTrack (const pcl::PointCloud::ConstPtr& keypoints) +PyramidalKLTTracker::setPointsToTrack (const pcl::PointCloud::ConstPtr& keypoints) { if (keypoints->size () <= keypoints_nbr_) keypoints_ = keypoints; @@ -70,9 +76,9 @@ pcl::tracking::PyramidalKLTTracker::setPointsToTrack (cons keypoints_status_->indices.resize (keypoints_->size (), 0); } -/////////////////////////////////////////////////////////////////////////////////////////////// + template inline void -pcl::tracking::PyramidalKLTTracker::setPointsToTrack (const pcl::PointIndicesConstPtr& points) +PyramidalKLTTracker::setPointsToTrack (const pcl::PointIndicesConstPtr& points) { assert ((input_ || ref_) && "[pcl::tracking::PyramidalKLTTracker] CALL setInputCloud FIRST!"); @@ -88,9 +94,9 @@ pcl::tracking::PyramidalKLTTracker::setPointsToTrack (cons setPointsToTrack (keypoints); } -/////////////////////////////////////////////////////////////////////////////////////////////// + template bool -pcl::tracking::PyramidalKLTTracker::initCompute () +PyramidalKLTTracker::initCompute () { // std::cout << ">>> [PyramidalKLTTracker::initCompute]" << std::endl; if (!PCLBase::initCompute ()) @@ -154,14 +160,15 @@ pcl::tracking::PyramidalKLTTracker::initCompute () computePyramids (ref_, ref_pyramid_, pcl::BORDER_REFLECT_101); return (true); } + initialized_ = true; return (true); } -/////////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::tracking::PyramidalKLTTracker::derivatives (const FloatImage& src, FloatImage& grad_x, FloatImage& grad_y) const +PyramidalKLTTracker::derivatives (const FloatImage& src, FloatImage& grad_x, FloatImage& grad_y) const { // std::cout << ">>> derivatives" << std::endl; //////////////////////////////////////////////////////// @@ -214,9 +221,9 @@ pcl::tracking::PyramidalKLTTracker::derivatives (const Flo } } -/////////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::tracking::PyramidalKLTTracker::downsample (const FloatImageConstPtr& input, +PyramidalKLTTracker::downsample (const FloatImageConstPtr& input, FloatImageConstPtr& output) const { FloatImage smoothed (input->width, input->height); @@ -229,9 +236,11 @@ pcl::tracking::PyramidalKLTTracker::downsample (const Floa ii[i] = 2 * i; FloatImagePtr down (new FloatImage (width, height)); -#ifdef _OPENMP -#pragma omp parallel for shared (output) firstprivate (ii) num_threads (threads_) -#endif +#pragma omp parallel for \ + default(none) \ + shared(down, height, output, smoothed, width) \ + firstprivate(ii) \ + num_threads(threads_) for (int j = 0; j < height; ++j) { int jj = 2*j; @@ -242,12 +251,12 @@ pcl::tracking::PyramidalKLTTracker::downsample (const Floa output = down; } -/////////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::tracking::PyramidalKLTTracker::downsample (const FloatImageConstPtr& input, - FloatImageConstPtr& output, - FloatImageConstPtr& output_grad_x, - FloatImageConstPtr& output_grad_y) const +PyramidalKLTTracker::downsample (const FloatImageConstPtr& input, + FloatImageConstPtr& output, + FloatImageConstPtr& output_grad_x, + FloatImageConstPtr& output_grad_y) const { downsample (input, output); FloatImagePtr grad_x (new FloatImage (input->width, input->height)); @@ -257,27 +266,28 @@ pcl::tracking::PyramidalKLTTracker::downsample (const Floa output_grad_y = grad_y; } -/////////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::tracking::PyramidalKLTTracker::convolve (const FloatImageConstPtr& input, FloatImage& output) const +PyramidalKLTTracker::convolve (const FloatImageConstPtr& input, FloatImage& output) const { FloatImagePtr tmp (new FloatImage (input->width, input->height)); convolveRows (input, *tmp); convolveCols (tmp, output); } -/////////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::tracking::PyramidalKLTTracker::convolveRows (const FloatImageConstPtr& input, FloatImage& output) const +PyramidalKLTTracker::convolveRows (const FloatImageConstPtr& input, FloatImage& output) const { int width = input->width; int height = input->height; int last = input->width - kernel_size_2_; int w = last - 1; -#ifdef _OPENMP -#pragma omp parallel for shared (output) num_threads (threads_) -#endif +#pragma omp parallel for \ + default(none) \ + shared(input, height, last, output, w, width) \ + num_threads(threads_) for (int j = 0; j < height; ++j) { for (int i = kernel_size_2_; i < last; ++i) @@ -297,9 +307,9 @@ pcl::tracking::PyramidalKLTTracker::convolveRows (const Fl } } -/////////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::tracking::PyramidalKLTTracker::convolveCols (const FloatImageConstPtr& input, FloatImage& output) const +PyramidalKLTTracker::convolveCols (const FloatImageConstPtr& input, FloatImage& output) const { output = FloatImage (input->width, input->height); @@ -308,9 +318,10 @@ pcl::tracking::PyramidalKLTTracker::convolveCols (const Fl int last = input->height - kernel_size_2_; int h = last -1; -#ifdef _OPENMP -#pragma omp parallel for shared (output) num_threads (threads_) -#endif +#pragma omp parallel for \ + default(none) \ + shared(input, h, height, last, output, width) \ + num_threads(threads_) for (int i = 0; i < width; ++i) { for (int j = kernel_size_2_; j < last; ++j) @@ -329,20 +340,21 @@ pcl::tracking::PyramidalKLTTracker::convolveCols (const Fl } } -/////////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::tracking::PyramidalKLTTracker::computePyramids (const PointCloudInConstPtr& input, - std::vector& pyramid, - pcl::InterpolationType border_type) const +PyramidalKLTTracker::computePyramids (const PointCloudInConstPtr& input, + std::vector& pyramid, + pcl::InterpolationType border_type) const { int step = 3; pyramid.resize (step * nb_levels_); FloatImageConstPtr previous; FloatImagePtr tmp (new FloatImage (input->width, input->height)); -#ifdef _OPENMP -#pragma omp parallel for num_threads (threads_) -#endif +#pragma omp parallel for \ + default(none) \ + shared(input, tmp) \ + num_threads(threads_) for (int i = 0; i < static_cast (input->size ()); ++i) tmp->points[i] = intensity_ (input->points[i]); previous = tmp; @@ -397,17 +409,17 @@ pcl::tracking::PyramidalKLTTracker::computePyramids (const } } -/////////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::tracking::PyramidalKLTTracker::spatialGradient (const FloatImage& img, - const FloatImage& grad_x, - const FloatImage& grad_y, - const Eigen::Array2i& location, - const Eigen::Array4f& weight, - Eigen::ArrayXXf& win, - Eigen::ArrayXXf& grad_x_win, - Eigen::ArrayXXf& grad_y_win, - Eigen::Array3f &covariance) const +PyramidalKLTTracker::spatialGradient (const FloatImage& img, + const FloatImage& grad_x, + const FloatImage& grad_y, + const Eigen::Array2i& location, + const Eigen::Array4f& weight, + Eigen::ArrayXXf& win, + Eigen::ArrayXXf& grad_x_win, + Eigen::ArrayXXf& grad_y_win, + Eigen::Array3f &covariance) const { const int step = img.width; covariance.setZero (); @@ -438,15 +450,15 @@ pcl::tracking::PyramidalKLTTracker::spatialGradient (const } } -/////////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::tracking::PyramidalKLTTracker::mismatchVector (const Eigen::ArrayXXf& prev, - const Eigen::ArrayXXf& prev_grad_x, - const Eigen::ArrayXXf& prev_grad_y, - const FloatImage& next, - const Eigen::Array2i& location, - const Eigen::Array4f& weight, - Eigen::Array2f &b) const +PyramidalKLTTracker::mismatchVector (const Eigen::ArrayXXf& prev, + const Eigen::ArrayXXf& prev_grad_x, + const Eigen::ArrayXXf& prev_grad_y, + const FloatImage& next, + const Eigen::Array2i& location, + const Eigen::Array4f& weight, + Eigen::Array2f &b) const { const int step = next.width; b.setZero (); @@ -467,16 +479,16 @@ pcl::tracking::PyramidalKLTTracker::mismatchVector (const } } -/////////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::tracking::PyramidalKLTTracker::track (const PointCloudInConstPtr& prev_input, - const PointCloudInConstPtr& input, - const std::vector& prev_pyramid, - const std::vector& pyramid, - const pcl::PointCloud::ConstPtr& prev_keypoints, - pcl::PointCloud::Ptr& keypoints, - std::vector& status, - Eigen::Affine3f& motion) const +PyramidalKLTTracker::track (const PointCloudInConstPtr& prev_input, + const PointCloudInConstPtr& input, + const std::vector& prev_pyramid, + const std::vector& pyramid, + const pcl::PointCloud::ConstPtr& prev_keypoints, + pcl::PointCloud::Ptr& keypoints, + std::vector& status, + Eigen::Affine3f& motion) const { std::vector > next_pts (prev_keypoints->size ()); Eigen::Array2f half_win ((track_width_-1)*0.5f, (track_height_-1)*0.5f); @@ -614,12 +626,13 @@ pcl::tracking::PyramidalKLTTracker::track (const PointClou } } } + motion = transformation_computer.getTransformation (); } -/////////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::tracking::PyramidalKLTTracker::computeTracking () +PyramidalKLTTracker::computeTracking () { if (!initialized_) return; @@ -637,4 +650,8 @@ pcl::tracking::PyramidalKLTTracker::computeTracking () keypoints_status_->indices = status; } +} // namespace tracking +} // namespace pcl + #endif + diff --git a/tracking/include/pcl/tracking/impl/tracking.hpp b/tracking/include/pcl/tracking/impl/tracking.hpp index 2078d047..47930d52 100644 --- a/tracking/include/pcl/tracking/impl/tracking.hpp +++ b/tracking/include/pcl/tracking/impl/tracking.hpp @@ -1,6 +1,7 @@ #ifndef PCL_TRACKING_IMPL_TRACKING_H_ #define PCL_TRACKING_IMPL_TRACKING_H_ +#include #include #include #include diff --git a/tracking/include/pcl/tracking/particle_filter.h b/tracking/include/pcl/tracking/particle_filter.h index f954f3ec..c8455139 100644 --- a/tracking/include/pcl/tracking/particle_filter.h +++ b/tracking/include/pcl/tracking/particle_filter.h @@ -1,15 +1,15 @@ #pragma once -#include - -#include -#include -#include +#include #include #include +#include +#include +#include #include + namespace pcl { namespace tracking diff --git a/tracking/include/pcl/tracking/pyramidal_klt.h b/tracking/include/pcl/tracking/pyramidal_klt.h index 3e379fbb..4131ce6d 100644 --- a/tracking/include/pcl/tracking/pyramidal_klt.h +++ b/tracking/include/pcl/tracking/pyramidal_klt.h @@ -37,6 +37,7 @@ #pragma once +#include #include #include #include diff --git a/tracking/include/pcl/tracking/tracker.h b/tracking/include/pcl/tracking/tracker.h index 0308e161..7b322588 100644 --- a/tracking/include/pcl/tracking/tracker.h +++ b/tracking/include/pcl/tracking/tracker.h @@ -40,6 +40,7 @@ #pragma once #include +#include #include #include #include @@ -57,11 +58,11 @@ namespace pcl { protected: using PCLBase::deinitCompute; - + public: using PCLBase::indices_; using PCLBase::input_; - + using BaseClass = PCLBase; using Ptr = shared_ptr< Tracker >; using ConstPtr = shared_ptr< const Tracker >; @@ -72,21 +73,21 @@ namespace pcl using PointCloudIn = pcl::PointCloud; using PointCloudInPtr = typename PointCloudIn::Ptr; using PointCloudInConstPtr = typename PointCloudIn::ConstPtr; - + using PointCloudState = pcl::PointCloud; using PointCloudStatePtr = typename PointCloudState::Ptr; using PointCloudStateConstPtr = typename PointCloudState::ConstPtr; - + public: /** \brief Empty constructor. */ Tracker (): search_ () {} - - /** \brief Base method for tracking for all points given in - * using the indices in setIndices () + + /** \brief Base method for tracking for all points given in + * using the indices in setIndices () */ - void + void compute (); - + protected: /** \brief The tracker name. */ std::string tracker_name_; @@ -95,7 +96,7 @@ namespace pcl SearchPtr search_; /** \brief Get a string representation of the name of this class. */ - inline const std::string& + inline const std::string& getClassName () const { return (tracker_name_); } /** \brief This method should get called before starting the actual computation. */ @@ -106,25 +107,25 @@ namespace pcl * to estimate the features for every point in the input dataset. This * is optional, if this is not set, it will only use the data in the * input cloud to estimate the features. This is useful when you only - * need to compute the features for a downsampled cloud. + * need to compute the features for a downsampled cloud. * \param search a pointer to a PointCloud message */ - inline void + inline void setSearchMethod (const SearchPtr &search) { search_ = search; } /** \brief Get a pointer to the point cloud dataset. */ - inline SearchPtr + inline SearchPtr getSearchMethod () { return (search_); } - + /** \brief Get an instance of the result of tracking. */ - virtual StateT + virtual StateT getResult () const = 0; - + private: /** \brief Abstract tracking method. */ virtual void computeTracking () = 0; - + public: PCL_MAKE_ALIGNED_OPERATOR_NEW }; diff --git a/tracking/include/pcl/tracking/tracking.h b/tracking/include/pcl/tracking/tracking.h index 110e14e6..b9286065 100644 --- a/tracking/include/pcl/tracking/tracking.h +++ b/tracking/include/pcl/tracking/tracking.h @@ -41,14 +41,6 @@ #include -#ifdef BUILD_Maintainer -# if defined __GNUC__ -# pragma GCC system_header -# elif defined _MSC_VER -# pragma warning(push, 1) -# endif -#endif - namespace pcl { namespace tracking @@ -121,12 +113,6 @@ POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::tracking::_ParticleXYZR, ) POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::tracking::ParticleXYZR, pcl::tracking::_ParticleXYZR) -#ifdef BUILD_Maintainer -# if defined _MSC_VER -# pragma warning(pop) -# endif -#endif - #ifdef PCL_NO_PRECOMPILE #include #endif diff --git a/visualization/CMakeLists.txt b/visualization/CMakeLists.txt index e956bed8..990a5f5b 100644 --- a/visualization/CMakeLists.txt +++ b/visualization/CMakeLists.txt @@ -174,4 +174,3 @@ PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/vtk" ${vtk_incs}) if(BUILD_TESTS) add_subdirectory(test) endif() - diff --git a/visualization/include/pcl/visualization/boost.h b/visualization/include/pcl/visualization/boost.h index 557eff2f..7f7ec385 100644 --- a/visualization/include/pcl/visualization/boost.h +++ b/visualization/include/pcl/visualization/boost.h @@ -48,7 +48,6 @@ #include #define BOOST_PARAMETER_MAX_ARITY 7 #include -#include #include #include #include diff --git a/visualization/include/pcl/visualization/common/common.h b/visualization/include/pcl/visualization/common/common.h index 260d05ef..4f3e94d3 100644 --- a/visualization/include/pcl/visualization/common/common.h +++ b/visualization/include/pcl/visualization/common/common.h @@ -108,7 +108,7 @@ namespace pcl PCL_VISUALIZER_IMMEDIATE_RENDERING, PCL_VISUALIZER_SHADING, PCL_VISUALIZER_LUT, /**< colormap type \ref pcl::visualization::LookUpTableRepresentationProperties */ - PCL_VISUALIZER_LUT_RANGE /**< two doubles (min and max) or \ref pcl::visualization::LookUpTableRepresentationProperties::PCL_VISUALIZER_LUT_RANGE_AUTO */ + PCL_VISUALIZER_LUT_RANGE /**< two doubles (min and max) or ::PCL_VISUALIZER_LUT_RANGE_AUTO */ }; enum RenderingRepresentationProperties @@ -142,7 +142,7 @@ namespace pcl * \param[in] colormap_type * \param[out] table a vtk lookup table * \note The list of available colormaps can be found in \ref pcl::visualization::LookUpTableRepresentationProperties. - */ + */ PCL_EXPORTS bool getColormapLUT (LookUpTableRepresentationProperties colormap_type, vtkSmartPointer &table); @@ -192,36 +192,36 @@ namespace pcl /** \brief Computes View matrix for Camera (Based on gluLookAt) * \param[out] view_mat the resultant matrix */ - void + void computeViewMatrix (Eigen::Matrix4d& view_mat) const; /** \brief Computes Projection Matrix for Camera * \param[out] proj the resultant matrix */ - void + void computeProjectionMatrix (Eigen::Matrix4d& proj) const; - /** \brief converts point to window coordiantes - * \param[in] pt xyz point to be converted - * \param[out] window_cord vector containing the pts' window X,Y, Z and 1 - * - * This function computes the projection and view matrix every time. - * It is very inefficient to use this for every point in the point cloud! - */ - template void + /** + * \brief Converts point to window coordinates + * \param[in] pt xyz point to be converted + * \param[out] window_cord vector containing the pts' window X,Y, Z and 1 + * \note This function computes the projection and view matrix every time. + * It is very inefficient to use this for every point in the point cloud! + */ + template void cvtWindowCoordinates (const PointT& pt, Eigen::Vector4d& window_cord) const; - /** \brief converts point to window coordiantes - * \param[in] pt xyz point to be converted - * \param[out] window_cord vector containing the pts' window X,Y, Z and 1 - * \param[in] composite_mat composite transformation matrix (proj*view) - * - * Use this function to compute window coordinates with a precomputed - * transformation function. The typical composite matrix will be - * the projection matrix * the view matrix. However, additional - * matrices like a camera disortion matrix can also be added. - */ - template void + /** + * \brief Converts point to window coordinates + * \param[in] pt xyz point to be converted + * \param[out] window_cord vector containing the pts' window X,Y, Z and 1 + * \param[in] composite_mat composite transformation matrix (proj*view) + * + * \note Use this function to compute window coordinates with a pre-computed transformation function. + * The typical composite matrix will be the projection matrix * the view matrix. However, additional matrices like + * a camera distortion matrix can also be added. + */ + template void cvtWindowCoordinates (const PointT& pt, Eigen::Vector4d& window_cord, const Eigen::Matrix4d& composite_mat) const; }; } diff --git a/visualization/include/pcl/visualization/common/impl/common.hpp b/visualization/include/pcl/visualization/common/impl/common.hpp index de3e20c8..61f24c9a 100644 --- a/visualization/include/pcl/visualization/common/impl/common.hpp +++ b/visualization/include/pcl/visualization/common/impl/common.hpp @@ -39,12 +39,6 @@ namespace pcl { namespace visualization { - /** \brief Converts point to window coordinates - * \param[in] pt xyz point to be converted - * \param[out] window_cord vector containing the pts' window X,Y, Z and 1 - * \note This function computes the projection and view matrix every time. - * It is very inefficient to use this for every point in the point cloud! - */ template void Camera::cvtWindowCoordinates (const PointT& pt, Eigen::Vector4d& window_cord) const { @@ -55,16 +49,6 @@ namespace pcl return; } - - /** \brief converts point to window coordiantes - * \param[in] pt xyz point to be converted - * \param[out] window_cord vector containing the pts' window X,Y, Z and 1 - * \param[in] composite_mat composite transformation matrix (proj*view) - * - * \note Use this function to compute window coordinates with a pre-computed transformation function. - * The typical composite matrix will be the projection matrix * the view matrix. However, additional matrices like - * a camera distortion matrix can also be added. - */ template void Camera::cvtWindowCoordinates (const PointT& pt, Eigen::Vector4d& window_cord, const Eigen::Matrix4d& composite_mat) const { diff --git a/visualization/include/pcl/visualization/common/impl/shapes.hpp b/visualization/include/pcl/visualization/common/impl/shapes.hpp index 5579ea55..e9acf5db 100644 --- a/visualization/include/pcl/visualization/common/impl/shapes.hpp +++ b/visualization/include/pcl/visualization/common/impl/shapes.hpp @@ -37,14 +37,21 @@ */ #pragma once + #include #include #include #include -//////////////////////////////////////////////////////////////////////////////////////////// + +namespace pcl +{ + +namespace visualization +{ + template vtkSmartPointer -pcl::visualization::createPolygon (const typename pcl::PointCloud::ConstPtr &cloud) +createPolygon (const typename pcl::PointCloud::ConstPtr &cloud) { vtkSmartPointer poly_grid; if (cloud->points.empty ()) @@ -70,9 +77,9 @@ pcl::visualization::createPolygon (const typename pcl::PointCloud::Const return (poly_grid); } -//////////////////////////////////////////////////////////////////////////////////////////// + template vtkSmartPointer -pcl::visualization::createPolygon (const pcl::PlanarPolygon &planar_polygon) +createPolygon (const pcl::PlanarPolygon &planar_polygon) { vtkSmartPointer poly_grid; if (planar_polygon.getContour ().empty ()) @@ -86,19 +93,19 @@ pcl::visualization::createPolygon (const pcl::PlanarPolygon &planar_poly for (std::size_t i = 0; i < planar_polygon.getContour ().size (); ++i) { - poly_points->SetPoint (i, planar_polygon.getContour ()[i].x, - planar_polygon.getContour ()[i].y, + poly_points->SetPoint (i, planar_polygon.getContour ()[i].x, + planar_polygon.getContour ()[i].y, planar_polygon.getContour ()[i].z); polygon->GetPointIds ()->SetId (i, i); } std::size_t closingContourId = planar_polygon.getContour ().size (); auto firstContour = planar_polygon.getContour ()[0]; - poly_points->SetPoint (closingContourId, firstContour.x, - firstContour.y, + poly_points->SetPoint (closingContourId, firstContour.x, + firstContour.y, firstContour.z); polygon->GetPointIds ()->SetId (closingContourId, closingContourId); - + allocVtkUnstructuredGrid (poly_grid); poly_grid->Allocate (1, 1); poly_grid->InsertNextCell (polygon->GetCellType (), polygon->GetPointIds ()); @@ -107,3 +114,6 @@ pcl::visualization::createPolygon (const pcl::PlanarPolygon &planar_poly return (poly_grid); } +} // namespace visualization +} // namespace pcl + diff --git a/visualization/include/pcl/visualization/image_viewer.h b/visualization/include/pcl/visualization/image_viewer.h index ff268cbc..8d01ba02 100644 --- a/visualization/include/pcl/visualization/image_viewer.h +++ b/visualization/include/pcl/visualization/image_viewer.h @@ -38,6 +38,7 @@ #pragma once +#include #include #include #include diff --git a/visualization/include/pcl/visualization/impl/histogram_visualizer.hpp b/visualization/include/pcl/visualization/impl/histogram_visualizer.hpp index df64ae17..b0358252 100644 --- a/visualization/include/pcl/visualization/impl/histogram_visualizer.hpp +++ b/visualization/include/pcl/visualization/impl/histogram_visualizer.hpp @@ -41,10 +41,16 @@ #include -//////////////////////////////////////////////////////////////////////////////////////////// + +namespace pcl +{ + +namespace visualization +{ + template bool -pcl::visualization::PCLHistogramVisualizer::addFeatureHistogram ( - const pcl::PointCloud &cloud, int hsize, +PCLHistogramVisualizer::addFeatureHistogram ( + const pcl::PointCloud &cloud, int hsize, const std::string &id, int win_width, int win_height) { RenWinInteractMap::iterator am_it = wins_.find (id); @@ -75,12 +81,12 @@ pcl::visualization::PCLHistogramVisualizer::addFeatureHistogram ( return (true); } -//////////////////////////////////////////////////////////////////////////////////////////// + template bool -pcl::visualization::PCLHistogramVisualizer::addFeatureHistogram ( - const pcl::PointCloud &cloud, +PCLHistogramVisualizer::addFeatureHistogram ( + const pcl::PointCloud &cloud, const std::string &field_name, - const int index, + const int index, const std::string &id, int win_width, int win_height) { if (index < 0 || index >= cloud.points.size ()) @@ -129,10 +135,10 @@ pcl::visualization::PCLHistogramVisualizer::addFeatureHistogram ( return (true); } -//////////////////////////////////////////////////////////////////////////////////////////// + template bool -pcl::visualization::PCLHistogramVisualizer::updateFeatureHistogram ( - const pcl::PointCloud &cloud, int hsize, +PCLHistogramVisualizer::updateFeatureHistogram ( + const pcl::PointCloud &cloud, int hsize, const std::string &id) { RenWinInteractMap::iterator am_it = wins_.find (id); @@ -142,11 +148,11 @@ pcl::visualization::PCLHistogramVisualizer::updateFeatureHistogram ( return (false); } RenWinInteract* renwinupd = &wins_[id]; - + vtkSmartPointer xy_array = vtkSmartPointer::New (); xy_array->SetNumberOfComponents (2); xy_array->SetNumberOfTuples (hsize); - + // Parse the cloud data and store it in the array double xy[2]; for (int d = 0; d < hsize; ++d) @@ -159,10 +165,10 @@ pcl::visualization::PCLHistogramVisualizer::updateFeatureHistogram ( return (true); } -//////////////////////////////////////////////////////////////////////////////////////////// + template bool -pcl::visualization::PCLHistogramVisualizer::updateFeatureHistogram ( - const pcl::PointCloud &cloud, const std::string &field_name, const int index, +PCLHistogramVisualizer::updateFeatureHistogram ( + const pcl::PointCloud &cloud, const std::string &field_name, const int index, const std::string &id) { if (index < 0 || index >= cloud.points.size ()) @@ -170,7 +176,7 @@ pcl::visualization::PCLHistogramVisualizer::updateFeatureHistogram ( PCL_ERROR ("[updateFeatureHistogram] Invalid point index (%d) given!\n", index); return (false); } - + // Get the fields present in this cloud std::vector fields; // Check if our field exists @@ -188,7 +194,7 @@ pcl::visualization::PCLHistogramVisualizer::updateFeatureHistogram ( return (false); } RenWinInteract* renwinupd = &wins_[id]; - + vtkSmartPointer xy_array = vtkSmartPointer::New (); xy_array->SetNumberOfComponents (2); xy_array->SetNumberOfTuples (fields[field_idx].count); @@ -204,10 +210,13 @@ pcl::visualization::PCLHistogramVisualizer::updateFeatureHistogram ( xy[1] = data; xy_array->SetTuple (d, xy); } - + reCreateActor (xy_array, renwinupd, cloud.fields[field_idx].count - 1); return (true); } +} // namespace visualization +} // namespace pcl + #endif diff --git a/visualization/include/pcl/visualization/impl/pcl_plotter.hpp b/visualization/include/pcl/visualization/impl/pcl_plotter.hpp index 1d2d4511..20ac237e 100644 --- a/visualization/include/pcl/visualization/impl/pcl_plotter.hpp +++ b/visualization/include/pcl/visualization/impl/pcl_plotter.hpp @@ -38,20 +38,27 @@ #ifndef PCL_VISUALUALIZATION_PCL_PLOTTER_IMPL_H_ #define PCL_VISUALUALIZATION_PCL_PLOTTER_IMPL_H_ + +namespace pcl +{ + +namespace visualization +{ + template bool -pcl::visualization::PCLPlotter::addFeatureHistogram ( - const pcl::PointCloud &cloud, int hsize, +PCLPlotter::addFeatureHistogram ( + const pcl::PointCloud &cloud, int hsize, const std::string &id, int win_width, int win_height) { std::vector array_x(hsize), array_y(hsize); - + // Parse the cloud data and store it in the array for (int i = 0; i < hsize; ++i) { array_x[i] = i; array_y[i] = cloud.points[0].histogram[i]; } - + this->addPlotData(array_x, array_y, id.c_str(), vtkChart::LINE); setWindowSize (win_width, win_height); return true; @@ -59,10 +66,10 @@ pcl::visualization::PCLPlotter::addFeatureHistogram ( template bool -pcl::visualization::PCLPlotter::addFeatureHistogram ( - const pcl::PointCloud &cloud, +PCLPlotter::addFeatureHistogram ( + const pcl::PointCloud &cloud, const std::string &field_name, - const int index, + const int index, const std::string &id, int win_width, int win_height) { if (index < 0 || index >= cloud.points.size ()) @@ -83,7 +90,7 @@ pcl::visualization::PCLPlotter::addFeatureHistogram ( int hsize = fields[field_idx].count; std::vector array_x (hsize), array_y (hsize); - + for (int i = 0; i < hsize; ++i) { array_x[i] = i; @@ -92,11 +99,14 @@ pcl::visualization::PCLPlotter::addFeatureHistogram ( memcpy (&data, reinterpret_cast (&cloud.points[index]) + fields[field_idx].offset + i * sizeof (float), sizeof (float)); array_y[i] = data; } - + this->addPlotData(array_x, array_y, id.c_str(), vtkChart::LINE); setWindowSize (win_width, win_height); return (true); } +} // namespace visualization +} // namespace pcl + #endif /* PCL_VISUALUALIZATION_PCL_PLOTTER_IMPL_H_ */ diff --git a/visualization/include/pcl/visualization/impl/pcl_visualizer.hpp b/visualization/include/pcl/visualization/impl/pcl_visualizer.hpp index b7bfcfc5..4b4900e3 100644 --- a/visualization/include/pcl/visualization/impl/pcl_visualizer.hpp +++ b/visualization/include/pcl/visualization/impl/pcl_visualizer.hpp @@ -264,6 +264,7 @@ pcl::visualization::PCLVisualizer::convertPointCloudToVTKPolyData ( std::copy (&cloud->points[i].x, &cloud->points[i].x + 3, &data[ptr]); j++; + ptr += 3; } nr_points = j; points->SetNumberOfPoints (nr_points); @@ -1477,6 +1478,8 @@ pcl::visualization::PCLVisualizer::updatePointCloud (const typename pcl::PointCl return (false); vtkSmartPointer polydata = reinterpret_cast(am_it->second.actor->GetMapper ())->GetInput (); + if (!polydata) + return false; // Convert the PointCloud to VTK PolyData convertPointCloudToVTKPolyData (cloud, polydata, am_it->second.cells); @@ -1584,6 +1587,8 @@ pcl::visualization::PCLVisualizer::updatePointCloud (const typename pcl::PointCl // Set the cells and the vertices vertices->SetCells (nr_points, cells); + // Set the cell count explicitly as the array doesn't get modified enough so the above method updates accordingly. See #4001 and #3452 + vertices->SetNumberOfCells(nr_points); // Get the colors from the handler bool has_colors = false; 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 774ddfda..c900f967 100644 --- a/visualization/include/pcl/visualization/impl/point_cloud_color_handlers.hpp +++ b/visualization/include/pcl/visualization/impl/point_cloud_color_handlers.hpp @@ -35,18 +35,24 @@ * */ -#ifndef PCL_POINT_CLOUD_COLOR_HANDLERS_IMPL_HPP_ -#define PCL_POINT_CLOUD_COLOR_HANDLERS_IMPL_HPP_ +#pragma once #include #include #include #include +#include // for pcl::isFinite + + +namespace pcl +{ + +namespace visualization +{ -/////////////////////////////////////////////////////////////////////////////////////////// template vtkSmartPointer -pcl::visualization::PointCloudColorHandlerCustom::getColor () const +PointCloudColorHandlerCustom::getColor () const { if (!capable_ || !cloud_) return nullptr; @@ -71,9 +77,9 @@ pcl::visualization::PointCloudColorHandlerCustom::getColor () const return scalars; } -/////////////////////////////////////////////////////////////////////////////////////////// + template vtkSmartPointer -pcl::visualization::PointCloudColorHandlerRandom::getColor () const +PointCloudColorHandlerRandom::getColor () const { if (!capable_ || !cloud_) return nullptr; @@ -89,8 +95,8 @@ pcl::visualization::PointCloudColorHandlerRandom::getColor () const double r, g, b; pcl::visualization::getRandomColors (r, g, b); - int r_ = static_cast (pcl_lrint (r * 255.0)), - g_ = static_cast (pcl_lrint (g * 255.0)), + int r_ = static_cast (pcl_lrint (r * 255.0)), + g_ = static_cast (pcl_lrint (g * 255.0)), b_ = static_cast (pcl_lrint (b * 255.0)); // Color every point @@ -104,9 +110,9 @@ pcl::visualization::PointCloudColorHandlerRandom::getColor () const return scalars; } -/////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::visualization::PointCloudColorHandlerRGBField::setInputCloud ( +PointCloudColorHandlerRGBField::setInputCloud ( const PointCloudConstPtr &cloud) { PointCloudColorHandler::setInputCloud (cloud); @@ -127,9 +133,9 @@ pcl::visualization::PointCloudColorHandlerRGBField::setInputCloud ( } } -/////////////////////////////////////////////////////////////////////////////////////////// + template vtkSmartPointer -pcl::visualization::PointCloudColorHandlerRGBField::getColor () const +PointCloudColorHandlerRGBField::getColor () const { if (!capable_ || !cloud_) return nullptr; @@ -165,7 +171,7 @@ pcl::visualization::PointCloudColorHandlerRGBField::getColor () const { // Copy the value at the specified field if (!std::isfinite (cloud_->points[cp].x) || - !std::isfinite (cloud_->points[cp].y) || + !std::isfinite (cloud_->points[cp].y) || !std::isfinite (cloud_->points[cp].z)) continue; @@ -191,10 +197,10 @@ pcl::visualization::PointCloudColorHandlerRGBField::getColor () const return scalars; } -/////////////////////////////////////////////////////////////////////////////////////////// + template -pcl::visualization::PointCloudColorHandlerHSVField::PointCloudColorHandlerHSVField (const PointCloudConstPtr &cloud) : - pcl::visualization::PointCloudColorHandler::PointCloudColorHandler (cloud) +PointCloudColorHandlerHSVField::PointCloudColorHandlerHSVField (const PointCloudConstPtr &cloud) : + PointCloudColorHandler::PointCloudColorHandler (cloud) { // Check for the presence of the "H" field field_idx_ = pcl::getFieldIndex ("h", fields_); @@ -222,9 +228,9 @@ pcl::visualization::PointCloudColorHandlerHSVField::PointCloudColorHandl capable_ = true; } -/////////////////////////////////////////////////////////////////////////////////////////// + template vtkSmartPointer -pcl::visualization::PointCloudColorHandlerHSVField::getColor () const +PointCloudColorHandlerHSVField::getColor () const { if (!capable_ || !cloud_) return nullptr; @@ -359,9 +365,9 @@ pcl::visualization::PointCloudColorHandlerHSVField::getColor () const return scalars; } -/////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::visualization::PointCloudColorHandlerGenericField::setInputCloud ( +PointCloudColorHandlerGenericField::setInputCloud ( const PointCloudConstPtr &cloud) { PointCloudColorHandler::setInputCloud (cloud); @@ -372,9 +378,9 @@ pcl::visualization::PointCloudColorHandlerGenericField::setInputCloud ( capable_ = false; } -/////////////////////////////////////////////////////////////////////////////////////////// + template vtkSmartPointer -pcl::visualization::PointCloudColorHandlerGenericField::getColor () const +PointCloudColorHandlerGenericField::getColor () const { if (!capable_ || !cloud_) return nullptr; @@ -432,9 +438,9 @@ pcl::visualization::PointCloudColorHandlerGenericField::getColor () cons return scalars; } -/////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::visualization::PointCloudColorHandlerRGBAField::setInputCloud ( +PointCloudColorHandlerRGBAField::setInputCloud ( const PointCloudConstPtr &cloud) { PointCloudColorHandler::setInputCloud (cloud); @@ -446,9 +452,9 @@ pcl::visualization::PointCloudColorHandlerRGBAField::setInputCloud ( capable_ = false; } -/////////////////////////////////////////////////////////////////////////////////////////// + template vtkSmartPointer -pcl::visualization::PointCloudColorHandlerRGBAField::getColor () const +PointCloudColorHandlerRGBAField::getColor () const { if (!capable_ || !cloud_) return nullptr; @@ -500,9 +506,9 @@ pcl::visualization::PointCloudColorHandlerRGBAField::getColor () const return scalars; } -/////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::visualization::PointCloudColorHandlerLabelField::setInputCloud (const PointCloudConstPtr &cloud) +PointCloudColorHandlerLabelField::setInputCloud (const PointCloudConstPtr &cloud) { PointCloudColorHandler::setInputCloud (cloud); field_idx_ = pcl::getFieldIndex ("label", fields_); @@ -513,9 +519,9 @@ pcl::visualization::PointCloudColorHandlerLabelField::setInputCloud (con } } -/////////////////////////////////////////////////////////////////////////////////////////// + template vtkSmartPointer -pcl::visualization::PointCloudColorHandlerLabelField::getColor () const +PointCloudColorHandlerLabelField::getColor () const { if (!capable_ || !cloud_) return nullptr; @@ -558,5 +564,6 @@ pcl::visualization::PointCloudColorHandlerLabelField::getColor () const return scalars; } -#endif // PCL_POINT_CLOUD_COLOR_HANDLERS_IMPL_HPP_ +} // namespace visualization +} // namespace pcl diff --git a/visualization/include/pcl/visualization/impl/point_cloud_geometry_handlers.hpp b/visualization/include/pcl/visualization/impl/point_cloud_geometry_handlers.hpp index 7b84fe5d..11d44894 100644 --- a/visualization/include/pcl/visualization/impl/point_cloud_geometry_handlers.hpp +++ b/visualization/include/pcl/visualization/impl/point_cloud_geometry_handlers.hpp @@ -36,15 +36,22 @@ * $Id: point_cloud_handlers.hpp 7678 2012-10-22 20:54:04Z rusu $ * */ + #ifndef PCL_POINT_CLOUD_GEOMETRY_HANDLERS_IMPL_HPP_ #define PCL_POINT_CLOUD_GEOMETRY_HANDLERS_IMPL_HPP_ #include -/////////////////////////////////////////////////////////////////////////////////////////// + +namespace pcl +{ + +namespace visualization +{ + template -pcl::visualization::PointCloudGeometryHandlerXYZ::PointCloudGeometryHandlerXYZ (const PointCloudConstPtr &cloud) - : pcl::visualization::PointCloudGeometryHandler::PointCloudGeometryHandler (cloud) +PointCloudGeometryHandlerXYZ::PointCloudGeometryHandlerXYZ (const PointCloudConstPtr &cloud) + : PointCloudGeometryHandler::PointCloudGeometryHandler (cloud) { field_x_idx_ = pcl::getFieldIndex ("x", fields_); if (field_x_idx_ == -1) @@ -58,9 +65,9 @@ pcl::visualization::PointCloudGeometryHandlerXYZ::PointCloudGeometryHand capable_ = true; } -/////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::visualization::PointCloudGeometryHandlerXYZ::getGeometry (vtkSmartPointer &points) const + +template void +PointCloudGeometryHandlerXYZ::getGeometry (vtkSmartPointer &points) const { if (!capable_) return; @@ -109,10 +116,10 @@ pcl::visualization::PointCloudGeometryHandlerXYZ::getGeometry (vtkSmartP } } -/////////////////////////////////////////////////////////////////////////////////////////// + template -pcl::visualization::PointCloudGeometryHandlerSurfaceNormal::PointCloudGeometryHandlerSurfaceNormal (const PointCloudConstPtr &cloud) - : pcl::visualization::PointCloudGeometryHandler::PointCloudGeometryHandler (cloud) +PointCloudGeometryHandlerSurfaceNormal::PointCloudGeometryHandlerSurfaceNormal (const PointCloudConstPtr &cloud) + : PointCloudGeometryHandler::PointCloudGeometryHandler (cloud) { field_x_idx_ = pcl::getFieldIndex ("normal_x", fields_); if (field_x_idx_ == -1) @@ -126,9 +133,9 @@ pcl::visualization::PointCloudGeometryHandlerSurfaceNormal::PointCloudGe capable_ = true; } -/////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::visualization::PointCloudGeometryHandlerSurfaceNormal::getGeometry (vtkSmartPointer &points) const + +template void +PointCloudGeometryHandlerSurfaceNormal::getGeometry (vtkSmartPointer &points) const { if (!capable_) return; @@ -150,6 +157,9 @@ pcl::visualization::PointCloudGeometryHandlerSurfaceNormal::getGeometry } } +} // namespace visualization +} // namespace pcl + #define PCL_INSTANTIATE_PointCloudGeometryHandlerXYZ(T) template class PCL_EXPORTS pcl::visualization::PointCloudGeometryHandlerXYZ; #define PCL_INSTANTIATE_PointCloudGeometryHandlerSurfaceNormal(T) template class PCL_EXPORTS pcl::visualization::PointCloudGeometryHandlerSurfaceNormal; diff --git a/visualization/include/pcl/visualization/impl/registration_visualizer.hpp b/visualization/include/pcl/visualization/impl/registration_visualizer.hpp index dab4491b..2b076c48 100644 --- a/visualization/include/pcl/visualization/impl/registration_visualizer.hpp +++ b/visualization/include/pcl/visualization/impl/registration_visualizer.hpp @@ -36,27 +36,32 @@ * */ +#pragma once + #include -////////////////////////////////////////////////////////////////////////////////////////////// + +namespace pcl +{ + template void -pcl::RegistrationVisualizer::startDisplay () +RegistrationVisualizer::startDisplay () { // Create and start the rendering thread. This will open the display window. viewer_thread_ = std::thread (&pcl::RegistrationVisualizer::runDisplay, this); } -////////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::RegistrationVisualizer::stopDisplay () +RegistrationVisualizer::stopDisplay () { // Stop the rendering thread. This will kill the display window. viewer_thread_.~thread (); } -////////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::RegistrationVisualizer::runDisplay () +RegistrationVisualizer::runDisplay () { // Open 3D viewer viewer_ @@ -179,9 +184,9 @@ pcl::RegistrationVisualizer::runDisplay () } } -////////////////////////////////////////////////////////////////////////////////////////////// + template void -pcl::RegistrationVisualizer::updateIntermediateCloud ( +RegistrationVisualizer::updateIntermediateCloud ( const pcl::PointCloud &cloud_src, const std::vector &indices_src, const pcl::PointCloud &cloud_tgt, @@ -212,3 +217,6 @@ pcl::RegistrationVisualizer::updateIntermediateCloud ( // Unlock local buffers visualizer_updating_mutex_.unlock (); } + +} // namespace pcl + diff --git a/visualization/include/pcl/visualization/pcl_painter2D.h b/visualization/include/pcl/visualization/pcl_painter2D.h index a74de876..f4180e94 100644 --- a/visualization/include/pcl/visualization/pcl_painter2D.h +++ b/visualization/include/pcl/visualization/pcl_painter2D.h @@ -98,7 +98,7 @@ namespace pcl this->transform_->SetMatrix (t->GetMatrix()); } - void applyInternals (vtkContext2D *painter) + void applyInternals (vtkContext2D *painter) const { painter->ApplyPen (pen_); painter->ApplyBrush (brush_); @@ -397,7 +397,7 @@ namespace pcl * \return[in] array containing the width and height of the window */ int * - getWindowSize (); + getWindowSize () const; /** \brief displays all the figures added in a window. */ diff --git a/visualization/include/pcl/visualization/pcl_plotter.h b/visualization/include/pcl/visualization/pcl_plotter.h index 7e0f4c32..3aeeb585 100644 --- a/visualization/include/pcl/visualization/pcl_plotter.h +++ b/visualization/include/pcl/visualization/pcl_plotter.h @@ -373,7 +373,7 @@ namespace pcl * \return[in] array containing the width and height of the window */ int* - getWindowSize (); + getWindowSize () const; /** \brief Return a pointer to the underlying VTK RenderWindow used. */ vtkSmartPointer diff --git a/visualization/include/pcl/visualization/pcl_visualizer.h b/visualization/include/pcl/visualization/pcl_visualizer.h index 4348c69a..908b9de9 100644 --- a/visualization/include/pcl/visualization/pcl_visualizer.h +++ b/visualization/include/pcl/visualization/pcl_visualizer.h @@ -103,7 +103,7 @@ namespace pcl */ PCLVisualizer (const std::string &name = "", const bool create_interactor = true); - /** \brief PCL Visualizer constructor. It looks through the passed argv arguments to find the "-cam *.cam" argument. + /** \brief PCL Visualizer constructor. It looks through the passed argv arguments to find the "-cam *.cam" argument. * If the search failed, the name for cam file is calculated with boost uuid. If there is no such file, camera is not initilalized. * \param[in] argc * \param[in] argv @@ -113,10 +113,10 @@ namespace pcl */ PCLVisualizer (int &argc, char **argv, const std::string &name = "", PCLVisualizerInteractorStyle* style = PCLVisualizerInteractorStyle::New (), const bool create_interactor = true); - + /** \brief PCL Visualizer constructor. - * \param[in] custom vtk renderer - * \param[in] custom vtk render window + * \param[in] ren custom vtk renderer + * \param[in] wind custom vtk render window * \param[in] create_interactor if true (default), create an interactor, false otherwise */ PCLVisualizer (vtkSmartPointer ren, vtkSmartPointer wind, const std::string &name = "", const bool create_interactor = true); @@ -124,8 +124,8 @@ namespace pcl /** \brief PCL Visualizer constructor. * \param[in] argc * \param[in] argv - * \param[in] custom vtk renderer - * \param[in] custom vtk render window + * \param[in] ren custom vtk renderer + * \param[in] wind custom vtk render window * \param[in] style interactor style (defaults to PCLVisualizerInteractorStyle) * \param[in] create_interactor if true (default), create an interactor, false otherwise */ @@ -286,11 +286,11 @@ namespace pcl spinOnce (int time = 1, bool force_redraw = false); /** \brief Adds a widget which shows an interactive axes display for orientation - * \param[in] interactor - Pointer to the vtk interactor object used by the PCLVisualizer window + * \param[in] interactor - Pointer to the vtk interactor object used by the PCLVisualizer window */ void addOrientationMarkerWidgetAxes (vtkRenderWindowInteractor* interactor); - + /** \brief Disables the Orientatation Marker Widget so it is removed from the renderer */ void removeOrientationMarkerWidgetAxes (); @@ -466,7 +466,7 @@ namespace pcl /** \brief Update a text to screen * \param[in] text the text to update * \param[in] xpos the new X position on screen - * \param[in] ypos the new Y position on screen + * \param[in] ypos the new Y position on screen * \param[in] id the text object id (default: equal to the "text" parameter) */ bool @@ -477,21 +477,21 @@ namespace pcl /** \brief Update a text to screen * \param[in] text the text to update * \param[in] xpos the new X position on screen - * \param[in] ypos the new Y position on screen + * \param[in] ypos the new Y position on screen * \param[in] r the red color value * \param[in] g the green color value * \param[in] b the blue color value * \param[in] id the text object id (default: equal to the "text" parameter) */ bool - updateText (const std::string &text, + updateText (const std::string &text, int xpos, int ypos, double r, double g, double b, const std::string &id = ""); /** \brief Update a text to screen * \param[in] text the text to update * \param[in] xpos the new X position on screen - * \param[in] ypos the new Y position on screen + * \param[in] ypos the new Y position on screen * \param[in] fontsize the fontsize of the text * \param[in] r the red color value * \param[in] g the green color value @@ -499,13 +499,13 @@ namespace pcl * \param[in] id the text object id (default: equal to the "text" parameter) */ bool - updateText (const std::string &text, + updateText (const std::string &text, int xpos, int ypos, int fontsize, double r, double g, double b, const std::string &id = ""); - /** \brief Set the pose of an existing shape. - * - * Returns false if the shape doesn't exist, true if the pose was successfully + /** \brief Set the pose of an existing shape. + * + * Returns false if the shape doesn't exist, true if the pose was successfully * updated. * * \param[in] id the shape or cloud object id (i.e., given on \a addLine etc.) @@ -630,7 +630,7 @@ namespace pcl const typename pcl::PointCloud::ConstPtr &pcs, int level = 100, float scale = 1.0f, const std::string &id = "cloud", int viewport = 0); - + /** \brief Add the estimated principal curvatures of a Point Cloud to screen. * \param[in] cloud the input point cloud dataset containing the XYZ data * \param[in] normals the input point cloud dataset containing the normal data @@ -1067,7 +1067,7 @@ namespace pcl int viewport = 0) { // If Nth not given, display all correspondences - return (addCorrespondences (source_points, target_points, + return (addCorrespondences (source_points, target_points, correspondences, 1, id, viewport)); } @@ -1158,7 +1158,7 @@ namespace pcl bool setPointCloudRenderingProperties (int property, double val1, double val2, const std::string &id = "cloud", int viewport = 0); - + /** \brief Set the rendering properties of a PointCloud * \param[in] property the property type * \param[in] value the value to be set @@ -1179,7 +1179,7 @@ namespace pcl bool getPointCloudRenderingProperties (int property, double &value, const std::string &id = "cloud"); - + /** \brief Get the rendering properties of a PointCloud * \param[in] property the property type * \param[out] val1 the resultant property value @@ -1193,13 +1193,13 @@ namespace pcl getPointCloudRenderingProperties (RenderingProperties property, double &val1, double &val2, double &val3, const std::string &id = "cloud"); - /** \brief Set whether the point cloud is selected or not + /** \brief Set whether the point cloud is selected or not * \param[in] selected whether the cloud is selected or not (true = selected) * \param[in] id the point cloud object id (default: cloud) */ bool setPointCloudSelected (const bool selected, const std::string &id = "cloud" ); - + /** \brief Set the rendering properties of a shape * \param[in] property the property type * \param[in] value the value to be set @@ -1256,8 +1256,8 @@ namespace pcl * \param[in] ymax the maximum Y coordinate for the viewport (0.0 <= 1.0) * \param[in] viewport the id of the new viewport * - * \note If no renderer for the current window exists, one will be created, and - * the viewport will be set to 0 ('all'). In case one or multiple renderers do + * \note If no renderer for the current window exists, one will be created, and + * the viewport will be set to 0 ('all'). In case one or multiple renderers do * exist, the viewport ID will be set to the total number of renderers - 1. */ void @@ -1780,7 +1780,7 @@ namespace pcl /** \brief Get camera file for camera parameter saving/restoring. * \note This will be valid only when valid "-cam" option were available in command line - * or a saved camera file were automatically loaded. + * or a saved camera file were automatically loaded. * \sa cameraParamsSet (), cameraFileLoaded () */ std::string @@ -1892,21 +1892,21 @@ namespace pcl { return (win_); } - + /** \brief Return a pointer to the underlying VTK Renderer Collection. */ vtkSmartPointer getRendererCollection () { return (rens_); } - + /** \brief Return a pointer to the CloudActorMap this visualizer uses. */ CloudActorMapPtr getCloudActorMap () { return (cloud_actor_map_); } - + /** \brief Return a pointer to the ShapeActorMap this visualizer uses. */ ShapeActorMapPtr getShapeActorMap () @@ -1931,7 +1931,7 @@ namespace pcl /** \brief Use Vertex Buffer Objects renderers. * This is an optimization for the obsolete OpenGL backend. Modern OpenGL2 backend (VTK ≥ 6.3) uses vertex * buffer objects by default, transparently for the user. - * \param[in] use_vbos set to true to use VBOs + * \param[in] use_vbos set to true to use VBOs */ void setUseVbos (bool use_vbos); @@ -1959,13 +1959,13 @@ namespace pcl * attached to a given vtkRenderWindow * \param[in,out] iren the vtkRenderWindowInteractor object to set up * \param[in,out] win a vtkRenderWindow object that the interactor is attached to - * \param[in,out] style a vtkInteractorStyle object + * \param[in,out] style a vtkInteractorStyle object */ void setupInteractor (vtkRenderWindowInteractor *iren, vtkRenderWindow *win, vtkInteractorStyle *style); - + /** \brief Get a pointer to the current interactor style used. */ inline vtkSmartPointer getInteractorStyle () @@ -2013,7 +2013,7 @@ namespace pcl { return (new ExitMainLoopTimerCallback); } - void + void Execute (vtkObject*, unsigned long event_id, void*) override; int right_timer_id; @@ -2026,7 +2026,7 @@ namespace pcl { return (new ExitCallback); } - void + void Execute (vtkObject*, unsigned long event_id, void*) override; PCLVisualizer* pcl_visualizer; @@ -2041,7 +2041,7 @@ namespace pcl FPSCallback (const FPSCallback& src) : vtkCommand (src), actor (src.actor), pcl_visualizer (src.pcl_visualizer), decimated (src.decimated), last_fps (src.last_fps) {} FPSCallback& operator = (const FPSCallback& src) { actor = src.actor; pcl_visualizer = src.pcl_visualizer; decimated = src.decimated; last_fps = src.last_fps; return (*this); } - void + void Execute (vtkObject*, unsigned long event_id, void*) override; vtkTextActor *actor; @@ -2083,7 +2083,7 @@ namespace pcl /** \brief Internal pointer to widget which contains a set of axes */ vtkSmartPointer axes_widget_; - + /** \brief Boolean that holds whether or not the camera parameters were manually initialized */ bool camera_set_; @@ -2113,7 +2113,7 @@ namespace pcl * \param[in] actor a pointer to the vtk actor object * \param[in] viewport port where the actor should be added to (default: 0/all) * - * \note If viewport is set to 0, the actor will be added to all existing + * \note If viewport is set to 0, the actor will be added to all existing * renders. To select a specific viewport use an integer between 1 and N. */ void @@ -2136,7 +2136,7 @@ namespace pcl void createActorFromVTKDataSet (const vtkSmartPointer &data, vtkSmartPointer &actor, - bool use_scalars = true); + bool use_scalars = true) const; /** \brief Internal method. Creates a vtk actor from a vtk polydata object. * \param[in] data the vtk polydata object to create an actor for @@ -2146,7 +2146,7 @@ namespace pcl void createActorFromVTKDataSet (const vtkSmartPointer &data, vtkSmartPointer &actor, - bool use_scalars = true); + bool use_scalars = true) const; /** \brief Converts a PCL templated PointCloud object to a vtk polydata object. * \param[in] cloud the input PCL PointCloud dataset @@ -2298,7 +2298,7 @@ namespace pcl * \param[in] tex_mat texture material in PCL format * \param[out] vtk_tex texture material in VTK format * \return 0 on success and -1 else. - * \note for now only image based textures are supported, image file must be in + * \note for now only image based textures are supported, image file must be in * tex_file attribute of \a tex_mat. */ int @@ -2311,7 +2311,7 @@ namespace pcl */ std::string getUniqueCameraFile (int argc, char **argv); - + //There's no reason these conversion functions shouldn't be public and static so others can use them. public: /** \brief Convert Eigen::Matrix4f to vtkMatrix4x4 @@ -2331,7 +2331,7 @@ namespace pcl convertToVtkMatrix (const Eigen::Vector4f &origin, const Eigen::Quaternion &orientation, vtkSmartPointer &vtk_matrix); - + /** \brief Convert vtkMatrix4x4 to an Eigen4f * \param[in] vtk_matrix the original VTK 4x4 matrix * \param[out] m the resultant Eigen 4x4 matrix diff --git a/visualization/include/pcl/visualization/point_cloud_color_handlers.h b/visualization/include/pcl/visualization/point_cloud_color_handlers.h index 24310909..62bb8ce1 100644 --- a/visualization/include/pcl/visualization/point_cloud_color_handlers.h +++ b/visualization/include/pcl/visualization/point_cloud_color_handlers.h @@ -114,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. */ - PCL_DEPRECATED("use getColor() without parameters instead") + PCL_DEPRECATED(1, 12, "use getColor() without parameters instead") virtual bool getColor (vtkSmartPointer &scalars) const { scalars = getColor (); @@ -616,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. */ - PCL_DEPRECATED("use getColor() without parameters instead") + PCL_DEPRECATED(1, 12, "use getColor() without parameters instead") virtual bool getColor (vtkSmartPointer &scalars) const { scalars = getColor (); diff --git a/visualization/include/pcl/visualization/point_picking_event.h b/visualization/include/pcl/visualization/point_picking_event.h index 5720a5ad..38eee9e2 100644 --- a/visualization/include/pcl/visualization/point_picking_event.h +++ b/visualization/include/pcl/visualization/point_picking_event.h @@ -71,7 +71,7 @@ namespace pcl performSinglePick (vtkRenderWindowInteractor *iren, float &x, float &y, float &z); int - performAreaPick (vtkRenderWindowInteractor *iren, std::vector &indices); + performAreaPick (vtkRenderWindowInteractor *iren, std::vector &indices) const; private: float x_, y_, z_; diff --git a/visualization/include/pcl/visualization/vtk/vtkVertexBufferObject.h b/visualization/include/pcl/visualization/vtk/vtkVertexBufferObject.h index af6d24ca..270abc84 100644 --- a/visualization/include/pcl/visualization/vtk/vtkVertexBufferObject.h +++ b/visualization/include/pcl/visualization/vtk/vtkVertexBufferObject.h @@ -42,6 +42,7 @@ class vtkUnsignedCharArray; class vtkOpenGLExtensionManager; class vtkRenderWindow; +PCL_DEPRECATED(1, 13, "The OpenGL backend of VTK is deprecated. Please switch to the OpenGL2 backend.") class PCL_EXPORTS vtkVertexBufferObject : public vtkObject { public: diff --git a/visualization/include/pcl/visualization/vtk/vtkVertexBufferObjectMapper.h b/visualization/include/pcl/visualization/vtk/vtkVertexBufferObjectMapper.h index 3bc4f9f1..bfe8efff 100644 --- a/visualization/include/pcl/visualization/vtk/vtkVertexBufferObjectMapper.h +++ b/visualization/include/pcl/visualization/vtk/vtkVertexBufferObjectMapper.h @@ -22,6 +22,7 @@ #pragma once #include +#include #include "vtkMapper.h" #include "vtkSmartPointer.h" @@ -34,6 +35,7 @@ class vtkShader2; class vtkShaderProgram2; class vtkVertexBufferObject; +PCL_DEPRECATED(1, 13, "The OpenGL backend of VTK is deprecated. Please switch to the OpenGL2 backend.") class PCL_EXPORTS vtkVertexBufferObjectMapper : public vtkMapper { public: diff --git a/visualization/src/cloud_viewer.cpp b/visualization/src/cloud_viewer.cpp index 919bdc67..b0b32a1a 100644 --- a/visualization/src/cloud_viewer.cpp +++ b/visualization/src/cloud_viewer.cpp @@ -38,7 +38,7 @@ #include #include #include -#include +#include #include #include diff --git a/visualization/src/common/io.cpp b/visualization/src/common/io.cpp index b489f663..08170e70 100644 --- a/visualization/src/common/io.cpp +++ b/visualization/src/common/io.cpp @@ -45,7 +45,7 @@ #include #include #include -#include +#include ////////////////////////////////////////////////////////////////////////////////////////////// void diff --git a/visualization/src/pcl_painter2D.cpp b/visualization/src/pcl_painter2D.cpp index 9384cd26..44fff8d8 100644 --- a/visualization/src/pcl_painter2D.cpp +++ b/visualization/src/pcl_painter2D.cpp @@ -349,7 +349,7 @@ pcl::visualization::PCLPainter2D::setWindowSize (int w, int h) ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// int* -pcl::visualization::PCLPainter2D::getWindowSize () +pcl::visualization::PCLPainter2D::getWindowSize () const { int *sz = new int[2]; sz[0] = win_width_; diff --git a/visualization/src/pcl_plotter.cpp b/visualization/src/pcl_plotter.cpp index 3acd2332..81b97d75 100644 --- a/visualization/src/pcl_plotter.cpp +++ b/visualization/src/pcl_plotter.cpp @@ -542,7 +542,7 @@ pcl::visualization::PCLPlotter::setWindowSize (int w, int h) ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// int* -pcl::visualization::PCLPlotter::getWindowSize () +pcl::visualization::PCLPlotter::getWindowSize () const { int *sz = new int[2]; sz[0] = win_width_; diff --git a/visualization/src/pcl_visualizer.cpp b/visualization/src/pcl_visualizer.cpp index e1966ae4..bb419949 100644 --- a/visualization/src/pcl_visualizer.cpp +++ b/visualization/src/pcl_visualizer.cpp @@ -1107,7 +1107,7 @@ getDefaultScalarInterpolationForDataSet (vtkDataSet* data) void pcl::visualization::PCLVisualizer::createActorFromVTKDataSet (const vtkSmartPointer &data, vtkSmartPointer &actor, - bool use_scalars) + bool use_scalars) const { // If actor is not initialized, initialize it here if (!actor) @@ -1185,7 +1185,7 @@ pcl::visualization::PCLVisualizer::createActorFromVTKDataSet (const vtkSmartPoin void pcl::visualization::PCLVisualizer::createActorFromVTKDataSet (const vtkSmartPointer &data, vtkSmartPointer &actor, - bool use_scalars) + bool use_scalars) const { // If actor is not initialized, initialize it here if (!actor) @@ -1926,10 +1926,10 @@ pcl::visualization::PCLVisualizer::getCameraFile () const } ///////////////////////////////////////////////////////////////////////////////////////////// +PCL_DEPRECATED(1, 12, "This method can safely not be called anymore as we're just re-rendering all scenes now.") void pcl::visualization::PCLVisualizer::updateCamera () { - PCL_WARN ("[pcl::visualization::PCLVisualizer::updateCamera()] This method was deprecated, just re-rendering all scenes now."); rens_->InitTraversal (); // Update the camera parameters win_->Render (); diff --git a/visualization/src/point_cloud_handlers.cpp b/visualization/src/point_cloud_handlers.cpp index 2c8c485c..c12ecd41 100644 --- a/visualization/src/point_cloud_handlers.cpp +++ b/visualization/src/point_cloud_handlers.cpp @@ -686,12 +686,17 @@ pcl::visualization::PointCloudGeometryHandler::getGeometry vtkSmartPointer data = vtkSmartPointer::New (); data->SetNumberOfComponents (3); + vtkIdType nr_points = cloud_->width * cloud_->height; + + if (!data->Resize(nr_points)) + { + PCL_ERROR("[point_cloud_handlers::getGeometry] Failed to allocate space for points in VTK array."); + throw std::bad_alloc(); + } + // Add all points - float dim; - vtkIdType j = 0; // true point index - float* pts = static_cast (malloc (nr_points * 3 * sizeof (float))); int point_offset = 0; // If the dataset has no invalid values, just copy all of them @@ -699,43 +704,36 @@ pcl::visualization::PointCloudGeometryHandler::getGeometry { for (vtkIdType i = 0; i < nr_points; ++i, point_offset+=cloud_->point_step) { - // Copy the value at the specified field - memcpy (&dim, &cloud_->data[point_offset + cloud_->fields[field_x_idx_].offset], sizeof (float)); - pts[i * 3 + 0] = dim; + const float* ptr = reinterpret_cast(&cloud_->data[point_offset + cloud_->fields[field_x_idx_].offset]); + data->InsertNextValue(*ptr); - memcpy (&dim, &cloud_->data[point_offset + cloud_->fields[field_y_idx_].offset], sizeof (float)); - pts[i * 3 + 1] = dim; + ptr = reinterpret_cast(&cloud_->data[point_offset + cloud_->fields[field_y_idx_].offset]); + data->InsertNextValue(*ptr); - memcpy (&dim, &cloud_->data[point_offset + cloud_->fields[field_z_idx_].offset], sizeof (float)); - pts[i * 3 + 2] = dim; + ptr = reinterpret_cast(&cloud_->data[point_offset + cloud_->fields[field_z_idx_].offset]); + data->InsertNextValue(*ptr); } - data->SetArray (&pts[0], nr_points * 3, 0); points->SetData (data); } else { for (vtkIdType i = 0; i < nr_points; ++i, point_offset+=cloud_->point_step) { - // Copy the value at the specified field - memcpy (&dim, &cloud_->data[point_offset + cloud_->fields[field_x_idx_].offset], sizeof (float)); - if (!std::isfinite (dim)) + const float* ptr = reinterpret_cast(&cloud_->data[point_offset + cloud_->fields[field_x_idx_].offset]); + if (!std::isfinite (*ptr)) continue; - pts[j * 3 + 0] = dim; + data->InsertNextValue(*ptr); - memcpy (&dim, &cloud_->data[point_offset + cloud_->fields[field_y_idx_].offset], sizeof (float)); - if (!std::isfinite (dim)) + ptr = reinterpret_cast(&cloud_->data[point_offset + cloud_->fields[field_y_idx_].offset]); + if (!std::isfinite (*ptr)) continue; - pts[j * 3 + 1] = dim; + data->InsertNextValue(*ptr); - memcpy (&dim, &cloud_->data[point_offset + cloud_->fields[field_z_idx_].offset], sizeof (float)); - if (!std::isfinite (dim)) + ptr = reinterpret_cast(&cloud_->data[point_offset + cloud_->fields[field_z_idx_].offset]); + if (!std::isfinite (*ptr)) continue; - pts[j * 3 + 2] = dim; - - // Set j and increment - j++; + data->InsertNextValue(*ptr); } - data->SetArray (&pts[0], j * 3, 0); points->SetData (data); } } diff --git a/visualization/src/point_picking_event.cpp b/visualization/src/point_picking_event.cpp index 1b5aad90..e9348cc6 100644 --- a/visualization/src/point_picking_event.cpp +++ b/visualization/src/point_picking_event.cpp @@ -167,7 +167,7 @@ pcl::visualization::PointPickingCallback::performSinglePick ( ///////////////////////////////////////////////////////////////////////////////////////////// int pcl::visualization::PointPickingCallback::performAreaPick (vtkRenderWindowInteractor *iren, - std::vector &indices) + std::vector &indices) const { vtkAreaPicker *picker = static_cast (iren->GetPicker ()); vtkRenderer *ren = iren->FindPokedRenderer (iren->GetEventPosition ()[0], iren->GetEventPosition ()[1]);